diff --git a/.github/release.yml b/.github/release.yml
new file mode 100644
index 000000000..0f7212fd7
--- /dev/null
+++ b/.github/release.yml
@@ -0,0 +1,34 @@
+# .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: 🪲 Bug Fixes
+      labels:
+        - bug
+    - title: 📄 Documentation
+      labels:
+        - documentation
+    - title: 💪 Other Changes
+      labels:
+        - "*"
\ No newline at end of file
diff --git a/.github/workflows/bepu-docs-github.yml b/.github/workflows/bepu-docs-github.yml
new file mode 100644
index 000000000..d242b50b2
--- /dev/null
+++ b/.github/workflows/bepu-docs-github.yml
@@ -0,0 +1,45 @@
+# 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: 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:
+        github_token: ${{ secrets.GITHUB_TOKEN }}
+        publish_dir: Documentation/_site
+        publish_branch: gh-pages
+        cname: docs.bepuphysics.com
\ No newline at end of file
diff --git a/.github/workflows/dotnet-core-publish.yml b/.github/workflows/dotnet-core-publish.yml
new file mode 100644
index 000000000..2225e8514
--- /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: 23
+
+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 '<Version>.*?</Version>', "<Version>${{ env.VERSION }}</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 }}
diff --git a/.github/workflows/dotnet-core.yml b/.github/workflows/dotnet-core.yml
index 3bd6cdd6b..4c4bce2ff 100644
--- a/.github/workflows/dotnet-core.yml
+++ b/.github/workflows/dotnet-core.yml
@@ -1,10 +1,14 @@
-name: .NET Core
+name: .NET Core - Build and Test
 
 on:
   push:
-    branches: [ master ]
+    paths-ignore:
+      - '.github/**'
+      - 'Documentation/**'  
   pull_request:
-    branches: [ master ]
+    paths-ignore:
+      - '.github/**'
+      - 'Documentation/**'
 
 jobs:
   build:
@@ -12,7 +16,11 @@ jobs:
     runs-on: windows-latest
 
     steps:
-    - uses: actions/checkout@v2
+    - uses: actions/checkout@v4
+    - uses: actions/setup-dotnet@v4
+      with:
+        dotnet-version: '9.x'
+        include-prerelease: true
     - name: Install dependencies
       run: |
         dotnet restore DemoContentBuilder
@@ -23,10 +31,11 @@ jobs:
         dotnet build Demos --configuration Release --no-restore
     - name: Test
       run: dotnet test DemoTests -c Release --verbosity normal
-    - name: Publish
-      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
diff --git a/.gitignore b/.gitignore
index 1710e230b..266ca1f2e 100644
--- a/.gitignore
+++ b/.gitignore
@@ -205,3 +205,10 @@ ModelManifest.xml
 
 # Don't ignore content files that happen to use the .obj extension.
 !Demos/Content/*.obj
+
+# JetBrains Rider
+.idea/
+
+# DocFX API Generated Pages
+Documentation/api/*
+Documentation/_site/*
\ No newline at end of file
diff --git a/BepuPhysics/BatchCompressor.cs b/BepuPhysics/BatchCompressor.cs
index 638b554a1..e622ec56e 100644
--- a/BepuPhysics/BatchCompressor.cs
+++ b/BepuPhysics/BatchCompressor.cs
@@ -1,9 +1,9 @@
-using BepuUtilities;
+using BepuPhysics.Constraints;
+using BepuUtilities;
 using BepuUtilities.Collections;
 using BepuUtilities.Memory;
 using System;
 using System.Diagnostics;
-using System.Linq;
 using System.Runtime.CompilerServices;
 using System.Threading;
 
@@ -12,7 +12,7 @@ namespace BepuPhysics
     /// <summary>
     /// Handles the movement of constraints from higher indexed batches into lower indexed batches to avoid accumulating a bunch of unnecessary ConstraintBatches.
     /// </summary>
-    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
 
 
         Action<int> 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;
@@ -107,13 +107,12 @@ public BatchCompressor(Solver solver, Bodies bodies, float targetCandidateFracti
         }
 
 
-
         void AnalysisWorker(int workerIndex)
         {
             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]);
             }
         }
 
@@ -124,7 +123,27 @@ 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.)
 
-        unsafe void DoJob(ref AnalysisRegion region, int workerIndex, BufferPool pool)
+
+        [MethodImpl(MethodImplOptions.AggressiveInlining)]
+        private void TryToFindBetterBatchForConstraint(
+            BufferPool pool, ref QuickList<Compression> compressions, ref TypeBatch typeBatch, int* bodyHandles, ref ActiveConstraintDynamicBodyHandleCollector handleAccumulator, TypeProcessor typeProcessor, int constraintIndex)
+        {
+            handleAccumulator.Count = 0;
+            Solver.EnumerateConnectedRawBodyReferences(ref typeBatch, constraintIndex, ref handleAccumulator);
+            var dynamicBodyHandles = new Span<int>(bodyHandles, handleAccumulator.Count);
+            for (int batchIndex = nextBatchIndex - 1; batchIndex >= 0; --batchIndex)
+            {
+                if (Solver.batchReferencedHandles[batchIndex].CanFit(dynamicBodyHandles))
+                {
+                    compressions.Add(new Compression { ConstraintHandle = typeBatch.IndexToHandle[constraintIndex], TargetBatch = batchIndex }, pool);
+                    return;
+                }
+            }
+
+        }
+
+
+        void DoJob(ref AnalysisRegion region, int workerIndex, BufferPool pool)
         {
             ref var compressions = ref this.workerCompressions[workerIndex];
             ref var batch = ref Solver.ActiveSet.Batches[nextBatchIndex];
@@ -135,27 +154,31 @@ 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];
-            ActiveConstraintBodyHandleCollector handleAccumulator;
+            ActiveConstraintDynamicBodyHandleCollector handleAccumulator;
             handleAccumulator.Bodies = Bodies;
             handleAccumulator.Handles = bodyHandles;
-            var bodyHandlesSpan = new Span<int>(bodyHandles, bodiesPerConstraint);
-            for (int i = region.StartIndexInTypeBatch; i < region.EndIndexInTypeBatch; ++i)
+            handleAccumulator.Count = 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 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, bodyHandles, ref handleAccumulator, typeProcessor, i);
+                }
+            }
+            else
+            {
+                for (int i = region.StartIndexInTypeBatch; i < region.EndIndexInTypeBatch; ++i)
+                {
+                    TryToFindBetterBatchForConstraint(pool, ref compressions, ref typeBatch, bodyHandles, ref handleAccumulator, typeProcessor, i);
                 }
             }
         }
 
+
         struct CompressionTarget
         {
             public ushort WorkerIndex;
@@ -172,23 +195,22 @@ 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)
         {
-            //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)
             {
                 //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.Index = 0;
-                Solver.EnumerateConnectedBodies(compression.ConstraintHandle, ref handleAccumulator);
-                if (!Solver.batchReferencedHandles[compression.TargetBatch].CanFit(new Span<int>(bodyHandles, typeProcessor.BodiesPerConstraint)))
+                handleAccumulator.Handles = dynamicBodyHandles;
+                handleAccumulator.Count = 0;
+                Solver.EnumerateConnectedRawBodyReferences(compression.ConstraintHandle, ref handleAccumulator);
+                if (!Solver.batchReferencedHandles[compression.TargetBatch].CanFit(new Span<int>(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.
@@ -222,7 +244,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<Compression>(Math.Max(8, maximumCompressionCount), threadDispatcher == null ? pool : threadDispatcher.GetThreadMemoryPool(i));
+                workerCompressions[i] = new QuickList<Compression>(Math.Max(8, maximumCompressionCount), threadDispatcher == null ? pool : threadDispatcher.WorkerPools[i]);
             }
 
             //In any given compression attempt, we only optimize over one ConstraintBatch.
@@ -292,7 +314,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
@@ -377,11 +399,16 @@ 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)
+            if (threadDispatcher == null)
+                workerCompressions[0].Dispose(pool);
+            else
             {
-                //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)));
+                for (int i = 0; i < workerCount; ++i)
+                {
+                    workerCompressions[i].Dispose(threadDispatcher.WorkerPools[i]);
+                }
             }
+
             pool.Return(ref workerCompressions);
         }
 
diff --git a/BepuPhysics/BepuPhysics.csproj b/BepuPhysics/BepuPhysics.csproj
index 35d575690..6d9803f7b 100644
--- a/BepuPhysics/BepuPhysics.csproj
+++ b/BepuPhysics/BepuPhysics.csproj
@@ -1,33 +1,24 @@
-<Project Sdk="Microsoft.NET.Sdk">
+<Project Sdk="Microsoft.NET.Sdk">
   <PropertyGroup>
-    <TargetFramework>net5.0</TargetFramework>
-    <Version>2.4.0-beta6</Version>
-    <Company>Bepu Entertainment LLC</Company>
-    <Authors>Ross Nordby</Authors>
     <Description>Speedy real time physics simulation library.</Description>
-    <Copyright>© Bepu Entertainment LLC</Copyright>
-    <PackageProjectUrl>https://github.com/bepu/bepuphysics2</PackageProjectUrl>
-    <PackageLicenseExpression>Apache-2.0</PackageLicenseExpression>
-    <RepositoryUrl>https://github.com/bepu/bepuphysics2</RepositoryUrl>
-    <PackageIcon>bepuphysicslogo256.png</PackageIcon>
     <Configurations>Debug;Release;ReleaseNoProfiling</Configurations>
-    <LangVersion>latest</LangVersion>
     <PackageTags>physics;3d;rigid body;real time;simulation</PackageTags>
-    <AllowUnsafeBlocks>True</AllowUnsafeBlocks>
-    
-    <SignAssembly>true</SignAssembly>
-    <DelaySign>false</DelaySign>
     <AssemblyOriginatorKeyFile>key.snk</AssemblyOriginatorKeyFile>
+
+    <!-- CS1573 Parameter has no matching param tag in the XML comment for (but other parameters do) -->
+    <!-- CS1591 Missing XML comment for publicly visible type or member -->
+    <!-- CA2014 Stackalloc in loop -->
+    <NoWarn>1573;1591;CA2014</NoWarn>
   </PropertyGroup>
-    
+
+  <Import Project="..\CommonSettings.props" />
+
   <PropertyGroup Condition="'$(Configuration)'=='Debug'">
     <Optimize>false</Optimize>
     <DefineConstants>TRACE;DEBUG;CHECKMATH;PROFILE</DefineConstants>
   </PropertyGroup>
 
   <PropertyGroup Condition="'$(Configuration)'=='Release'">
-    <DebugSymbols>true</DebugSymbols>
-    <DebugType>embedded</DebugType>
     <Optimize>true</Optimize>
     <DefineConstants>TRACE;RELEASE;PROFILE</DefineConstants>
   </PropertyGroup>
@@ -76,10 +67,6 @@
       <Generator>TextTemplatingFileGenerator</Generator>
       <LastGenOutput>ContactNonconvexTypes.cs</LastGenOutput>
     </None>
-    <None Include="..\Documentation\images\bepuphysicslogo256.png">
-      <Pack>True</Pack>
-      <PackagePath></PackagePath>
-    </None>
   </ItemGroup>
 
 </Project>
\ No newline at end of file
diff --git a/BepuPhysics/Bodies.cs b/BepuPhysics/Bodies.cs
index b2de36660..fe0148ed4 100644
--- a/BepuPhysics/Bodies.cs
+++ b/BepuPhysics/Bodies.cs
@@ -3,22 +3,25 @@
 using System.Diagnostics;
 using System.Numerics;
 using System.Runtime.CompilerServices;
-using BepuPhysics.Constraints;
 using BepuPhysics.Collidables;
 using BepuPhysics.CollisionDetection;
 using BepuUtilities;
+using System.Runtime.Intrinsics;
+using System.Runtime.Intrinsics.X86;
 
 namespace BepuPhysics
 {
+    /// <summary>
+    /// Location of a body in memory.
+    /// </summary>
     public struct BodyMemoryLocation
     {
         /// <summary>
-        /// 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.
         /// </summary>
         public int SetIndex;
         /// <summary>
-        /// 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.
         /// </summary>
         public int Index;
     }
@@ -26,13 +29,16 @@ public struct BodyMemoryLocation
     /// <summary>
     /// Collection of all allocated bodies.
     /// </summary>
-    public class Bodies
+    public partial class Bodies
     {
         /// <summary>
         /// Remaps a body handle integer value to the actual array index of the body.
         /// The backing array index may change in response to cache optimization.
         /// </summary>
         public Buffer<BodyMemoryLocation> HandleToLocation;
+        /// <summary>
+        /// Pool from which handles are pulled for new bodies.
+        /// </summary>
         public IdPool HandlePool;
         /// <summary>
         /// 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.
@@ -45,13 +51,24 @@ public class Bodies
         /// <returns>Reference to the active body set.</returns>
         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.
         /// <summary>
-        /// 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.
+        /// Gets a reference to a body in the collection.
+        /// </summary>
+        /// <param name="handle">Handle of the body to pull a reference for.</param>
+        /// <returns>Reference to the requested body.</returns>
+        /// <remarks>This is an alias for the older <see cref="GetBodyReference"/> and the <see cref="BodyReference"/> constructor. They are all equivalent.</remarks>
+        public BodyReference this[BodyHandle handle]
+        {
+            get
+            {
+                return new BodyReference(handle, this);
+            }
+        }
+
+
+        /// <summary>
+        /// Gets the pool used by the bodies collection to allocate and free memory.
         /// </summary>
-        public Buffer<BodyInertia> Inertias;
         public BufferPool Pool { get; private set; }
 
         internal IslandAwakener awakener;
@@ -75,7 +92,7 @@ public class Bodies
         /// <param name="initialBodyCapacity">Initial number of bodies to allocate space for in the active set.</param>
         /// <param name="initialIslandCapacity">Initial number of islands to allocate space for in the Sets buffer.</param>
         /// <param name="initialConstraintCapacityPerBody">Expected number of constraint references per body to allocate space for.</param>
-        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;
@@ -113,7 +130,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.DynamicsState[location.Index].Motion.Pose, collidable.Shape, out var bodyBounds);
                 if (location.SetIndex == 0)
                 {
                     broadPhase.UpdateActiveBounds(collidable.BroadPhaseIndex, bodyBounds.Min, bodyBounds.Max);
@@ -133,7 +150,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(
@@ -163,7 +180,7 @@ void RemoveCollidableFromBroadPhase(ref Collidable collidable)
         /// </summary>
         /// <param name="description">Description of the body to add.</param>
         /// <returns>Handle of the created body.</returns>
-        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();
@@ -186,6 +203,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;
         }
 
@@ -265,9 +287,47 @@ internal void AddConstraint(int bodyIndex, ConstraintHandle constraintHandle, in
         /// </summary>
         /// <param name="bodyIndex">Index of the active body.</param>
         /// <param name="constraintHandle">Handle of the constraint to remove.</param>
-        internal void RemoveConstraintReference(int bodyIndex, ConstraintHandle constraintHandle)
+        /// <returns>True if the number of constraints remaining attached to the body is 0, false otherwise.</returns>
+        internal bool RemoveConstraintReference(int bodyIndex, ConstraintHandle constraintHandle)
+        {
+            return ActiveSet.RemoveConstraintReference(bodyIndex, constraintHandle, MinimumConstraintCapacityPerBody, Pool);
+        }
+
+        /// <summary>
+        /// Gets whether the inertia matches that of a kinematic body (that is, all inverse mass and inertia components are zero).
+        /// </summary>
+        /// <param name="inertia">Body inertia to analyze.</param>
+        /// <returns>True if all components of inverse mass and inertia are zero, false otherwise.</returns>
+        [MethodImpl(MethodImplOptions.AggressiveInlining)]
+        public unsafe static bool IsKinematic(BodyInertia inertia)
+        {
+            return IsKinematic(&inertia);
+        }
+
+        /// <summary>
+        /// Gets whether the inertia matches that of a kinematic body (that is, all inverse mass and inertia components are zero).
+        /// </summary>
+        /// <param name="inertia">Body inertia to analyze. Must be a reference to fixed data; a pointer will be taken.</param>
+        /// <returns>True if all components of inverse mass and inertia are zero, false otherwise.</returns>
+        /// <remarks>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.</remarks>
+        [MethodImpl(MethodImplOptions.AggressiveInlining)]
+        internal unsafe static bool IsKinematicUnsafeGCHole(ref BodyInertia inertia)
         {
-            ActiveSet.RemoveConstraintReference(bodyIndex, constraintHandle, MinimumConstraintCapacityPerBody, Pool);
+            return IsKinematic((BodyInertia*)Unsafe.AsPointer(ref inertia));
+        }
+
+        /// <summary>
+        /// Checks inertia lanes for kinematicity (all inverse mass and inertia values are zero).
+        /// </summary>
+        /// <param name="inertia">Inertia to examine for kinematicity.</param>
+        /// <returns>Mask of lanes which contain zeroed inverse masses and inertias.</returns>
+        [MethodImpl(MethodImplOptions.AggressiveInlining)]
+        public static Vector<int> 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<float>.Zero);
         }
 
         /// <summary>
@@ -276,9 +336,18 @@ internal void RemoveConstraintReference(int bodyIndex, ConstraintHandle constrai
         /// <param name="inertia">Body inertia to analyze.</param>
         /// <returns>True if all components of inverse mass and inertia are zero, false otherwise.</returns>
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public static bool IsKinematic(in BodyInertia inertia)
+        public unsafe static bool IsKinematic(BodyInertia* inertia)
         {
-            return inertia.InverseMass == 0 && HasLockedInertia(inertia.InverseInertiaTensor);
+            if (Avx.IsSupported)
+            {
+                var inertiaVector = Avx.LoadVector256((float*)inertia);
+                var masked = Avx.CompareEqual(inertiaVector, Vector256<float>.Zero);
+                return (Avx.MoveMask(masked) & 0x7F) == 0x7F;
+            }
+            else
+            {
+                return inertia->InverseMass == 0 && HasLockedInertia(&inertia->InverseInertiaTensor);
+            }
         }
 
         /// <summary>
@@ -287,62 +356,64 @@ public static bool IsKinematic(in BodyInertia inertia)
         /// <param name="inertia">Body inertia to analyze.</param>
         /// <returns>True if all components of inverse mass and inertia are zero, false otherwise.</returns>
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public static bool HasLockedInertia(in Symmetric3x3 inertia)
+        public unsafe static bool HasLockedInertia(Symmetric3x3 inertia)
         {
-            return inertia.XX == 0 &&
-                   inertia.YX == 0 &&
-                   inertia.YY == 0 &&
-                   inertia.ZX == 0 &&
-                   inertia.ZY == 0 &&
-                   inertia.ZZ == 0;
+            return HasLockedInertia(&inertia);
         }
 
-        private struct ConnectedDynamicCounter : IForEach<int>
+        /// <summary>
+        /// Gets whether the angular inertia matches that of a kinematic body (that is, all inverse inertia tensor components are zero).
+        /// </summary>
+        /// <param name="inertia">Body inertia to analyze.</param>
+        /// <returns>True if all components of inverse mass and inertia are zero, false otherwise.</returns>
+        [MethodImpl(MethodImplOptions.AggressiveInlining)]
+        public unsafe static bool HasLockedInertia(Symmetric3x3* inertia)
         {
-            public Bodies Bodies;
-            public int DynamicCount;
-            [MethodImpl(MethodImplOptions.AggressiveInlining)]
-            public void LoopBody(int bodyIndex)
+            if (Avx.IsSupported)
+            {
+                var inertiaVector = Avx.LoadVector256((float*)inertia);
+                var masked = Avx.CompareEqual(inertiaVector, Vector256<float>.Zero);
+                return (Avx.MoveMask(masked) & 0x3F) == 0x3F;
+            }
+            else
             {
-                //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]))
-                    ++DynamicCount;
+                return inertia->XX == 0 &&
+                       inertia->YX == 0 &&
+                       inertia->YY == 0 &&
+                       inertia->ZX == 0 &&
+                       inertia->ZY == 0 &&
+                       inertia->ZZ == 0;
             }
         }
 
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        void UpdateForKinematicStateChange(BodyHandle handle, ref BodyMemoryLocation location, ref BodySet set, bool newlyKinematic)
+        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.LocalInertias[location.Index]) ? 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);
+                    solver.UpdateReferencesForBodyBecomingKinematic(handle, location.Index);
                 }
-            }
-            if (newlyKinematic)
-            {
-                ref var constraints = ref set.Constraints[location.Index];
-                ConnectedDynamicCounter enumerator;
-                enumerator.Bodies = this;
-                for (int i = 0; i < constraints.Count; ++i)
+                else
                 {
-                    ref var constraint = ref constraints[i];
-                    enumerator.DynamicCount = 0;
-                    solver.EnumerateConnectedBodies(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);
-                    }
+                    solver.UpdateReferencesForBodyBecomingDynamic(handle, location.Index);
                 }
             }
         }
@@ -368,9 +439,16 @@ 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;
-            UpdateForKinematicStateChange(handle, ref location, ref set, newlyKinematic);
+            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;
+            //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);
         }
 
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
@@ -382,7 +460,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.Poses[activeBodyIndex], set.LocalInertias[activeBodyIndex], ref set.Collidables[activeBodyIndex]);
+                    ref var state = ref set.DynamicsState[activeBodyIndex];
+                    AddCollidableToBroadPhase(handle, state.Motion.Pose, state.Inertia.Local, ref set.Collidables[activeBodyIndex]);
                 }
                 else
                 {
@@ -435,10 +514,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.LocalInertias[location.Index]);
+            var nowKinematic = IsKinematic(description.LocalInertia);
+            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, newlyKinematic);
+            UpdateForKinematicStateChange(handle, ref location, ref set, previouslyKinematic, nowKinematic);
             UpdateBounds(handle);
         }
 
@@ -455,11 +535,26 @@ public void GetDescription(BodyHandle handle, out BodyDescription description)
             set.GetDescription(location.Index, out description);
         }
 
+        /// <summary>
+        /// Gets the description of a body by handle.
+        /// </summary>
+        /// <param name="handle">Handle of the body to look up.</param>
+        /// <returns>Description of the body.</returns>
+        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;
+        }
+
         /// <summary>
         /// Gets a reference to a body by its handle.
         /// </summary>
         /// <param name="handle">Handle of the body to grab a reference of.</param>
         /// <returns>Reference to the desired body.</returns>
+        /// <remarks>This is an alias for <see cref="this[BodyHandle]"/> and the <see cref="BodyReference"/> constructor. They are all equivalent.</remarks>
         public BodyReference GetBodyReference(BodyHandle handle)
         {
             ValidateExistingHandle(handle);
@@ -478,6 +573,24 @@ public bool BodyExists(BodyHandle bodyHandle)
             return bodyHandle.Value >= 0 && bodyHandle.Value < HandleToLocation.Length && HandleToLocation[bodyHandle.Value].SetIndex >= 0;
         }
 
+        /// <summary>
+        /// Computes the number of bodies contained in the simulation.
+        /// </summary>
+        /// <returns>Number of bodies contained in the simulation.</returns>
+        /// <remarks>Enumerates all <see cref="BodySet"/> instances in the <see cref="Bodies"/> 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.</remarks>
+        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)
         {
@@ -504,18 +617,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.DynamicsState[j];
                         try
                         {
-                            pose.Position.Validate();
-                            pose.Orientation.ValidateOrientation();
-                            velocity.Linear.Validate();
-                            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: {pose.Position}, orientation: {pose.Orientation}, linear: {velocity.Linear}, angular: {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;
                         }
 
@@ -524,563 +636,34 @@ internal void ValidateMotionStates()
             }
         }
 
-        [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        private static void GatherInertiaForBody(ref BodyInertia source, 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;
-        }
-
-        /// <summary>
-        /// Gathers inertia for one body bundle into an AOSOA bundle.
-        /// </summary>
-        /// <param name="references">Active body indices being gathered.</param>
-        /// <param name="count">Number of bodies in the bundle.</param>
-        /// <param name="inertiaA">Gathered inertia of body A.</param>
-        /// <param name="inertiaB">Gathered inertia of body B.</param>
-        [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public void GatherInertia(ref Vector<int> references, int count,
-            out BodyInertias inertiaA)
-        {
-            Unsafe.SkipInit(out inertiaA);
-            Debug.Assert(count >= 0 && count <= Vector<float>.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<Vector<int>, int>(ref references);
-            for (int i = 0; i < count; ++i)
-            {
-                GatherInertiaForBody(ref Inertias[Unsafe.Add(ref baseIndexA, i)], ref GatherScatter.GetOffsetInstance(ref inertiaA, i));
-            }
-        }
-
-        /// <summary>
-        /// Gathers inertia for two body bundles into AOSOA bundles.
-        /// </summary>
-        /// <param name="references">Active body indices being gathered.</param>
-        /// <param name="count">Number of bodies in the bundle.</param>
-        /// <param name="inertiaA">Gathered inertia of body A.</param>
-        /// <param name="inertiaB">Gathered inertia of body B.</param>
-        [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<float>.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<Vector<int>, int>(ref references.IndexA);
-            ref var baseIndexB = ref Unsafe.As<Vector<int>, 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));
-            }
-        }
-
-        /// <summary>
-        /// Gathers inertia for three body bundles into AOSOA bundles.
-        /// </summary>
-        /// <param name="references">Active body indices being gathered.</param>
-        /// <param name="count">Number of bodies in the bundle.</param>
-        /// <param name="inertiaA">Gathered inertia of body A.</param>
-        /// <param name="inertiaB">Gathered inertia of body B.</param>
-        /// <param name="inertiaC">Gathered inertia of body C.</param>
-        [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<float>.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<Vector<int>, int>(ref references.IndexA);
-            ref var baseIndexB = ref Unsafe.As<Vector<int>, int>(ref references.IndexB);
-            ref var baseIndexC = ref Unsafe.As<Vector<int>, 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));
-            }
-        }
-
-        /// <summary>
-        /// Gathers inertia for four body bundles into AOSOA bundles.
-        /// </summary>
-        /// <param name="references">Active body indices being gathered.</param>
-        /// <param name="count">Number of bodies in the bundle.</param>
-        /// <param name="inertiaA">Gathered inertia of body A.</param>
-        /// <param name="inertiaB">Gathered inertia of body B.</param>
-        /// <param name="inertiaC">Gathered inertia of body C.</param>
-        /// <param name="inertiaD">Gathered inertia of body D.</param>
-        [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<float>.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<Vector<int>, int>(ref references.IndexA);
-            ref var baseIndexB = ref Unsafe.As<Vector<int>, int>(ref references.IndexB);
-            ref var baseIndexC = ref Unsafe.As<Vector<int>, int>(ref references.IndexC);
-            ref var baseIndexD = ref Unsafe.As<Vector<int>, 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));
-            }
-        }
 
-        /// <summary>
-        /// Gathers orientations for two body bundles into AOSOA bundles.
-        /// </summary>
-        /// <param name="references">Active body indices being gathered.</param>
-        /// <param name="count">Number of body pairs in the bundle.</param>
-        /// <param name="orientationA">Gathered orientation of body A.</param>
-        /// <param name="orientationB">Gathered orientation of body B.</param>
-        //[MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public void GatherOrientation(ref TwoBodyReferences references, int count,
-            out QuaternionWide orientationA, out QuaternionWide orientationB)
+        internal void ValidateAwakeMotionStatesByHash(HashDiagnosticType type)
         {
-            Unsafe.SkipInit(out orientationA);
-            Unsafe.SkipInit(out orientationB);
-            Debug.Assert(count >= 0 && count <= Vector<float>.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<Vector<int>, int>(ref references.IndexA);
-            ref var baseIndexB = ref Unsafe.As<Vector<int>, 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));
-            }
-        }
-
-        /// <summary>
-        /// Gathers orientations for one body bundles into AOSOA bundles.
-        /// </summary>
-        /// <param name="references">Active body indices being gathered.</param>
-        /// <param name="count">Number of body pairs in the bundle.</param>
-        /// <param name="orientation">Gathered orientation of bodies in the bundle.</param>
-        //[MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public void GatherOrientation(ref Vector<int> references, int count,
-            out QuaternionWide orientation)
-        {
-            Unsafe.SkipInit(out orientation);
-            Debug.Assert(count >= 0 && count <= Vector<float>.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<Vector<int>, 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));
-            }
-        }
-
-
-
-        /// <summary>
-        /// Gathers pose information for a body bundle into an AOSOA bundle.
-        /// </summary>
-        /// <param name="references">Active body indices being gathered.</param>
-        /// <param name="count">Number of body pairs in the bundle.</param>
-        /// <param name="position">Gathered absolute position of the body.</param>
-        /// <param name="orientation">Gathered orientation of the body.</param>
-        //[MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public void GatherPose(ref Vector<int> references, int count, out Vector3Wide position, out QuaternionWide orientation)
-        {
-            Unsafe.SkipInit(out position);
-            Unsafe.SkipInit(out orientation);
-            //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<float>.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<Vector<int>, int>(ref references);
-
-            ref var poses = ref ActiveSet.Poses;
-            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));
-
-            }
-        }
-
-        /// <summary>
-        /// Gathers orientations and relative positions for a two body bundle into an AOSOA bundle.
-        /// </summary>
-        /// <param name="references">Active body indices being gathered.</param>
-        /// <param name="count">Number of body pairs in the bundle.</param>
-        /// <param name="offsetB">Gathered offsets from body A to body B.</param>
-        /// <param name="orientationA">Gathered orientation of body A.</param>
-        /// <param name="orientationB">Gathered orientation of body B.</param>
-        //[MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public void GatherPose(ref TwoBodyReferences references, int count,
-            out Vector3Wide offsetB, out QuaternionWide orientationA, out QuaternionWide orientationB)
-        {
-            Unsafe.SkipInit(out Vector3Wide positionA);
-            Unsafe.SkipInit(out Vector3Wide positionB);
-            Unsafe.SkipInit(out orientationA);
-            Unsafe.SkipInit(out orientationB);
-            Debug.Assert(count >= 0 && count <= Vector<float>.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<Vector<int>, int>(ref references.IndexA);
-            ref var baseIndexB = ref Unsafe.As<Vector<int>, 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);
-                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));
-            }
-            //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 offsetB);
-        }
-
-        /// <summary>
-        /// Gathers relative positions for a two body bundle into an AOSOA bundle.
-        /// </summary>
-        /// <param name="references">Active body indices being gathered.</param>
-        /// <param name="count">Number of body pairs in the bundle.</param>
-        /// <param name="ab">Gathered offset from body A to of body B.</param>
-        //[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<float>.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<Vector<int>, int>(ref references.IndexA);
-            ref var baseIndexB = ref Unsafe.As<Vector<int>, 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);
-        }
-
-        /// <summary>
-        /// Gathers relative positions for a three body bundle into an AOSOA bundle.
-        /// </summary>
-        /// <param name="references">Active body indices being gathered.</param>
-        /// <param name="count">Number of body pairs in the bundle.</param>
-        /// <param name="ab">Gathered offset from body A to of body B.</param>
-        /// <param name="ac">Gathered offset from body A to of body C.</param>
-        //[MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public void GatherOffsets(ref ThreeBodyReferences references, int count, out Vector3Wide ab, out Vector3Wide ac)
-        {
-            Unsafe.SkipInit(out Vector3Wide positionA);
-            Unsafe.SkipInit(out Vector3Wide positionB);
-            Unsafe.SkipInit(out Vector3Wide positionC);
-            Debug.Assert(count >= 0 && count <= Vector<float>.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<Vector<int>, int>(ref references.IndexA);
-            ref var baseIndexB = ref Unsafe.As<Vector<int>, int>(ref references.IndexB);
-            ref var baseIndexC = ref Unsafe.As<Vector<int>, int>(ref references.IndexC);
-
-            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));
-                Vector3Wide.WriteFirst(poses[Unsafe.Add(ref baseIndexC, i)].Position, ref GatherScatter.GetOffsetInstance(ref positionC, i));
-            }
-            //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.
-            Vector3Wide.Subtract(positionB, positionA, out ab);
-            Vector3Wide.Subtract(positionC, positionA, out ac);
-        }
-        /// <summary>
-        /// Gathers relative positions for a four body bundle into an AOSOA bundle.
-        /// </summary>
-        /// <param name="references">Active body indices being gathered.</param>
-        /// <param name="count">Number of body pairs in the bundle.</param>
-        /// <param name="ab">Gathered offset from body A to of body B.</param>
-        /// <param name="ac">Gathered offset from body A to of body C.</param>
-        /// <param name="ad">Gathered offset from body A to of body D.</param>
-        //[MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public void GatherOffsets(ref FourBodyReferences references, int count, out Vector3Wide ab, out Vector3Wide ac, out Vector3Wide ad)
-        {
-            Unsafe.SkipInit(out Vector3Wide positionA);
-            Unsafe.SkipInit(out Vector3Wide positionB);
-            Unsafe.SkipInit(out Vector3Wide positionC);
-            Unsafe.SkipInit(out Vector3Wide positionD);
-            Debug.Assert(count >= 0 && count <= Vector<float>.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<Vector<int>, int>(ref references.IndexA);
-            ref var baseIndexB = ref Unsafe.As<Vector<int>, int>(ref references.IndexB);
-            ref var baseIndexC = ref Unsafe.As<Vector<int>, int>(ref references.IndexC);
-            ref var baseIndexD = ref Unsafe.As<Vector<int>, int>(ref references.IndexD);
-
-            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));
-                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));
-            }
-            //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.
-            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<BodyVelocity> 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;
-        }
-
-        /// <summary>
-        /// Gathers velocities for one body bundle and stores it into a velocity bundle.
-        /// </summary>
-        /// <param name="references">Active set indices of the bodies to gather velocity data for.</param>
-        /// <param name="count">Number of bodies in the bundle.</param>
-        /// <param name="velocities">Gathered velocities.</param>
-        [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public static unsafe void GatherVelocities(ref Buffer<BodyVelocity> sourceVelocities, ref Vector<int> 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<Vector<int>, int>(ref references);
-            for (int i = 0; i < count; ++i)
-            {
-                GatherVelocities(ref sourceVelocities, ref velocities, ref baseIndex, i);
-            }
-        }
-
-        /// <summary>
-        /// Gathers velocities for two body bundles and stores it into velocity bundles.
-        /// </summary>
-        /// <param name="references">Active set indices of the bodies to gather velocity data for.</param>
-        /// <param name="count">Number of body pairs in the bundle.</param>
-        /// <param name="velocitiesA">Gathered velocities of A bodies.</param>
-        /// <param name="velocitiesB">Gathered velocities of B bodies.</param>
-        [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public static unsafe void GatherVelocities(ref Buffer<BodyVelocity> 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<float>.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<Vector<int>, int>(ref references.IndexA);
-            ref var baseIndexB = ref Unsafe.As<Vector<int>, 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);
-            }
-        }
-
-        /// <summary>
-        /// Gathers velocities for three body bundles and stores it into velocity bundles.
-        /// </summary>
-        /// <param name="references">Active set indices of the bodies to gather velocity data for.</param>
-        /// <param name="count">Number of body pairs in the bundle.</param>
-        /// <param name="velocitiesA">Gathered velocities of A bodies.</param>
-        /// <param name="velocitiesB">Gathered velocities of B bodies.</param>
-        /// <param name="velocitiesC">Gathered velocities of C bodies.</param>
-        [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public static unsafe void GatherVelocities(ref Buffer<BodyVelocity> 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<float>.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<Vector<int>, int>(ref references.IndexA);
-            ref var baseIndexB = ref Unsafe.As<Vector<int>, int>(ref references.IndexB);
-            ref var baseIndexC = ref Unsafe.As<Vector<int>, 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);
-            }
-        }
-
-        /// <summary>
-        /// Gathers velocities for four body bundles and stores it into velocity bundles.
-        /// </summary>
-        /// <param name="references">Active set indices of the bodies to gather velocity data for.</param>
-        /// <param name="count">Number of body pairs in the bundle.</param>
-        /// <param name="velocitiesA">Gathered velocities of A bodies.</param>
-        /// <param name="velocitiesB">Gathered velocities of B bodies.</param>
-        /// <param name="velocitiesC">Gathered velocities of C bodies.</param>
-        /// <param name="velocitiesD">Gathered velocities of D bodies.</param>
-        [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public static unsafe void GatherVelocities(ref Buffer<BodyVelocity> 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<float>.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<Vector<int>, int>(ref references.IndexA);
-            ref var baseIndexB = ref Unsafe.As<Vector<int>, int>(ref references.IndexB);
-            ref var baseIndexC = ref Unsafe.As<Vector<int>, int>(ref references.IndexC);
-            ref var baseIndexD = ref Unsafe.As<Vector<int>, 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<BodyVelocity> targets, 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);
-        }
-
-
-        /// <summary>
-        /// Scatters velocities for one body bundle into the active body set.
-        /// </summary>
-        /// <param name="sourceVelocities">Velocities of body bundle A to scatter.</param>
-        /// <param name="references">Active set indices of the bodies to scatter velocity data to.</param>
-        /// <param name="count">Number of body pairs in the bundle.</param>
-        [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public static unsafe void ScatterVelocities(ref BodyVelocities sourceVelocities, ref Buffer<BodyVelocity> targetVelocities, ref Vector<int> references, int count)
-        {
-            Debug.Assert(count >= 0 && count <= Vector<float>.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<Vector<int>, int>(ref references);
-            for (int i = 0; i < count; ++i)
-            {
-                ScatterVelocities(ref sourceVelocities, ref targetVelocities, ref baseIndex, i);
-            }
-        }
-
-        /// <summary>
-        /// Scatters velocities for two body bundles into the active body set.
-        /// </summary>
-        /// <param name="sourceVelocitiesA">Velocities of body bundle A to scatter.</param>
-        /// <param name="sourceVelocitiesA">Velocities of body bundle B to scatter.</param>
-        /// <param name="references">Active set indices of the bodies to scatter velocity data to.</param>
-        /// <param name="count">Number of body pairs in the bundle.</param>
-        [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public static unsafe void ScatterVelocities(ref BodyVelocities sourceVelocitiesA, ref BodyVelocities sourceVelocitiesB, ref Buffer<BodyVelocity> targetVelocities,
-            ref TwoBodyReferences references, int count)
-        {
-            Debug.Assert(count >= 0 && count <= Vector<float>.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<Vector<int>, int>(ref references.IndexA);
-            ref var baseIndexB = ref Unsafe.As<Vector<int>, 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);
-            }
-        }
-
-        /// <summary>
-        /// Scatters velocities for three body bundles into the active body set.
-        /// </summary>
-        /// <param name="sourceVelocitiesA">Velocities of body bundle A to scatter.</param>
-        /// <param name="sourceVelocitiesB">Velocities of body bundle B to scatter.</param>
-        /// <param name="sourceVelocitiesC">Velocities of body bundle A to scatter.</param>
-        /// <param name="references">Active set indices of the bodies to scatter velocity data to.</param>
-        /// <param name="count">Number of body pairs in the bundle.</param>
-        [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public static unsafe void ScatterVelocities(
-            ref BodyVelocities sourceVelocitiesA, ref BodyVelocities sourceVelocitiesB, ref BodyVelocities sourceVelocitiesC,
-            ref Buffer<BodyVelocity> targetVelocities, ref ThreeBodyReferences references, int count)
-        {
-            Debug.Assert(count >= 0 && count <= Vector<float>.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<Vector<int>, int>(ref references.IndexA);
-            ref var baseIndexB = ref Unsafe.As<Vector<int>, int>(ref references.IndexB);
-            ref var baseIndexC = ref Unsafe.As<Vector<int>, int>(ref references.IndexC);
-            for (int i = 0; i < count; ++i)
+            var instance = InvasiveHashDiagnostics.Instance;
+            ref int hash = ref instance.GetHashForType(type);
+            ref var set = ref ActiveSet;
+            for (int j = 0; j < set.Count; ++j)
             {
-                ScatterVelocities(ref sourceVelocitiesA, ref targetVelocities, ref baseIndexA, i);
-                ScatterVelocities(ref sourceVelocitiesB, ref targetVelocities, ref baseIndexB, i);
-                ScatterVelocities(ref sourceVelocitiesC, ref targetVelocities, ref baseIndexC, i);
+                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);
+                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);
             }
         }
 
-        /// <summary>
-        /// Scatters velocities for four body bundles into the active body set.
-        /// </summary>
-        /// <param name="sourceVelocitiesA">Velocities of body bundle A to scatter.</param>
-        /// <param name="sourceVelocitiesB">Velocities of body bundle B to scatter.</param>
-        /// <param name="sourceVelocitiesC">Velocities of body bundle A to scatter.</param>
-        /// <param name="sourceVelocitiesD">Velocities of body bundle B to scatter.</param>
-        /// <param name="references">Active set indices of the bodies to scatter velocity data to.</param>
-        /// <param name="count">Number of body pairs in the bundle.</param>
-        [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public static unsafe void ScatterVelocities(
-            ref BodyVelocities sourceVelocitiesA, ref BodyVelocities sourceVelocitiesB, ref BodyVelocities sourceVelocitiesC, ref BodyVelocities sourceVelocitiesD,
-            ref Buffer<BodyVelocity> targetVelocities, ref FourBodyReferences references, int count)
+        internal void ValidateAwakeCollidablesByHash(HashDiagnosticType type)
         {
-            Debug.Assert(count >= 0 && count <= Vector<float>.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<Vector<int>, int>(ref references.IndexA);
-            ref var baseIndexB = ref Unsafe.As<Vector<int>, int>(ref references.IndexB);
-            ref var baseIndexC = ref Unsafe.As<Vector<int>, int>(ref references.IndexC);
-            ref var baseIndexD = ref Unsafe.As<Vector<int>, int>(ref references.IndexD);
-            for (int i = 0; i < count; ++i)
+            var instance = InvasiveHashDiagnostics.Instance;
+            ref int hash = ref instance.GetHashForType(type);
+            ref var set = ref ActiveSet;
+            for (int j = 0; j < set.Count; ++j)
             {
-                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);
+                instance.ContributeToHash(ref hash, set.Collidables[j]);
             }
         }
 
@@ -1120,13 +703,14 @@ struct ActiveConstraintBodyHandleEnumerator<TInnerEnumerator> : IForEach<int> wh
             public int SourceBodyIndex;
 
             [MethodImpl(MethodImplOptions.AggressiveInlining)]
-            public void LoopBody(int connectedBodyIndex)
+            public void LoopBody(int encodedBodyIndex)
             {
-                if (SourceBodyIndex != connectedBodyIndex)
+                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 
                     //must convert the solver-provided indices to handles.
-                    InnerEnumerator.LoopBody(bodies.ActiveSet.IndexToHandle[connectedBodyIndex]);
+                    InnerEnumerator.LoopBody(bodies.ActiveSet.IndexToHandle[bodyIndex]);
                 }
             }
 
@@ -1158,7 +742,6 @@ public void LoopBody(int connectedBodyHandle)
         /// <typeparam name="TEnumerator">Type of the enumerator to execute on each connected body.</typeparam>
         /// <param name="activeBodyIndex">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.</param>
         /// <param name="enumerator">Enumerator instance to run on each connected body.</param>
-        /// <param name="solver">Solver from which to pull constraint body references.</param>
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
         internal void EnumerateConnectedBodyIndices<TEnumerator>(int activeBodyIndex, ref TEnumerator enumerator) where TEnumerator : IForEach<int>
         {
@@ -1171,7 +754,7 @@ internal void EnumerateConnectedBodyIndices<TEnumerator>(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.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. 
@@ -1185,7 +768,6 @@ internal void EnumerateConnectedBodyIndices<TEnumerator>(int activeBodyIndex, re
         /// <typeparam name="TEnumerator">Type of the enumerator to execute on each connected body.</typeparam>
         /// <param name="bodyHandle">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.</param>
         /// <param name="enumerator">Enumerator instance to run on each connected body.</param>
-        /// <param name="solver">Solver from which to pull constraint body references.</param>
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
         public void EnumerateConnectedBodies<TEnumerator>(BodyHandle bodyHandle, ref TEnumerator enumerator) where TEnumerator : IForEach<BodyHandle>
         {
@@ -1204,7 +786,7 @@ public void EnumerateConnectedBodies<TEnumerator>(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;
             }
@@ -1217,7 +799,7 @@ public void EnumerateConnectedBodies<TEnumerator>(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;
             }
@@ -1258,31 +840,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.
-        /// <summary>
-        /// Reallocates the inertias buffer for the target capacity. Will not shrink below the size of the current active set.
-        /// </summary>
-        internal void ResizeInertias(int capacity)
-        {
-            var targetCapacity = BufferPool.GetCapacityForCount<BodyInertia>(Math.Max(capacity, ActiveSet.Count));
-            if (Inertias.Length != targetCapacity)
-            {
-                Pool.ResizeToAtLeast(ref Inertias, targetCapacity, Math.Min(Inertias.Length, ActiveSet.Count));
-            }
-        }
-        /// <summary>
-        /// Guarantees that the inertias capacity is sufficient for the given capacity.
-        /// </summary>
-        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));
-            }
-        }
 
         /// <summary>
         /// Resizes the allocated spans for active body data. Note that this is conservative; it will never orphan existing objects.
@@ -1295,7 +852,6 @@ public void Resize(int capacity)
             {
                 ActiveSet.InternalResize(targetBodyCapacity, Pool);
             }
-            ResizeInertias(capacity);
             var targetHandleCapacity = BufferPool.GetCapacityForCount<int>(Math.Max(capacity, HandlePool.HighestPossiblyClaimedId + 1));
             if (HandleToLocation.Length != targetHandleCapacity)
             {
@@ -1328,7 +884,6 @@ public void EnsureCapacity(int capacity)
             {
                 ActiveSet.InternalResize(capacity, Pool);
             }
-            EnsureInertiasCapacity(capacity);
             if (HandleToLocation.Length < capacity)
             {
                 ResizeHandles(capacity);
@@ -1363,8 +918,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
new file mode 100644
index 000000000..ffebddca1
--- /dev/null
+++ b/BepuPhysics/Bodies_GatherScatter.cs
@@ -0,0 +1,755 @@
+using BepuUtilities.Memory;
+using System.Diagnostics;
+using System.Numerics;
+using System.Runtime.CompilerServices;
+using BepuPhysics.Constraints;
+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(int index, int bodyIndexInBundle, ref Buffer<BodyDynamics> states, ref BodyInertiaWide gatheredInertias)
+        {
+            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;
+            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 WriteGatherMotionState(int index, int bodyIndexInBundle, ref Buffer<BodyDynamics> states,
+            ref Vector3Wide position, ref QuaternionWide orientation, ref BodyVelocityWide velocity)
+        {
+            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));
+            Vector3Wide.WriteFirst(state.Velocity.Angular, ref GetOffsetInstance(ref velocity.Angular, bodyIndexInBundle));
+        }
+
+
+        [MethodImpl(MethodImplOptions.AggressiveInlining)]
+        unsafe static void FallbackGatherMotionState(BodyDynamics* states, Vector<int> 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);
+            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);
+
+            for (int i = 0; i < Vector<int>.Count; ++i)
+            {
+                var encodedBodyIndex = encodedBodyIndices[i];
+                if (encodedBodyIndex < 0)
+                    continue;
+                var stateValues = (float*)(states + (encodedBodyIndex & BodyReferenceMask));
+                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];
+            }
+        }
+        [MethodImpl(MethodImplOptions.AggressiveInlining)]
+        unsafe static void FallbackGatherInertia(BodyDynamics* states, Vector<int> encodedBodyIndices, ref BodyInertiaWide inertia, int offsetInFloats)
+        {
+            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 < Vector<int>.Count; ++i)
+            {
+                var encodedBodyIndex = encodedBodyIndices[i];
+                if (encodedBodyIndex < 0)
+                    continue;
+                var inertiaValues = (float*)(states + (encodedBodyIndex & BodyReferenceMask)) + 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];
+            }
+        }
+
+        public const int DoesntExistFlagIndex = 31;
+        public const int KinematicFlagIndex = 30;
+        public const int KinematicMask = 1 << KinematicFlagIndex;
+        /// <summary>
+        /// Constraint body references greater than a given unsigned value are either kinematic (bit 30 set) or correspond to an empty lane (bit 31 set).
+        /// </summary>
+        public const uint DynamicLimit = KinematicMask;
+        public const uint BodyReferenceMetadataMask = (1u << DoesntExistFlagIndex) | KinematicMask;
+        /// <summary>
+        /// 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.
+        /// </summary>
+        public const int BodyReferenceMask = (int)~BodyReferenceMetadataMask;
+
+        /// <summary>
+        /// Checks whether a constraint encoded body reference value refers to a dynamic body.
+        /// </summary>
+        /// <param name="encodedBodyReferenceValue">Raw encoded value taken from a constraint.</param>
+        /// <returns>True if the encoded body reference refers to a dynamic body, false otherwise.</returns>
+        [MethodImpl(MethodImplOptions.AggressiveInlining)]
+        public static bool IsEncodedDynamicReference(int encodedBodyReferenceValue)
+        {
+            return (uint)encodedBodyReferenceValue < DynamicLimit;
+        }
+        /// <summary>
+        /// Checks whether a constraint encoded body reference value refers to a kinematic body.
+        /// </summary>
+        /// <param name="encodedBodyReferenceValue">Raw encoded value taken from a constraint.</param>
+        /// <returns>True if the encoded body reference refers to a kinematic body, false otherwise.</returns>
+        [MethodImpl(MethodImplOptions.AggressiveInlining)]
+        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.
+
+        /// <summary>
+        /// 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 <see cref="Vector{T}.Count"/>.
+        /// </summary>
+        /// <param name="states">Array-of-structures data to transpose.</param>
+        /// <param name="position">Array-of-structures-of-arrays positions.</param>
+        /// <param name="orientation">Array-of-structures-of-arrays orientations.</param>
+        /// <param name="velocity">Array-of-structures-of-arrays velocities.</param>
+        public static unsafe void TransposeMotionStates(Buffer<MotionState> states, out Vector3Wide position, out QuaternionWide orientation, out BodyVelocityWide velocity)
+        {
+            Debug.Assert(states.Length > 0 && states.Length <= Vector<float>.Count);
+            if (Avx.IsSupported && Vector<float>.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<float>.Zero : Avx.LoadAlignedVector256(s1);
+                    var m2 = empty2 ? Vector256<float>.Zero : Avx.LoadAlignedVector256(s2);
+                    var m3 = empty3 ? Vector256<float>.Zero : Avx.LoadAlignedVector256(s3);
+                    var m4 = empty4 ? Vector256<float>.Zero : Avx.LoadAlignedVector256(s4);
+                    var m5 = empty5 ? Vector256<float>.Zero : Avx.LoadAlignedVector256(s5);
+                    var m6 = empty6 ? Vector256<float>.Zero : Avx.LoadAlignedVector256(s6);
+                    var m7 = empty7 ? Vector256<float>.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<float>.Zero : Avx.LoadAlignedVector256(s1 + 8);
+                    var m2 = empty2 ? Vector256<float>.Zero : Avx.LoadAlignedVector256(s2 + 8);
+                    var m3 = empty3 ? Vector256<float>.Zero : Avx.LoadAlignedVector256(s3 + 8);
+                    var m4 = empty4 ? Vector256<float>.Zero : Avx.LoadAlignedVector256(s4 + 8);
+                    var m5 = empty5 ? Vector256<float>.Zero : Avx.LoadAlignedVector256(s5 + 8);
+                    var m6 = empty6 ? Vector256<float>.Zero : Avx.LoadAlignedVector256(s6 + 8);
+                    var m7 = empty7 ? Vector256<float>.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<TAccessFilter>(Vector<int> encodedBodyIndices, bool worldInertia, out Vector3Wide position, out QuaternionWide orientation, out BodyVelocityWide velocity, out BodyInertiaWide inertia)
+            where TAccessFilter : unmanaged, IBodyAccessFilter
+        {
+            var solverStates = ActiveSet.DynamicsState.Memory;
+            Unsafe.SkipInit(out TAccessFilter filter);
+            if (Avx.IsSupported && Vector<float>.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<float>.Zero : Avx.LoadAlignedVector256(s0);
+                    var m1 = empty1 ? Vector256<float>.Zero : Avx.LoadAlignedVector256(s1);
+                    var m2 = empty2 ? Vector256<float>.Zero : Avx.LoadAlignedVector256(s2);
+                    var m3 = empty3 ? Vector256<float>.Zero : Avx.LoadAlignedVector256(s3);
+                    var m4 = empty4 ? Vector256<float>.Zero : Avx.LoadAlignedVector256(s4);
+                    var m5 = empty5 ? Vector256<float>.Zero : Avx.LoadAlignedVector256(s5);
+                    var m6 = empty6 ? Vector256<float>.Zero : Avx.LoadAlignedVector256(s6);
+                    var m7 = empty7 ? Vector256<float>.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));
+
+                    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);
+                    }
+                }
+
+                {
+                    //Second half.
+                    var m0 = empty0 ? Vector256<float>.Zero : Avx.LoadAlignedVector256(s0 + 8);
+                    var m1 = empty1 ? Vector256<float>.Zero : Avx.LoadAlignedVector256(s1 + 8);
+                    var m2 = empty2 ? Vector256<float>.Zero : Avx.LoadAlignedVector256(s2 + 8);
+                    var m3 = empty3 ? Vector256<float>.Zero : Avx.LoadAlignedVector256(s3 + 8);
+                    var m4 = empty4 ? Vector256<float>.Zero : Avx.LoadAlignedVector256(s4 + 8);
+                    var m5 = empty5 ? Vector256<float>.Zero : Avx.LoadAlignedVector256(s5 + 8);
+                    var m6 = empty6 ? Vector256<float>.Zero : Avx.LoadAlignedVector256(s6 + 8);
+                    var m7 = empty7 ? Vector256<float>.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<float>.Zero : Avx.LoadAlignedVector256(s0 + offsetInFloats);
+                    var m1 = empty1 ? Vector256<float>.Zero : Avx.LoadAlignedVector256(s1 + offsetInFloats);
+                    var m2 = empty2 ? Vector256<float>.Zero : Avx.LoadAlignedVector256(s2 + offsetInFloats);
+                    var m3 = empty3 ? Vector256<float>.Zero : Avx.LoadAlignedVector256(s3 + offsetInFloats);
+                    var m4 = empty4 ? Vector256<float>.Zero : Avx.LoadAlignedVector256(s4 + offsetInFloats);
+                    var m5 = empty5 ? Vector256<float>.Zero : Avx.LoadAlignedVector256(s5 + offsetInFloats);
+                    var m6 = empty6 ? Vector256<float>.Zero : Avx.LoadAlignedVector256(s6 + offsetInFloats);
+                    var m7 = empty7 ? Vector256<float>.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
+            {
+                Unsafe.SkipInit(out position);
+                Unsafe.SkipInit(out orientation);
+                Unsafe.SkipInit(out velocity);
+                Unsafe.SkipInit(out inertia);
+                FallbackGatherMotionState(solverStates, encodedBodyIndices, ref position, ref orientation, ref velocity);
+                FallbackGatherInertia(solverStates, encodedBodyIndices, ref inertia, worldInertia ? 24 : 16);
+            }
+        }
+
+        //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, Vector<int> encodedBodyIndices, Vector<int> mask)
+        {
+            if (Avx.IsSupported && Vector<float>.Count == 8)
+            {
+                var states = ActiveSet.DynamicsState.Memory;
+                {
+                    var m0 = orientation.X.AsVector256();
+                    var m1 = orientation.Y.AsVector256();
+                    var m2 = orientation.Z.AsVector256();
+                    var m3 = orientation.W.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); //Laze alert.
+                    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));
+
+                    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(); }
+                    //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
+            {
+                for (int innerIndex = 0; innerIndex < Vector<int>.Count; ++innerIndex)
+                {
+                    if (mask[innerIndex] == 0)
+                        continue;
+                    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]);
+
+                }
+            }
+
+        }
+
+
+        //[MethodImpl(MethodImplOptions.AggressiveInlining)]
+        public unsafe void ScatterInertia(
+            ref BodyInertiaWide inertia, Vector<int> encodedBodyIndices, Vector<int> mask)
+        {
+            if (Avx.IsSupported && Vector<float>.Count == 8)
+            {
+                var states = ActiveSet.DynamicsState.Memory;
+                {
+                    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); //Laze alert.
+                    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));
+
+                    //Note the offset; we're scattering into the world inertias.
+                    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(); }
+                    //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
+            {
+                for (int innerIndex = 0; innerIndex < Vector<int>.Count; ++innerIndex)
+                {
+                    if (mask[innerIndex] == 0)
+                        continue;
+                    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];
+                    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];
+                }
+            }
+        }
+
+
+        //[MethodImpl(MethodImplOptions.AggressiveInlining)]
+        public unsafe void ScatterVelocities<TAccessFilter>(ref BodyVelocityWide sourceVelocities, ref Vector<int> encodedBodyIndices) where TAccessFilter : unmanaged, IBodyAccessFilter
+        {
+            if (Avx.IsSupported && Vector<float>.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);
+
+                if (filter.AccessLinearVelocity ^ filter.AccessAngularVelocity)
+                {
+                    //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<float> 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 = (uint*)Unsafe.AsPointer(ref encodedBodyIndices);
+                    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());
+                    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
+                {
+                    //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();
+
+                    //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 = (uint*)Unsafe.AsPointer(ref encodedBodyIndices);
+                    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)));
+                    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)));
+                }
+
+                //{
+                //    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
+            {
+                var indices = (uint*)Unsafe.AsPointer(ref encodedBodyIndices);
+                for (int innerIndex = 0; innerIndex < Vector<int>.Count; ++innerIndex)
+                {
+                    if (indices[innerIndex] >= DynamicLimit)
+                        continue;
+                    ref var sourceSlot = ref GetOffsetInstance(ref sourceVelocities, innerIndex);
+                    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/BodyDescription.cs b/BepuPhysics/BodyDescription.cs
index 73d57652d..5c97d6e4d 100644
--- a/BepuPhysics/BodyDescription.cs
+++ b/BepuPhysics/BodyDescription.cs
@@ -1,12 +1,12 @@
 using BepuPhysics.Collidables;
-using BepuPhysics.Constraints;
-using BepuUtilities;
-using BepuUtilities.Memory;
-using System.Numerics;
-using System.Runtime.CompilerServices;
+using System.Runtime.InteropServices;
 
 namespace BepuPhysics
 {
+    /// <summary>
+    /// Describes the thresholds for a body going to sleep.
+    /// </summary>
+    [StructLayout(LayoutKind.Sequential)]
     public struct BodyActivityDescription
     {
         /// <summary>
@@ -30,14 +30,43 @@ public BodyActivityDescription(float sleepThreshold, byte minimumTimestepCountUn
             SleepThreshold = sleepThreshold;
             MinimumTimestepCountUnderThreshold = minimumTimestepCountUnderThreshold;
         }
+
+        /// <summary>
+        /// Creates a body activity description. Uses a <see cref="MinimumTimestepCountUnderThreshold"/> of 32.
+        /// </summary>
+        /// <param name="sleepThreshold">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.</param>
+        public static implicit operator BodyActivityDescription(float sleepThreshold)
+        {
+            return new BodyActivityDescription(sleepThreshold);
+        }
     }
 
+    /// <summary>
+    /// Describes a body's state.
+    /// </summary>
+    [StructLayout(LayoutKind.Sequential)]
     public struct BodyDescription
     {
+        /// <summary>
+        /// Position and orientation of the body.
+        /// </summary>
         public RigidPose Pose;
-        public BodyInertia LocalInertia;
+        /// <summary>
+        /// Linear and angular velocity of the body.
+        /// </summary>
         public BodyVelocity Velocity;
+        /// <summary>
+        /// Mass and inertia tensor of the body.
+        /// </summary>
+        public BodyInertia LocalInertia;
+        /// <summary>
+        /// Shape and collision detection settings for the body.
+        /// </summary>
         public CollidableDescription Collidable;
+        /// <summary>
+        /// Sleeping settings for the body.
+        /// </summary>
         public BodyActivityDescription Activity;
 
         //Convex shape helpers.
@@ -79,7 +108,7 @@ public static BodyActivityDescription GetDefaultActivity<TShape>(in TShape shape
         /// <param name="collidable">Collidable to associate with the body.</param>
         /// <param name="activity">Activity settings for the body.</param>
         /// <returns>Constructed description for the body.</returns>
-        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 };
         }
@@ -92,38 +121,11 @@ public static BodyDescription CreateDynamic(in RigidPose pose, in BodyVelocity v
         /// <param name="collidable">Collidable to associate with the body.</param>
         /// <param name="activity">Activity settings for the body.</param>
         /// <returns>Constructed description for the body.</returns>
-        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 };
         }
 
-        /// <summary>
-        /// Creates a dynamic body description with identity orientation.
-        /// </summary>
-        /// <param name="position">Position of the body.</param>
-        /// <param name="velocity">Initial velocity of the body.</param>
-        /// <param name="inertia">Local inertia of the body.</param>
-        /// <param name="collidable">Collidable to associate with the body.</param>
-        /// <param name="activity">Activity settings for the body.</param>
-        /// <returns>Constructed description for the body.</returns>
-        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 };
-        }
-
-        /// <summary>
-        /// Creates a dynamic body description with zero initial velocity and identity orientation.
-        /// </summary>
-        /// <param name="position">Position of the body.</param>
-        /// <param name="inertia">Local inertia of the body.</param>
-        /// <param name="collidable">Collidable to associate with the body.</param>
-        /// <param name="activity">Activity settings for the body.</param>
-        /// <returns>Constructed description for the body.</returns>
-        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 };
-        }
-
         /// <summary>
         /// Creates a dynamic body description with collidable, inertia, and activity descriptions generated from a convex shape. Adds the shape to the given shape set.
         /// </summary>
@@ -134,38 +136,19 @@ public static BodyDescription CreateDynamic(in Vector3 position, in BodyInertia
         /// <param name="shapes">Shape collection to add the shape to.</param>
         /// <param name="shape">Shape to add to the shape set and to create the body from.</param>
         /// <returns>Constructed description for the body.</returns>
-        public static BodyDescription CreateConvexDynamic<TConvexShape>(
-            in RigidPose pose, in BodyVelocity velocity, float mass, Shapes shapes, in TConvexShape shape)
-            where TConvexShape : unmanaged, IConvexShape
+        public static BodyDescription CreateConvexDynamic<TConvexShape>(RigidPose pose, BodyVelocity velocity, float mass, Shapes shapes, in TConvexShape shape) where TConvexShape : unmanaged, IConvexShape
         {
             var description = new BodyDescription
             {
                 Pose = pose,
                 Velocity = velocity,
                 Activity = GetDefaultActivity(shape),
-                Collidable = new CollidableDescription(shapes.Add(shape), GetDefaultSpeculativeMargin(shape))
+                Collidable = shapes.Add(shape)
             };
-            shape.ComputeInertia(mass, out description.LocalInertia);
+            description.LocalInertia = shape.ComputeInertia(mass);
             return description;
         }
 
-        /// <summary>
-        /// 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.
-        /// </summary>
-        /// <typeparam name="TConvexShape">Type of the shape to create a body for.</typeparam>
-        /// <param name="position">Position of the body.</param>
-        /// <param name="velocity">Initial velocity of the body.</param>
-        /// <param name="mass">Mass of the body. The inertia tensor will be calculated based on this mass and the shape.</param>
-        /// <param name="shapes">Shape collection to add the shape to.</param>
-        /// <param name="shape">Shape to add to the shape set and to create the body from.</param>
-        /// <returns>Constructed description for the body.</returns>
-        public static BodyDescription CreateConvexDynamic<TConvexShape>(
-            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);
-        }
-
         /// <summary>
         /// 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.
         /// </summary>
@@ -175,29 +158,11 @@ public static BodyDescription CreateConvexDynamic<TConvexShape>(
         /// <param name="shapes">Shape collection to add the shape to.</param>
         /// <param name="shape">Shape to add to the shape set and to create the body from.</param>
         /// <returns>Constructed description for the body.</returns>
-        public static BodyDescription CreateConvexDynamic<TConvexShape>(
-            in RigidPose pose, float mass, Shapes shapes, in TConvexShape shape)
-            where TConvexShape : unmanaged, IConvexShape
+        public static BodyDescription CreateConvexDynamic<TConvexShape>(RigidPose pose, float mass, Shapes shapes, in TConvexShape shape) where TConvexShape : unmanaged, IConvexShape
         {
             return CreateConvexDynamic(pose, default, mass, shapes, shape);
         }
 
-        /// <summary>
-        /// 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.
-        /// </summary>
-        /// <typeparam name="TConvexShape">Type of the shape to create a body for.</typeparam>
-        /// <param name="position">Position of the body.</param>
-        /// <param name="mass">Mass of the body. The inertia tensor will be calculated based on this mass and the shape.</param>
-        /// <param name="shapes">Shape collection to add the shape to.</param>
-        /// <param name="shape">Shape to add to the shape set and to create the body from.</param>
-        /// <returns>Constructed description for the body.</returns>
-        public static BodyDescription CreateConvexDynamic<TConvexShape>(
-            in Vector3 position, float mass, Shapes shapes, in TConvexShape shape)
-            where TConvexShape : unmanaged, IConvexShape
-        {
-            return CreateConvexDynamic(new RigidPose(position), default, mass, shapes, shape);
-        }
-
         /// <summary>
         /// Creates a kinematic body description.
         /// </summary>
@@ -206,7 +171,7 @@ public static BodyDescription CreateConvexDynamic<TConvexShape>(
         /// <param name="collidable">Collidable to associate with the body.</param>
         /// <param name="activity">Activity settings for the body.</param>
         /// <returns>Constructed description for the body.</returns>
-        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 };
         }
@@ -218,36 +183,11 @@ public static BodyDescription CreateKinematic(in RigidPose pose, in BodyVelocity
         /// <param name="collidable">Collidable to associate with the body.</param>
         /// <param name="activity">Activity settings for the body.</param>
         /// <returns>Constructed description for the body.</returns>
-        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 };
         }
 
-        /// <summary>
-        /// Creates a kinematic body description with identity orientation.
-        /// </summary>
-        /// <param name="position">Position of the body.</param>
-        /// <param name="velocity">Initial velocity of the body.</param>
-        /// <param name="collidable">Collidable to associate with the body.</param>
-        /// <param name="activity">Activity settings for the body.</param>
-        /// <returns>Constructed description for the body.</returns>
-        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 };
-        }
-
-        /// <summary>
-        /// Creates a kinematic body description with identity orientation and zero initial velocity.
-        /// </summary>
-        /// <param name="position">Position of the body.</param>
-        /// <param name="collidable">Collidable to associate with the body.</param>
-        /// <param name="activity">Activity settings for the body.</param>
-        /// <returns>Constructed description for the body.</returns>
-        public static BodyDescription CreateKinematic(in Vector3 position, in CollidableDescription collidable, in BodyActivityDescription activity)
-        {
-            return new BodyDescription { Pose = new RigidPose(position), Activity = activity, Collidable = collidable };
-        }
-
         /// <summary>
         /// Creates a kinematic body description with collidable and activity descriptions generated from a convex shape. Adds the shape to the given shape set.
         /// </summary>
@@ -257,35 +197,18 @@ public static BodyDescription CreateKinematic(in Vector3 position, in Collidable
         /// <param name="shapes">Shape collection to add the shape to.</param>
         /// <param name="shape">Shape to add to the shape set and to create the body from.</param>
         /// <returns>Constructed description for the body.</returns>
-        public static BodyDescription CreateConvexKinematic<TConvexShape>(
-            in RigidPose pose, in BodyVelocity velocity, Shapes shapes, in TConvexShape shape)
-            where TConvexShape : unmanaged, IConvexShape
+        public static BodyDescription CreateConvexKinematic<TConvexShape>(RigidPose pose, BodyVelocity velocity, Shapes shapes, in TConvexShape shape) where TConvexShape : unmanaged, IConvexShape
         {
             var description = new BodyDescription
             {
                 Pose = pose,
                 Velocity = velocity,
                 Activity = GetDefaultActivity(shape),
-                Collidable = new CollidableDescription(shapes.Add(shape), GetDefaultSpeculativeMargin(shape))
+                Collidable = shapes.Add(shape)
             };
             return description;
         }
 
-        /// <summary>
-        /// 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.
-        /// </summary>
-        /// <typeparam name="TConvexShape">Type of the shape to create a body for.</typeparam>
-        /// <param name="position">Position of the body.</param>
-        /// <param name="velocity">Initial velocity of the body.</param>
-        /// <param name="shapes">Shape collection to add the shape to.</param>
-        /// <param name="shape">Shape to add to the shape set and to create the body from.</param>
-        /// <returns>Constructed description for the body.</returns>
-        public static BodyDescription CreateConvexKinematic<TConvexShape>(
-            in Vector3 position, in BodyVelocity velocity, Shapes shapes, in TConvexShape shape)
-            where TConvexShape : unmanaged, IConvexShape
-        {
-            return CreateConvexKinematic(new RigidPose(position), velocity, shapes, shape);
-        }
 
         /// <summary>
         /// 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.
@@ -295,29 +218,9 @@ public static BodyDescription CreateConvexKinematic<TConvexShape>(
         /// <param name="shapes">Shape collection to add the shape to.</param>
         /// <param name="shape">Shape to add to the shape set and to create the body from.</param>
         /// <returns>Constructed description for the body.</returns>
-        public static BodyDescription CreateConvexKinematic<TConvexShape>(
-            in RigidPose pose, Shapes shapes, in TConvexShape shape)
-            where TConvexShape : unmanaged, IConvexShape
+        public static BodyDescription CreateConvexKinematic<TConvexShape>(RigidPose pose, Shapes shapes, TConvexShape shape) where TConvexShape : unmanaged, IConvexShape
         {
             return CreateConvexKinematic(pose, default, shapes, shape);
         }
-
-        /// <summary>
-        /// 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.
-        /// </summary>
-        /// <typeparam name="TConvexShape">Type of the shape to create a body for.</typeparam>
-        /// <param name="position">Position of the body.</param>
-        /// <param name="shapes">Shape collection to add the shape to.</param>
-        /// <param name="shape">Shape to add to the shape set and to create the body from.</param>
-        /// <returns>Constructed description for the body.</returns>
-        public static BodyDescription CreateConvexKinematic<TConvexShape>(
-            in Vector3 position, Shapes shapes, in TConvexShape shape)
-            where TConvexShape : unmanaged, IConvexShape
-        {
-            return CreateConvexKinematic(new RigidPose(position), default, shapes, shape);
-        }
-
     }
-
-
 }
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
-{
-    /// <summary>
-    /// 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.
-    /// </summary>
-    public partial class BodyLayoutOptimizer
-    {
-        Bodies bodies;
-        BroadPhase broadPhase;
-        Solver solver;
-
-        float optimizationFraction;
-        /// <summary>
-        /// Gets or sets the fraction of all bodies to update each frame.
-        /// </summary>
-        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<int>
-        {
-            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/BodyProperties.cs b/BepuPhysics/BodyProperties.cs
index 180ff6936..85d93c495 100644
--- a/BepuPhysics/BodyProperties.cs
+++ b/BepuPhysics/BodyProperties.cs
@@ -1,43 +1,127 @@
-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;
 
 namespace BepuPhysics
 {
+    /// <summary>
+    /// Describes the pose and velocity of a body.
+    /// </summary>
+    [StructLayout(LayoutKind.Sequential, Size = 64, Pack = 1)]
+    public struct MotionState
+    {
+        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 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;
+
+        /// <summary>
+        /// Pose of the body.
+        /// </summary>
+        public RigidPose Pose;
+        /// <summary>
+        /// Linear and angular velocity of the body.
+        /// </summary>
+        public BodyVelocity Velocity;
+
+        /// <summary>
+        /// Returns a string representing the MotionState.
+        /// </summary>
+        /// <returns>String representing the MotionState.</returns>
+        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.
     //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.
     /// <summary>
     /// Represents a rigid transformation.
     /// </summary>
+    [StructLayout(LayoutKind.Sequential, Size = 32, 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.
+        /// <summary>
+        /// Orientation of the pose.
+        /// </summary>
         public Quaternion Orientation;
+        /// <summary>
+        /// Position of the pose.
+        /// </summary>
+        public Vector3 Position;
 
-        public static RigidPose Identity { get; } = new RigidPose(new Vector3());
+        /// <summary>
+        /// Returns a pose with a position at (0,0,0) and identity orientation.
+        /// </summary>
+        public static RigidPose Identity => new RigidPose(default);
 
+        /// <summary>
+        /// Creates a rigid pose with the given position and orientation.
+        /// </summary>
+        /// <param name="position">Position of the pose.</param>
+        /// <param name="orientation">Orientation of the pose.</param>
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public RigidPose(in Vector3 position, in Quaternion orientation)
+        public RigidPose(Vector3 position, Quaternion orientation)
         {
             Position = position;
             Orientation = orientation;
         }
+
+        /// <summary>
+        /// Creates a rigid pose with the given position and identity orientation.
+        /// </summary>
+        /// <param name="position">Position of the pose.</param>
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public RigidPose(in Vector3 position)
+        public RigidPose(Vector3 position)
         {
             Position = position;
             Orientation = Quaternion.Identity;
         }
 
+        /// <summary>
+        /// Creates a pose by treating a <see cref="Vector3"/> as a position. Orientation is set to identity.
+        /// </summary>
+        /// <param name="position">Position to use in the pose.</param>
+        public static implicit operator RigidPose(Vector3 position)
+        {
+            return new RigidPose(position);
+        }
+
+        /// <summary>
+        /// Creates a pose by treating a <see cref="Quaternion"/> as an orientation in the pose. Position is set to zero.
+        /// </summary>
+        /// <param name="orientation">Orientation to use in the pose.</param>
+        public static implicit operator RigidPose(Quaternion orientation)
+        {
+            return new RigidPose(default, orientation);
+        }
+
+        /// <summary>
+        /// Creates a pose from a tuple of a position and orientation.
+        /// </summary>
+        /// <param name="poseComponents">Position and orientation to use in the pose.</param>
+        public static implicit operator RigidPose((Vector3 position, Quaternion orientation) poseComponents)
+        {
+            return new RigidPose(poseComponents.position, poseComponents.orientation);
+        }
+
         /// <summary>
         /// Transforms a vector by the rigid pose: v * pose.Orientation + pose.Position.
         /// </summary>
@@ -45,7 +129,7 @@ public RigidPose(in Vector3 position)
         /// <param name="pose">Pose to transform the vector with.</param>
         /// <param name="result">Transformed vector.</param>
         [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;
@@ -57,7 +141,7 @@ public static void Transform(in Vector3 v, in RigidPose pose, out Vector3 result
         /// <param name="pose">Pose to invert and transform the vector with.</param>
         /// <param name="result">Transformed vector.</param>
         [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);
@@ -87,76 +171,218 @@ 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;
         }
+
+        /// <summary>
+        /// Returns a string representing the RigidPose as "Position, Orientation".
+        /// </summary>
+        /// <returns>String representing the RigidPose.</returns>
+        public override string ToString()
+        {
+            return $"{Position}, {Orientation}";
+        }
     }
 
+    /// <summary>
+    /// Linear and angular velocity for a body.
+    /// </summary>
+    [StructLayout(LayoutKind.Explicit, Size = 32)]
     public struct BodyVelocity
     {
+        /// <summary>
+        /// Linear velocity associated with the body.
+        /// </summary>
+        [FieldOffset(0)]
         public Vector3 Linear;
+
+        /// <summary>
+        /// Angular velocity associated with the body.
+        /// </summary>
+        [FieldOffset(16)]
         public Vector3 Angular;
 
+        /// <summary>
+        /// Creates a new set of body velocities. Angular velocity is set to zero.
+        /// </summary>
+        /// <param name="linear">Linear velocity to use for the body.</param>
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public BodyVelocity(in Vector3 linear)
+        public BodyVelocity(Vector3 linear)
         {
             Linear = linear;
             Angular = default;
         }
 
+        /// <summary>
+        /// Creates a new set of body velocities.
+        /// </summary>
+        /// <param name="linear">Linear velocity to use for the body.</param>
+        /// <param name="angular">Angular velocity to use for the body.</param>
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public BodyVelocity(in Vector3 linear, in Vector3 angular)
+        public BodyVelocity(Vector3 linear, Vector3 angular)
         {
             Linear = linear;
             Angular = angular;
         }
+
+        /// <summary>
+        /// Creates a body velocity by treating a <see cref="Vector3"/> as a linear velocity. Angular velocity is set to zero.
+        /// </summary>
+        /// <param name="linearVelocity">Linear velocity to use in the body velocity.</param>
+        public static implicit operator BodyVelocity(Vector3 linearVelocity)
+        {
+            return new BodyVelocity(linearVelocity);
+        }
+
+        /// <summary>
+        /// Creates a body velocity from a tuple of linear and angular velocities..
+        /// </summary>
+        /// <param name="velocities">Velocities to use in the body velocity.</param>
+        public static implicit operator BodyVelocity((Vector3 linearVelocity, Vector3 angularVelocity) velocities)
+        {
+            return new BodyVelocity(velocities.linearVelocity, velocities.angularVelocity);
+        }
+
+        /// <summary>
+        /// Returns a string representing the BodyVelocity as "Linear, Angular".
+        /// </summary>
+        /// <returns>String representing the BodyVelocity.</returns>
+        public override string ToString()
+        {
+            return $"{Linear}, {Angular}";
+        }
     }
 
+    /// <summary>
+    /// Stores the inertia for a body.
+    /// </summary>
+    /// <remarks>This representation stores the inverse mass and inverse inertia tensor. Most of the high frequency use cases in the engine naturally use the inverse.</remarks>
+    [StructLayout(LayoutKind.Sequential, Size = 32, Pack = 4)]
     public struct BodyInertia
     {
+        /// <summary>
+        /// Inverse of the body's inertia tensor.
+        /// </summary>
         public Symmetric3x3 InverseInertiaTensor;
+        /// <summary>
+        /// Inverse of the body's mass.
+        /// </summary>
         public float InverseMass;
+
+        /// <summary>
+        /// Returns a string representing the BodyInertia as "InverseMass, InverseInertiaTensor".
+        /// </summary>
+        /// <returns>String representing the BodyInertia.</returns>
+        public override string ToString()
+        {
+            return $"{InverseMass}, {InverseInertiaTensor}";
+        }
     }
 
-    public struct RigidPoses
+    /// <summary>
+    /// Stores the local and world views of a body's inertia, packed together for efficient access.
+    /// </summary>
+    [StructLayout(LayoutKind.Sequential)]
+    public struct BodyInertias
+    {
+        /// <summary>
+        /// Local inertia of the body.
+        /// </summary>
+        public BodyInertia Local;
+        /// <summary>
+        /// 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.
+        /// </summary>
+        /// <remarks>
+        /// 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.</remarks>
+        public BodyInertia World;
+
+        /// <summary>
+        /// Returns a string representing the BodyInertias.
+        /// </summary>
+        /// <returns>String representing the BodyInertias.</returns>
+        public override string ToString()
+        {
+            return $"Local: {Local}, World: {World}";
+        }
+    }
+
+    /// <summary>
+    /// Stores all body information needed by the solver together.
+    /// </summary>
+    /// <remarks>
+    /// 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.
+    /// </remarks>
+    [StructLayout(LayoutKind.Sequential)]
+    public struct BodyDynamics
+    {
+        /// <summary>
+        /// Pose and velocity information for the body.
+        /// </summary>
+        public MotionState Motion;
+        /// <summary>
+        /// Inertia information for the body.
+        /// </summary>
+        public BodyInertias Inertia;
+
+        /// <summary>
+        /// Returns a string representing the BodyDynamics.
+        /// </summary>
+        /// <returns>String representing the BodyDynamics.</returns>
+        public override string ToString()
+        {
+            return $"Motion: {Motion}, Inertia: {Inertia}";
+        }
+    }
+
+
+    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);
         }
     }
 
-    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...
+        //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<float> InverseMass;
     }
 
+    /// <summary>
+    /// Describes how a body sleeps, and its current state with respect to sleeping.
+    /// </summary>
     public struct BodyActivity
     {
         /// <summary>
diff --git a/BepuPhysics/BodyReference.cs b/BepuPhysics/BodyReference.cs
index 2cd84a271..91b204b83 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
 {
@@ -30,6 +26,7 @@ public struct BodyReference
         /// </summary>
         /// <param name="handle">Handle of the body to refer to.</param>
         /// <param name="bodies">Collection containing the body.</param>
+        /// <remarks>This is equivalent to <see cref="Bodies.GetBodyReference"/> and <see cref="Bodies.this[BodyHandle]"/>.</remarks>
         public BodyReference(BodyHandle handle, Bodies bodies)
         {
             Handle = handle;
@@ -98,7 +95,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].DynamicsState[location.Index].Motion.Velocity;
             }
         }
 
@@ -111,7 +108,33 @@ 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].DynamicsState[location.Index].Motion.Pose;
+            }
+        }
+
+        /// <summary>
+        /// Gets a reference to the body's motion state, including both pose and velocity.
+        /// </summary>
+        public ref MotionState MotionState
+        {
+            [MethodImpl(MethodImplOptions.NoInlining)]
+            get
+            {
+                ref var location = ref MemoryLocation;
+                return ref Bodies.Sets[location.SetIndex].DynamicsState[location.Index].Motion;
+            }
+        }
+
+        /// <summary>
+        /// Gets a reference to the body's solver-relevant state, including pose, velocity, and inertia.
+        /// </summary>
+        public ref BodyDynamics Dynamics
+        {
+            [MethodImpl(MethodImplOptions.NoInlining)]
+            get
+            {
+                ref var location = ref MemoryLocation;
+                return ref Bodies.Sets[location.SetIndex].DynamicsState[location.Index];
             }
         }
 
@@ -137,7 +160,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].DynamicsState[location.Index].Inertia.Local;
             }
         }
 
@@ -182,12 +205,12 @@ public CollidableReference CollidableReference
         /// <summary>
         /// Gets whether the body is kinematic, meaning its inverse inertia and mass are all zero.
         /// </summary>
-        public bool Kinematic { get { return Bodies.IsKinematic(LocalInertia); } }
+        public bool Kinematic { get { return Bodies.IsKinematicUnsafeGCHole(ref LocalInertia); } }
 
         /// <summary>
         /// Gets whether the body has locked inertia, meaning its inverse inertia tensor is zero.
         /// </summary>
-        public bool HasLockedInertia { get { return Bodies.HasLockedInertia(LocalInertia.InverseInertiaTensor); } }
+        public unsafe bool HasLockedInertia { get { return Bodies.HasLockedInertia((Symmetric3x3*)Unsafe.AsPointer(ref LocalInertia.InverseInertiaTensor)); } }
 
         /// <summary>
         /// If the body is dynamic, turns the body kinematic by setting all inverse inertia and mass values to zero and activates it.
@@ -219,9 +242,10 @@ 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];
-            PoseIntegration.RotateInverseInertia(localInertia.InverseInertiaTensor, pose.Orientation, out 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.
+            ref var state = ref set.DynamicsState[location.Index];
+            PoseIntegration.RotateInverseInertia(state.Inertia.Local.InverseInertiaTensor, state.Motion.Pose.Orientation, out inverseInertia);
         }
 
         /// <summary>
@@ -324,7 +348,7 @@ public void UpdateBounds()
         /// <param name="impulse">Impulse to apply to the body.</param>
         /// <param name="impulseOffset">World space offset from the center of the body to apply the impulse at.</param>
         [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);
@@ -339,12 +363,10 @@ public static void ApplyImpulse(in Vector3 impulse, in Vector3 impulseOffset, re
         /// <param name="impulse">Impulse to apply to the body.</param>
         /// <param name="impulseOffset">World space offset from the center of the body to apply the impulse at.</param>
         [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 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.DynamicsState[index];
+            ApplyImpulse(impulse, impulseOffset, ref state.Inertia.Local, ref state.Motion.Pose, ref state.Motion.Velocity);
         }
 
         /// <summary>
@@ -354,7 +376,7 @@ public static void ApplyImpulse(in BodySet set, int index, in Vector3 impulse, i
         /// <param name="inverseInertiaTensor">Inverse inertia tensor to transform the impulse with.</param>
         /// <param name="angularVelocity">Angular velocity to be modified.</param>
         [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;
@@ -367,19 +389,19 @@ public static void ApplyAngularImpulse(in Vector3 angularImpulse, in Symmetric3x
         /// <param name="inverseMass">Inverse mass to transform the impulse with.</param>
         /// <param name="linearVelocity">Linear velocity to be modified.</param>
         [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;
         }
 
 
         /// <summary>
-        /// 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.
         /// </summary>
         /// <param name="impulse">Impulse to apply to the body.</param>
         /// <param name="impulseOffset">World space offset to apply the impulse at.</param>
         [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);
@@ -390,20 +412,21 @@ public void ApplyImpulse(in Vector3 impulse, in Vector3 impulseOffset)
         /// </summary>
         /// <param name="impulse">Impulse to apply to the velocity.</param>
         [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];
-            ApplyLinearImpulse(impulse, set.LocalInertias[location.Index].InverseMass, ref set.Velocities[location.Index].Linear);
+            ref var state = ref set.DynamicsState[location.Index];
+            ApplyLinearImpulse(impulse, state.Inertia.Local.InverseMass, ref state.Motion.Velocity.Linear);
         }
 
         /// <summary>
-        /// Computes the velocity of an offset point attached to the body.
+        /// Computes the velocity of a world space offset point attached to the body.
         /// </summary>
-        /// <param name="offset">Offset from the body's center to </param>
+        /// <param name="offset">World space offset from the body's center to the point to measure.</param>
         /// <param name="velocity">Effective velocity of the point if it were attached to the body.</param>
         [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);
         }
@@ -413,14 +436,24 @@ public void GetVelocityForOffset(in Vector3 offset, out Vector3 velocity)
         /// </summary>
         /// <param name="angularImpulse">Impulse to apply to the velocity.</param>
         [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];
-            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.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);
+            ApplyAngularImpulse(angularImpulse, inverseInertia, ref state.Motion.Velocity.Angular);
+        }
+
+        /// <summary>
+        /// Implicitly converts a <see cref="BodyReference"/> to the <see cref="BodyHandle"/> that the body reference was created from.
+        /// </summary>
+        /// <param name="reference">Body reference to extract the handle from.</param>
+        public static implicit operator BodyHandle(BodyReference reference)
+        {
+            return reference.Handle;
         }
     }
 }
diff --git a/BepuPhysics/BodySet.cs b/BepuPhysics/BodySet.cs
index 789096f4e..e994eb30d 100644
--- a/BepuPhysics/BodySet.cs
+++ b/BepuPhysics/BodySet.cs
@@ -35,9 +35,10 @@ public struct BodySet
         /// </summary>
         public Buffer<BodyHandle> IndexToHandle;
 
-        public Buffer<RigidPose> Poses;
-        public Buffer<BodyVelocity> Velocities;
-        public Buffer<BodyInertia> LocalInertias;
+        /// <summary>
+        /// Stores all data involved in solving constraints for a body, including pose, velocity, and inertia.
+        /// </summary>
+        public Buffer<BodyDynamics> DynamicsState;
 
         /// <summary>
         /// The collidables owned by each body in the set. Speculative margins, continuity settings, and shape indices can be changed directly.
@@ -89,9 +90,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];
-                LocalInertias[bodyIndex] = LocalInertias[movedBodyIndex];
+                DynamicsState[bodyIndex] = DynamicsState[movedBodyIndex];
                 Activity[bodyIndex] = Activity[movedBodyIndex];
                 Collidables[bodyIndex] = Collidables[movedBodyIndex];
                 //Note that the constraint list is NOT disposed before being overwritten.
@@ -126,15 +125,24 @@ 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;
-            LocalInertias[index] = description.LocalInertia;
+            ref var state = ref DynamicsState[index];
+            state.Motion.Pose = description.Pose;
+            state.Motion.Velocity = description.Velocity;
+            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;
-            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;
+            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.
+            collidable.SpeculativeMargin = 0;
             ref var activity = ref Activity[index];
             activity.SleepThreshold = description.Activity.SleepThreshold;
             activity.MinimumTimestepsUnderThreshold = description.Activity.MinimumTimestepCountUnderThreshold;
@@ -144,13 +152,15 @@ 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.LocalInertia = LocalInertias[index];
+            ref var state = ref DynamicsState[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;
-            description.Collidable.SpeculativeMargin = collidable.SpeculativeMargin;
+            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;
@@ -169,8 +179,16 @@ internal void AddConstraint(int bodyIndex, ConstraintHandle constraintHandle, in
             constraints.AllocateUnsafely() = constraint;
         }
 
+        /// <summary>
+        /// Removes a constraint from a body's constraint list.
+        /// </summary>
+        /// <param name="bodyIndex">Index of the body to remove the constraint reference from.</param>
+        /// <param name="constraintHandle">Handle of the constraint to remove.</param>
+        /// <param name="minimumConstraintCapacityPerBody">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.</param>
+        /// <param name="pool">Pool to use to resize the constraint list.</param>
+        /// <returns>True if the number of constraints remaining attached to the body is 0, false otherwise.</returns>
         [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.
@@ -195,6 +213,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)
@@ -210,43 +229,21 @@ public bool BodyIsConstrainedBy(int bodyIndex, ConstraintHandle constraintHandle
             return false;
         }
 
-
-        /// <summary>
-        /// Swaps the memory of two bodies. Indexed by memory slot, not by handle index.
-        /// </summary>
-        /// <param name="slotA">Memory slot of the first body to swap.</param>
-        /// <param name="slotB">Memory slot of the second body to swap.</param>
-        [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        internal void Swap(int slotA, int slotB, ref Buffer<BodyMemoryLocation> 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 Poses[slotA], ref Poses[slotB]);
-            Helpers.Swap(ref Velocities[slotA], ref Velocities[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]);
-        }
-
-        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.
             //You may want to change this in the future if memory use is concerning.
             targetBodyCapacity = BufferPool.GetCapacityForCount<int>(targetBodyCapacity);
-            Debug.Assert(Poses.Length != BufferPool.GetCapacityForCount<RigidPoses>(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);
-            pool.ResizeToAtLeast(ref LocalInertias, targetBodyCapacity, Count);
+            Debug.Assert(DynamicsState.Length != BufferPool.GetCapacityForCount<RigidPoseWide>(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);
             pool.ResizeToAtLeast(ref Constraints, targetBodyCapacity, Count);
         }
 
-        public unsafe void Clear(BufferPool pool)
+        public void Clear(BufferPool pool)
         {
             for (int i = 0; i < Count; ++i)
             {
@@ -261,9 +258,7 @@ public unsafe void Clear(BufferPool pool)
         /// <param name="pool">Pool to return the set's top level buffers to.</param>
         public void DisposeBuffers(BufferPool pool)
         {
-            pool.Return(ref Poses);
-            pool.Return(ref Velocities);
-            pool.Return(ref LocalInertias);
+            pool.Return(ref DynamicsState);
             pool.Return(ref IndexToHandle);
             pool.Return(ref Collidables);
             pool.Return(ref Activity);
diff --git a/BepuPhysics/BoundingBoxHelpers.cs b/BepuPhysics/BoundingBoxHelpers.cs
index 5929164ae..5c5948c6b 100644
--- a/BepuPhysics/BoundingBoxHelpers.cs
+++ b/BepuPhysics/BoundingBoxHelpers.cs
@@ -1,18 +1,16 @@
 using BepuPhysics.Collidables;
 using BepuUtilities;
 using System;
-using System.Collections.Generic;
 using System.Numerics;
 using System.Runtime.CompilerServices;
-using System.Text;
 
 namespace BepuPhysics
 {
     public static class BoundingBoxHelpers
     {
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public static void GetAngularBoundsExpansion(in Vector<float> angularSpeed, in Vector<float> vectorDt, in Vector<float> maximumRadius,
-            in Vector<float> maximumAngularExpansion, out Vector<float> angularExpansion)
+        public static Vector<float> GetAngularBoundsExpansion(Vector<float> angularSpeed, Vector<float> vectorDt, Vector<float> maximumRadius,
+            Vector<float> 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,13 +40,27 @@ An extra few dozen ALU cycles is unlikely to meaningfully change the execution t
             var cosAngleMinusOne = a2 * new Vector<float>(-1f / 2f) + a4 * new Vector<float>(1f / 24f) - a6 * new Vector<float>(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<float>(-2f) * maximumRadius * maximumRadius * cosAngleMinusOne));
         }
 
+
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public static void GetBoundsExpansion(in Vector3Wide linearVelocity, in Vector3Wide angularVelocity, float dt,
-            in Vector<float> maximumRadius, in Vector<float> maximumAngularExpansion, out Vector3Wide minExpansion, out Vector3Wide maxExpansion)
+        public static void GetBoundsExpansion(Vector3Wide linearVelocity, Vector<float> dtWide, Vector<float> angularExpansion, out Vector3Wide minExpansion, out Vector3Wide maxExpansion)
+        {
+            var linearDisplacement = linearVelocity * dtWide;
+            var zero = Vector<float>.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<float> dtWide, Vector<float> maximumRadius, Vector<float> 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,26 +105,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<float> vectorDt = new Vector<float>(dt);
-            Vector3Wide.Scale(linearVelocity, vectorDt, out var linearDisplacement);
-
-            var zero = Vector<float>.Zero;
-            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);
-            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, ref BodyVelocities velocities, float dt,
-            ref Vector<float> maximumRadius, ref Vector<float> maximumAngularExpansion, ref Vector<float> maximumExpansion)
+        public static void ExpandBoundingBoxes(BodyVelocityWide velocities, Vector<float> dtWide, Vector<float> maximumRadius, Vector<float> maximumAngularExpansion, Vector<float> maximumExpansion,
+            ref Vector3Wide min, ref Vector3Wide max)
         {
-            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);
+            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);
@@ -121,10 +125,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 float GetAngularBoundsExpansion(float angularVelocityMagnitude, float dt, float maximumRadius, float maximumAngularExpansion)
         {
-            var angularVelocityMagnitude = angularVelocity.Length();
             var a = MathHelper.Min(angularVelocityMagnitude * dt, MathHelper.Pi / 3f);
             var a2 = a * a;
             var a4 = a2 * a2;
@@ -132,19 +134,29 @@ 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 MathHelper.Min(maximumAngularExpansion, (float)Math.Sqrt(-2f * maximumRadius * maximumRadius * cosAngleMinusOne));
         }
 
+
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public static void GetBoundsExpansion(in Vector3 linearVelocity, in Vector3 angularVelocity, float dt,
+        public static void GetBoundsExpansion(Vector3 linearVelocity, float dt, float angularExpansion, out Vector3 minExpansion, out Vector3 maxExpansion)
+        {
+            var linearDisplacement = linearVelocity * dt;
+            var zero = Vector3.Zero;
+            var broadcastExpansion = new Vector3(angularExpansion);
+            minExpansion = Vector3.Min(zero, linearDisplacement) - broadcastExpansion;
+            maxExpansion = Vector3.Max(zero, linearDisplacement) + broadcastExpansion;
+        }
+
+        [MethodImpl(MethodImplOptions.AggressiveInlining)]
+        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;
             Vector3 zero = default;
             minExpansion = Vector3.Min(zero, linearDisplacement);
             maxExpansion = Vector3.Max(zero, linearDisplacement);
-            GetAngularBoundsExpansion(angularVelocity, dt, maximumRadius, maximumAngularExpansion, out var angularExpansion);
+            var angularExpansion = new Vector3(GetAngularBoundsExpansion(angularVelocity.Length(), dt, maximumRadius, maximumAngularExpansion));
 
             var maximumAllowedExpansionBroadcasted = new Vector3(maximumAllowedExpansion);
             minExpansion = Vector3.Max(-maximumAllowedExpansionBroadcasted, minExpansion - angularExpansion);
@@ -152,7 +164,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);
@@ -161,7 +173,7 @@ public static void ExpandBoundingBox(ref Vector3 min, ref Vector3 max, in Vector
         }
 
         [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<float>.Zero, expansion, out var minExpansion);
             Vector3Wide.Max(Vector<float>.Zero, expansion, out var maxExpansion);
@@ -176,11 +188,12 @@ 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.
         /// </summary>
         [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<float> radiusA, in Vector3Wide localPositionA, in Vector3Wide localRelativeLinearVelocityA, in Vector3Wide angularVelocityA, in Vector3Wide angularVelocityB, float dt,
             in Vector<float> maximumRadius, in Vector<float> maximumAngularExpansion, in Vector<float> maximumAllowedExpansion)
         {
-            GetBoundsExpansion(localRelativeLinearVelocityA, angularVelocityA, dt,
+            var dtWide = new Vector<float>(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<float>.Zero))
@@ -189,7 +202,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<float>(dt), out var angularExpansionB);
+                var angularExpansionB = GetAngularBoundsExpansion(Vector.SquareRoot(angularSpeedBSquared), maximumRadius + worstCaseRadius, maximumAngularExpansion + worstCaseRadius, dtWide);
                 Vector3Wide.Subtract(minExpansion, angularExpansionB, out minExpansion);
                 Vector3Wide.Add(maxExpansion, angularExpansionB, out maxExpansion);
             }
@@ -204,26 +217,8 @@ public static unsafe void ExpandLocalBoundingBoxes(ref Vector3Wide min, ref Vect
             Vector3Wide.Add(max, localPositionA, out max);
         }
 
-        /// <summary>
-        /// Expands a bounding box by an extremely small amount heuristically.
-        /// </summary>
-        /// <param name="maximumRadius">Maximum radius of the shape to base the expansion on.</param>
-        /// <param name="min">Minimum bounds of the shape to expand.</param>
-        /// <param name="max">Maximum bounds of the shape to expand.</param>
-        /// <remarks>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.</remarks>
-        [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public static void EpsilonExpandLocalBoundingBoxes(Vector<float> maximumRadius, ref Vector3Wide min, ref Vector3Wide max)
-        {
-            var expansion = maximumRadius * new Vector<float>(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)
+        public static void ExpandBoundingBox(Vector3 expansion, ref Vector3 min, ref Vector3 max)
         {
             var minExpansion = Vector3.Min(default, expansion);
             var maxExpansion = Vector3.Max(default, expansion);
@@ -234,8 +229,8 @@ public unsafe static void ExpandBoundingBox(in Vector3 expansion, ref Vector3 mi
         /// <summary>
         /// 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.
         /// </summary>
-        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 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.
@@ -250,12 +245,12 @@ 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.Length(), 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 combinedAngularExpansion = angularExpansionA + angularExpansionB;
+            var angularExpansionB = GetAngularBoundsExpansion(velocityB.Angular.Length(), dt, worstCaseRadiusB + maximumRadiusA, worstCaseRadiusB + maximumAngularExpansionA);
+            var combinedAngularExpansion = new Vector3(angularExpansionA + angularExpansionB);
 
             min = localOriginToA + min - combinedAngularExpansion;
             max = localOriginToA + max + combinedAngularExpansion;
@@ -264,8 +259,8 @@ public static unsafe void GetLocalBoundingBoxForSweep(TypedIndex shapeIndex, Sha
         /// <summary>
         /// 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.
         /// </summary>
-        public static unsafe void GetLocalBoundingBoxForSweep<TConvex>(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 void GetLocalBoundingBoxForSweep<TConvex>(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.
@@ -276,11 +271,11 @@ public static unsafe void GetLocalBoundingBoxForSweep<TConvex>(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.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();
-            BoundingBoxHelpers.GetAngularBoundsExpansion(velocityB.Angular, dt, worstCaseRadiusB + maximumRadiusA, worstCaseRadiusB + maximumAngularExpansionA, out var angularExpansionB);
-            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/CollidableProperty.cs b/BepuPhysics/CollidableProperty.cs
index 2d0746d56..62dd41823 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
 {
@@ -50,8 +48,7 @@ public CollidableProperty(Simulation simulation, BufferPool pool = null)
         /// <summary>
         /// Initializes the property collection if the Bodies/Statics-less constructor was used.
         /// </summary>
-        /// <param name="bodies">Bodies collection to track.</param>
-        /// <param name="statics">Statics collection to track.</param>
+        /// <param name="simulation">Simulation whose bodies and statics will be tracked.</param>
         public void Initialize(Simulation simulation)
         {
             if (this.simulation != null)
@@ -94,7 +91,7 @@ public ref T this[StaticHandle staticHandle]
         /// <summary>
         /// Gets a reference to the properties associated with a collidable.
         /// </summary>
-        /// <param name="staticHandle">Collidable to retrieve the properties for.</param>
+        /// <param name="collidable">Collidable to retrieve the properties for.</param>
         /// <returns>Reference to properties associated with a collidable.</returns>
         public ref T this[CollidableReference collidable]
         {
@@ -146,7 +143,7 @@ public ref T Allocate(StaticHandle handle)
         /// <summary>
         /// Ensures there is space for a given collidable reference and returns a reference to the used memory.
         /// </summary>
-        /// <param name="handle">Collidable reference to allocate for.</param>
+        /// <param name="collidableReference">Collidable reference to allocate for.</param>
         /// <returns>Reference to the data for the given collidable.</returns>
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
         public ref T Allocate(CollidableReference collidableReference)
diff --git a/BepuPhysics/Collidables/BigCompound.cs b/BepuPhysics/Collidables/BigCompound.cs
index a172c3b96..33d044bce 100644
--- a/BepuPhysics/Collidables/BigCompound.cs
+++ b/BepuPhysics/Collidables/BigCompound.cs
@@ -4,15 +4,16 @@
 using BepuUtilities.Memory;
 using System.Diagnostics;
 using BepuUtilities;
-using BepuUtilities.Collections;
 using BepuPhysics.Trees;
 using BepuPhysics.CollisionDetection.CollisionTasks;
+using System.Runtime.InteropServices;
 
 namespace BepuPhysics.Collidables
 {
     /// <summary>
     /// Compound shape containing a bunch of shapes accessible through a tree acceleration structure. Useful for compounds with lots of children.
     /// </summary>
+    [StructLayout(LayoutKind.Sequential)]
     public struct BigCompound : ICompoundShape
     {
         /// <summary>
@@ -24,31 +25,95 @@ public struct BigCompound : ICompoundShape
         /// </summary>
         public Buffer<CompoundChild> Children;
 
+        /// <summary>
+        /// Creates a BigCompound shape instance, but leaves the Tree in an unbuilt state. The Tree must be built before the compound can be used.
+        /// </summary>
+        /// <param name="children">Children to use in the compound.</param>
+        /// <param name="pool">Pool used to allocate acceleration structures.</param>
+        /// <returns>Created compound shape.</returns>
+        /// <remarks>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.</remarks>
+        public static BigCompound CreateWithoutTreeBuild(Buffer<CompoundChild> 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.)
+                //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;
+        }
+
+
+        /// <summary>
+        /// Fills a buffer of subtrees according to a buffer of triangles.
+        /// </summary>
+        /// <remarks>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.</remarks>
+        /// <param name="children">Children to build subtrees from.</param>
+        /// <param name="shapes">Shapes set in which child shapes are allocated.</param>
+        /// <param name="subtrees">Subtrees created for the triangles.</param>
+        public static void FillSubtreesForChildren(Span<CompoundChild> children, Shapes shapes, Span<NodeChild> 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);
+            }
+        }
+
+        /// <summary>
+        /// Creates a compound shape instance and builds an acceleration structure using a sweep builder.
+        /// </summary>
+        /// <param name="children">Children to use in the compound.</param>
+        /// <param name="shapes">Shapes set in which child shapes are allocated.</param>
+        /// <param name="pool">Pool used to allocate acceleration structures.</param>
+        /// <returns>Created compound shape.</returns>
+        /// <remarks>The sweep builder is significantly slower than the binned builder, but can sometimes create higher quality trees.
+        /// <para>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.</para></remarks>
+        public unsafe static BigCompound CreateWithSweepBuild(Buffer<CompoundChild> children, Shapes shapes, BufferPool pool)
+        {
+            var compound = CreateWithoutTreeBuild(children, pool);
+            pool.Take<NodeChild>(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<BoundingBox>());
+            pool.Return(ref subtrees);
+            return compound;
+        }
+
         /// <summary>
         /// Creates a compound shape with an acceleration structure.
         /// </summary>
         /// <param name="children">Set of children in the compound.</param>
         /// <param name="shapes">Shapes set in which child shapes are allocated.</param>
         /// <param name="pool">Pool to use to allocate acceleration structures.</param>
+        /// <param name="dispatcher">Thread dispatcher to use to multithread the acceleration structure build, if any.</param>
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public BigCompound(Buffer<CompoundChild> children, Shapes shapes, BufferPool pool)
+        public unsafe BigCompound(Buffer<CompoundChild> 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<BoundingBox> 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<NodeChild> subtrees);
+            FillSubtreesForChildren(children, shapes, subtrees);
+            Tree.BinnedBuild(subtrees, pool, dispatcher);
+            pool.Return(ref subtrees);
         }
 
-        public void ComputeBounds(in 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)
@@ -60,9 +125,9 @@ public void ComputeBounds(in Quaternion orientation, Shapes shapeBatches, out Ve
         }
 
         [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<TRayHitHandler> : IRayLeafTester where TRayHitHandler : struct, IShapeRayHitHandler
@@ -87,7 +152,7 @@ public LeafTester(in Buffer<CompoundChild> 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, BufferPool pool)
             {
                 if (Handler.AllowTest(leafIndex))
                 {
@@ -95,7 +160,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.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.");
@@ -106,18 +171,18 @@ public unsafe void TestLeaf(int leafIndex, RayData* rayData, float* maximumT)
             }
         }
 
-        public void RayTest<TRayHitHandler>(in RigidPose pose, in RayData ray, ref float maximumT, Shapes shapes, ref TRayHitHandler hitHandler) where TRayHitHandler : struct, IShapeRayHitHandler
+        public readonly void RayTest<TRayHitHandler>(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<TRayHitHandler>(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<TRayHitHandler>(in RigidPose pose, ref RaySource rays, Shapes shapes, ref TRayHitHandler hitHandler) where TRayHitHandler : struct, IShapeRayHitHandler
+        public readonly unsafe void RayTest<TRayHitHandler>(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.
@@ -130,30 +195,68 @@ public unsafe void RayTest<TRayHitHandler>(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;
         }
 
         [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<BigCompound>(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];
         }
 
+        /// <summary>
+        /// Adds a child to the compound.
+        /// </summary>
+        /// <param name="child">Child to add to the compound.</param>
+        /// <param name="pool">Pool to use to resize the compound's children buffer if necessary.</param>
+        /// <param name="shapes">Shapes collection containing the compound's children.</param>
+        /// <remarks><para>This function keeps the <see cref="Tree"/> 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 <see cref="Tree.RefitAndRefine(BufferPool, int, float)"/> 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.</para></remarks>
+        public void Add(CompoundChild child, BufferPool pool, Shapes shapes)
+        {
+            pool.Resize(ref Children, Children.Length + 1, Children.Length);
+            Children[^1] = child;
+            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.");
+        }
+
+        /// <summary>
+        /// Removes a child from the compound by index. The last child is pulled to fill the gap left by the removed child.
+        /// </summary>
+        /// <param name="childIndex">Index of the child to remove from the compound.</param>
+        /// <param name="pool">Pool to use to resize the compound's children buffer if necessary.</param>
+        /// <remarks>This function keeps the <see cref="Tree"/> 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 <see cref="Tree.RefitAndRefine(BufferPool, int, float)"/> 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.</remarks>
+        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<TSubpairOverlaps> : IBreakableForEach<int> where TSubpairOverlaps : ICollisionTaskSubpairOverlaps
         {
             public BufferPool Pool;
@@ -179,7 +282,7 @@ public unsafe void FindLocalOverlaps<TOverlaps, TSubpairOverlaps>(ref Buffer<Ove
             {
                 ref var pair = ref pairs[i];
                 enumerator.Overlaps = Unsafe.AsPointer(ref overlaps.GetOverlapsForPair(i));
-                Unsafe.AsRef<BigCompound>(pair.Container).Tree.GetOverlaps(pair.Min, pair.Max, ref enumerator);
+                Unsafe.AsRef<BigCompound>(pair.Container).Tree.GetOverlaps(pair.Min, pair.Max, pool, ref enumerator);
             }
         }
 
@@ -193,13 +296,54 @@ public void TestLeaf(int leafIndex, ref float maximumT)
                 Unsafe.AsRef<TOverlaps>(Overlaps).Allocate(Pool) = leafIndex;
             }
         }
-        public unsafe void FindLocalOverlaps<TOverlaps>(in Vector3 min, in Vector3 max, in Vector3 sweep, float maximumT, BufferPool pool, Shapes shapes, void* overlaps)
+        public readonly unsafe void FindLocalOverlaps<TOverlaps>(Vector3 min, Vector3 max, Vector3 sweep, float maximumT, BufferPool pool, Shapes shapes, void* overlaps)
             where TOverlaps : ICollisionTaskSubpairOverlaps
         {
             SweepLeafTester<TOverlaps> enumerator;
             enumerator.Pool = pool;
             enumerator.Overlaps = overlaps;
-            Tree.Sweep(min, max, sweep, maximumT, ref enumerator);
+            Tree.Sweep(min, max, sweep, maximumT, pool, ref enumerator);
+        }
+
+        [MethodImpl(MethodImplOptions.AggressiveInlining)]
+        public readonly void FindLocalOverlaps<TEnumerator>(Vector3 min, Vector3 max, BufferPool pool, Shapes shapes, ref TEnumerator enumerator)
+            where TEnumerator : IBreakableForEach<int>
+        {
+            Tree.GetOverlaps(min, max, pool, ref enumerator);
+        }
+
+        /// <summary>
+        /// Computes the inertia of a compound. Does not recenter the child poses.
+        /// </summary>
+        /// <param name="childMasses">Masses of the children.</param>
+        /// <param name="shapes">Shapes collection containing the data for the compound child shapes.</param>
+        /// <returns>Inertia of the compound.</returns>
+        public readonly BodyInertia ComputeInertia(Span<float> childMasses, Shapes shapes)
+        {
+            return CompoundBuilder.ComputeInertia(Children, childMasses, shapes);
+        }
+
+        /// <summary>
+        /// Computes the inertia of a compound. Recenters the child poses around the calculated center of mass.
+        /// </summary>
+        /// <param name="shapes">Shapes collection containing the data for the compound child shapes.</param>
+        /// <param name="childMasses">Masses of the children.</param>
+        /// <param name="centerOfMass">Calculated center of mass of the compound. Subtracted from all the compound child poses.</param>
+        /// <returns>Inertia of the compound.</returns>
+        public readonly BodyInertia ComputeInertia(Span<float> 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.
+            //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;
         }
 
         public void Dispose(BufferPool bufferPool)
@@ -212,7 +356,7 @@ public void Dispose(BufferPool bufferPool)
         /// Type id of compound shapes.
         /// </summary>
         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/BoundingBoxBatcher.cs b/BepuPhysics/Collidables/BoundingBoxBatcher.cs
index a9b11ee69..80d5ec0df 100644
--- a/BepuPhysics/Collidables/BoundingBoxBatcher.cs
+++ b/BepuPhysics/Collidables/BoundingBoxBatcher.cs
@@ -1,14 +1,11 @@
 using BepuPhysics.Collidables;
 using BepuPhysics.CollisionDetection;
 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
 {
@@ -69,18 +66,46 @@ public static BoundsContinuation CreateCompoundChildContinuation(int compoundBod
 
     public struct BoundingBoxInstance
     {
-        public RigidPose Pose;
-        public BodyVelocity Velocities;
         public int ShapeIndex;
         public BoundsContinuation Continuation;
     }
 
-    public struct BoundingBoxInstanceWide<TShape, TShapeWide> where TShape : unmanaged, IShape where TShapeWide : unmanaged, IShapeWide<TShape>
+    public struct BoundingBoxBatch
     {
-        public TShapeWide Shape;
-        public Vector<float> MaximumExpansion;
-        public RigidPoses Pose;
-        public BodyVelocities Velocities;
+        public Buffer<int> ShapeIndices;
+        public Buffer<BoundsContinuation> Continuations;
+        public Buffer<MotionState> 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
@@ -92,7 +117,7 @@ public struct BoundingBoxBatcher
         internal float dt;
 
         int minimumBatchIndex, maximumBatchIndex;
-        Buffer<QuickList<BoundingBoxInstance>> batches;
+        Buffer<BoundingBoxBatch> batches;
 
         /// <summary>
         /// 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.
@@ -100,7 +125,7 @@ public struct BoundingBoxBatcher
         /// </summary>
         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;
@@ -116,68 +141,82 @@ public unsafe BoundingBoxBatcher(Bodies bodies, Shapes shapes, BroadPhase broadP
 
         public unsafe void ExecuteConvexBatch<TShape, TShapeWide>(ConvexShapeBatch<TShape, TShapeWide> shapeBatch) where TShape : unmanaged, IConvexShape where TShapeWide : unmanaged, IShapeWide<TShape>
         {
-            var instanceBundle = default(BoundingBoxInstanceWide<TShape, TShapeWide>);
-            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 Buffer<byte>(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 dtWide = new Vector<float>(dt);
+            Span<float> minimumMarginSpan = stackalloc float[Vector<float>.Count];
+            Span<float> maximumMarginSpan = stackalloc float[Vector<float>.Count];
+            Span<int> allowExpansionBeyondSpeculativeMarginSpan = stackalloc int[Vector<int>.Count];
             for (int bundleStartIndex = 0; bundleStartIndex < batch.Count; bundleStartIndex += Vector<float>.Count)
             {
                 int countInBundle = batch.Count - bundleStartIndex;
                 if (countInBundle > Vector<float>.Count)
                     countInBundle = Vector<float>.Count;
-                ref var bundleInstancesStart = ref Unsafe.Add(ref instancesBase, bundleStartIndex);
+
                 //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);
-                    //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]);
-                    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;
+                    var indexInBatch = bundleStartIndex + innerIndex;
+                    var shapeIndex = shapeIndices[indexInBatch];
+                    shapeWide.WriteSlot(innerIndex, shapeBatch.shapes[shapeIndex]);
+                    ref var collidable = ref activeSet.Collidables[continuations[indexInBatch].BodyIndex];
+                    minimumMarginSpan[innerIndex] = collidable.MinimumSpeculativeMargin;
+                    maximumMarginSpan[innerIndex] = collidable.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);
+
+                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<float>(minimumMarginSpan);
+                var maximumSpeculativeMargin = new Vector<float>(maximumMarginSpan);
+                var allowExpansionBeyondSpeculativeMargin = new Vector<int>(allowExpansionBeyondSpeculativeMarginSpan);
+                speculativeMargin = Vector.Max(minimumSpeculativeMargin, Vector.Min(maximumSpeculativeMargin, speculativeMargin));
+                var maximumBoundsExpansion = Vector.ConditionalSelect(allowExpansionBeyondSpeculativeMargin, new Vector<float>(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.
-                Vector3Wide.Add(instanceBundle.Pose.Position, bundleMin, out bundleMin);
-                Vector3Wide.Add(instanceBundle.Pose.Position, 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];
+                    broadPhase.GetActiveBoundsPointers(collidable.BroadPhaseIndex, out var minPointer, out var maxPointer);
+
                     //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]);
+                        collidable.SpeculativeMargin = MathF.Max(collidable.SpeculativeMargin, speculativeMargin[innerIndex]);
+                        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]);
+                        collidable.SpeculativeMargin = speculativeMargin[innerIndex];
+                        *minPointer = new Vector3(bundleMin.X[innerIndex], bundleMin.Y[innerIndex], bundleMin.Z[innerIndex]);
+                        *maxPointer = new Vector3(bundleMax.X[innerIndex], bundleMax.Y[innerIndex], bundleMax.Z[innerIndex]);
                     }
                 }
             }
@@ -185,30 +224,44 @@ public unsafe void ExecuteConvexBatch<TShape, TShapeWide>(ConvexShapeBatch<TShap
 
         public unsafe void ExecuteHomogeneousCompoundBatch<TShape, TChildShape, TChildShapeWide>(HomogeneousCompoundShapeBatch<TShape, TChildShape, TChildShapeWide> shapeBatch)
             where TShape : unmanaged, IHomogeneousCompoundShape<TChildShape, TChildShapeWide>
-            where TChildShape : IConvexShape
-            where TChildShapeWide : IShapeWide<TChildShape>
+            where TChildShape : unmanaged, IConvexShape
+            where TChildShapeWide : unmanaged, IShapeWide<TChildShape>
         {
             ref var batch = ref batches[shapeBatch.TypeId];
             ref var activeSet = ref bodies.ActiveSet;
             for (int i = 0; i < batch.Count; ++i)
             {
-                ref var instance = ref batch[i];
-                var bodyIndex = instance.Continuation.BodyIndex;
+                var shapeIndex = batch.ShapeIndices[i];
+                ref var motionState = ref batch.MotionStates[i];
+                var bodyIndex = batch.Continuations[i].BodyIndex;
                 ref var collidable = ref activeSet.Collidables[bodyIndex];
-                broadPhase.GetActiveBoundsPointers(collidable.BroadPhaseIndex, out var min, out var max);
-                var maximumAllowedExpansion = collidable.Continuity.AllowExpansionBeyondSpeculativeMargin ? float.MaxValue : collidable.SpeculativeMargin;
-                shapeBatch[instance.ShapeIndex].ComputeBounds(instance.Pose.Orientation, out *min, out *max);
+                shapeBatch[shapeIndex].ComputeBounds(motionState.Pose.Orientation, out var min, out var max);
                 //Working on the assumption that dynamic meshes are extremely rare, and that dynamic meshes with extremely high angular velocity are even rarer,
                 //we're just going to use a simplistic upper bound for angular expansion. This simplifies the mesh bounding box calculation quite a bit (no dot products).
-                var absMin = Vector3.Abs(*min);
-                var absMax = Vector3.Abs(*max);
+                var absMin = Vector3.Abs(min);
+                var absMax = Vector3.Abs(max);
                 var maximumRadius = Vector3.Max(absMin, absMax).Length();
 
                 var minimumComponents = Vector3.Min(absMin, absMax);
                 var minimumRadius = MathHelper.Min(minimumComponents.X, MathHelper.Min(minimumComponents.Y, minimumComponents.Z));
-                BoundingBoxHelpers.ExpandBoundingBox(ref *min, ref *max, instance.Velocities.Linear, instance.Velocities.Angular, dt, maximumRadius, maximumRadius - minimumRadius, maximumAllowedExpansion);
-                *min += instance.Pose.Position;
-                *max += instance.Pose.Position;
+                var maximumAngularExpansion = maximumRadius - minimumRadius;
+
+                //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(motionState.Velocity.Angular.Length(), dt, maximumRadius, maximumAngularExpansion);
+                var speculativeMargin = motionState.Velocity.Linear.Length() * dt + angularBoundsExpansion;
+                speculativeMargin = MathF.Max(collidable.MinimumSpeculativeMargin, MathF.Min(collidable.MaximumSpeculativeMargin, speculativeMargin));
+                collidable.SpeculativeMargin = speculativeMargin;
+                var maximumAllowedExpansion = collidable.Continuity.AllowExpansionBeyondSpeculativeMargin ? float.MaxValue : speculativeMargin;
+                BoundingBoxHelpers.GetBoundsExpansion(motionState.Velocity.Linear, dt, angularBoundsExpansion, out var minExpansion, out var maxExpansion);
+                var broadcastMaximumBoundsExpansion = new Vector3(maximumAllowedExpansion);
+                minExpansion = Vector3.Max(-broadcastMaximumBoundsExpansion, minExpansion);
+                maxExpansion = Vector3.Min(broadcastMaximumBoundsExpansion, maxExpansion);
+
+                broadPhase.GetActiveBoundsPointers(collidable.BroadPhaseIndex, out var minPointer, out var maxPointer);
+                *minPointer = motionState.Pose.Position + (min + minExpansion);
+                *maxPointer = motionState.Pose.Position + (max + maxExpansion);
             }
         }
 
@@ -220,27 +273,30 @@ public unsafe void ExecuteCompoundBatch<TShape>(CompoundShapeBatch<TShape> 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;
-                //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);
+                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 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[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<BoundingBoxInstance>(CollidablesPerFlush, pool);
+                batchSlot = new BoundingBoxBatch(pool, CollidablesPerFlush);
                 if (typeIndex < minimumBatchIndex)
                     minimumBatchIndex = typeIndex;
                 if (typeIndex > maximumBatchIndex)
@@ -250,16 +306,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;
             }
         }
@@ -314,10 +364,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/Collidables/Box.cs b/BepuPhysics/Collidables/Box.cs
index ffbbca4f2..f59537c73 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);
@@ -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,9 +159,10 @@ 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)
+        public static ShapeBatch CreateShapeBatch(BufferPool pool, int initialCapacity, Shapes shapeBatches)
         {
             return new ConvexShapeBatch<Box, BoxWide>(pool, initialCapacity);
         }
@@ -169,7 +171,7 @@ public readonly ShapeBatch CreateShapeBatch(BufferPool pool, int initialCapacity
         /// Type id of box shapes.
         /// </summary>
         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; } }
     }
 
 
@@ -197,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<byte> memory) { }
 
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
         public void WriteSlot(int index, in Box source)
@@ -219,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
@@ -228,7 +230,7 @@ public int MinimumWideRayCount
             }
         }
 
-        public void RayTest(ref RigidPoses pose, ref RayWide ray, out Vector<int> intersected, out Vector<float> t, out Vector3Wide normal)
+        public void RayTest(ref RigidPoseWide pose, ref RayWide ray, out Vector<int> intersected, out Vector<float> 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 c6efb2d35..b5cb17f83 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);
@@ -69,8 +69,7 @@ public readonly bool RayTest(in RigidPose pose, in Vector3 origin, in Vector3 di
 
             //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, in Vector3 origin, in Vector3 di
                     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, in Vector3 origin, in Vector3 di
             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, in Vector3 origin, in Vector3 di
                 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);
@@ -154,8 +156,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,9 +176,10 @@ 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)
+        public static ShapeBatch CreateShapeBatch(BufferPool pool, int initialCapacity, Shapes shapeBatches)
         {
             return new ConvexShapeBatch<Capsule, CapsuleWide>(pool, initialCapacity);
         }
@@ -186,7 +190,7 @@ public readonly ShapeBatch CreateShapeBatch(BufferPool pool, int initialCapacity
         /// Type id of capsule shapes.
         /// </summary>
         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<Capsule>
@@ -210,7 +214,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<byte> memory) { }
 
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
         public void WriteSlot(int index, in Capsule source)
@@ -221,7 +225,7 @@ public void WriteSlot(int index, in Capsule source)
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
         public void GetBounds(ref QuaternionWide orientations, int countInBundle, out Vector<float> maximumRadius, out Vector<float> 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);
 
@@ -234,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
@@ -243,7 +247,7 @@ public int MinimumWideRayCount
             }
         }
 
-        public void RayTest(ref RigidPoses pose, ref RayWide ray, out Vector<int> intersected, out Vector<float> t, out Vector3Wide normal)
+        public void RayTest(ref RigidPoseWide pose, ref RayWide ray, out Vector<int> intersected, out Vector<float> 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);
@@ -282,10 +286,14 @@ public void RayTest(ref RigidPoses pose, ref RayWide ray, out Vector<int> inters
             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<float> sphereY = Vector.ConditionalSelect(
-                Vector.BitwiseOr(
-                    Vector.BitwiseAnd(Vector.GreaterThan(cylinderHitLocation.Y, HalfLength), rayIsntParallel),
-                    Vector.AndNot(Vector.LessThanOrEqual(d.Y, Vector<float>.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<float>.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<float> sphereY = Vector.ConditionalSelect(rayIsntParallel, nonParallelSphereY, parallelSphereY);
 
             o.Y -= sphereY;
             Vector3Wide.Dot(o, d, out var capB);
diff --git a/BepuPhysics/Collidables/Collidable.cs b/BepuPhysics/Collidables/Collidable.cs
index 0eefc8528..1d34a7f8c 100644
--- a/BepuPhysics/Collidables/Collidable.cs
+++ b/BepuPhysics/Collidables/Collidable.cs
@@ -1,20 +1,23 @@
-using System.Runtime.CompilerServices;
+using BepuPhysics.CollisionDetection;
 using System.Runtime.InteropServices;
 
 namespace BepuPhysics.Collidables
 {
+    /// <summary>
+    /// Defines how a collidable will handle collision detection in the presence of velocity.
+    /// </summary>
     public enum ContinuousDetectionMode
     {
         /// <summary>
-        /// <para>No dedicated continuous detection is performed. Default speculative contact generation will occur within the speculative margin.</para>
+        /// <para>No sweep tests are performed. Default speculative contact generation will occur within the speculative margin.</para>
         /// <para>The collidable's bounding box will not be expanded by velocity beyond the speculative margin.</para>
-        /// <para>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.</para>
+        /// <para>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.</para>
         /// </summary>
         Discrete = 0,
         /// <summary>
-        /// <para>No dedicated continuous detection is performed. Default speculative contact generation will occur within the speculative margin.</para>
-        /// <para>The collidable's bounding box will be expanded by velocity beyond the speculative margin if necessary.</para>
+        /// <para>No sweep tests are performed. Default speculative contact generation will occur within the speculative margin.</para>
+        /// <para>The collidable's bounding box will be expanded by velocity without being limited by the speculative margin.</para>
         /// <para>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.</para>
         /// </summary>
@@ -26,50 +29,60 @@ public enum ContinuousDetectionMode
         Continuous = 2,
     }
 
-    [StructLayout(LayoutKind.Explicit)]
-    public struct ContinuousDetectionSettings
+    /// <summary>
+    /// Defines how a collidable handles collisions with significant velocity.
+    /// </summary>
+    [StructLayout(LayoutKind.Sequential)]
+    public struct ContinuousDetection
     {
         /// <summary>
         /// The continuous collision detection mode.
         /// </summary>
-        [FieldOffset(0)]
         public ContinuousDetectionMode Mode;
+
         /// <summary>
-        /// 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 <see cref="ContinuousDetectionMode.Continuous"/>, this defines the minimum progress that the sweep test will make when searching for the first time of impact.
+        /// Collisions lasting less than <see cref="MinimumSweepTimestep"/> may be missed by the sweep test. Using larger values can significantly increase the performance of sweep tests.
         /// </summary>
-        [FieldOffset(4)]
         public float MinimumSweepTimestep;
+
         /// <summary>
-        /// If using ContinuousDetectionMode.Continuous, sweep tests will terminate if the time of impact region has been refined to be smaller than SweepConvergenceThreshold.
+        /// If using <see cref="ContinuousDetectionMode.Continuous"/>, sweep tests will terminate if the time of impact region has been refined to be smaller than <see cref="SweepConvergenceThreshold"/>.
         /// 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.
         /// </summary>
-        [FieldOffset(8)]
         public float SweepConvergenceThreshold;
 
-        public bool AllowExpansionBeyondSpeculativeMargin { [MethodImpl(MethodImplOptions.AggressiveInlining)] get { return (uint)Mode > 0; } }
+        /// <summary>
+        /// Gets whether the continuous collision detection configuration will permit bounding box expansion beyond the calculated speculative margin.
+        /// </summary>
+        public bool AllowExpansionBeyondSpeculativeMargin => (uint)Mode > 0;
 
         /// <summary>
-        /// <para>No dedicated continuous detection will be performed. Default speculative contact generation will occur within the speculative margin.</para>
+        /// <para>No sweep tests are performed. Default speculative contact generation will occur within the speculative margin.</para>
         /// <para>The collidable's bounding box will not be expanded by velocity beyond the speculative margin.</para>
-        /// <para>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.</para>
+        /// <para>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.</para>
+        /// <para>Note that Discrete and Passive only differ if maximum speculative margin is restricted.</para>
         /// </summary>
-        public static ContinuousDetectionSettings Discrete
+        /// <returns>Detection settings for the given discrete configuration.</returns>
+        public static ContinuousDetection Discrete
         {
-            get { return new ContinuousDetectionSettings(); }
+            get
+            {
+                return new() { Mode = ContinuousDetectionMode.Discrete };
+            }
         }
 
         /// <summary>
-        /// <para>No dedicated continuous detection is performed. Default speculative contact generation will occur within the speculative margin.</para>
-        /// <para>The collidable's bounding box will be expanded by velocity beyond the speculative margin if necessary.</para>
-        /// <para>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.</para>
+        /// <para>No sweep tests are performed. Default speculative contact generation will occur within the speculative margin.</para>
+        /// <para>The collidable's bounding box and speculative margin will be expanded by velocity.</para>
+        /// <para>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.</para>
         /// </summary>
-        public static ContinuousDetectionSettings Passive
+        /// <returns>Detection settings for the passive configuration.</returns>
+        public static ContinuousDetection Passive
         {
-            get { return new ContinuousDetectionSettings() { Mode = ContinuousDetectionMode.Passive }; }
+            get { return new() { Mode = ContinuousDetectionMode.Passive }; }
         }
 
         /// <summary>
@@ -82,35 +95,59 @@ 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.</param>
-        /// <returns>Settings reflecting a continuous detection mode.</returns>
-        public static ContinuousDetectionSettings Continuous(float minimumSweepTimestep, float sweepConvergenceThreshold)
+        /// <returns>Detection settings for the given continuous configuration.</returns>
+        public static ContinuousDetection Continuous(float minimumSweepTimestep = 1e-3f, float sweepConvergenceThreshold = 1e-3f)
         {
-            return new ContinuousDetectionSettings { Mode = ContinuousDetectionMode.Continuous, MinimumSweepTimestep = minimumSweepTimestep, SweepConvergenceThreshold = sweepConvergenceThreshold };
+            return new()
+            {
+                Mode = ContinuousDetectionMode.Continuous,
+                MinimumSweepTimestep = minimumSweepTimestep,
+                SweepConvergenceThreshold = sweepConvergenceThreshold
+            };
         }
     }
 
     /// <summary>
-    /// 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 <see cref="TypedIndex"/>) are not capable of colliding with anything.
     /// This can be used for a body which needs no collidable representation.
     /// </summary>
     public struct Collidable
     {
+        /// <summary>
+        /// 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 <see cref="Bodies.SetShape"/> or <see cref="Bodies.ApplyDescription"/>; those functions update the relevant state.
+        /// </summary>
+        public TypedIndex Shape;
         /// <summary>
         /// Continuous collision detection settings for this collidable. Includes the collision detection mode to use and tuning variables associated with those modes.
         /// </summary>
-        public ContinuousDetectionSettings Continuity;
-
+        public ContinuousDetection Continuity;
         /// <summary>
-        /// 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.
+        /// Lower bound on the value of the speculative margin used by the collidable.
         /// </summary>
-        public TypedIndex Shape;
+        /// <remarks>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.</remarks>
+        public float MinimumSpeculativeMargin;
+        /// <summary>
+        /// Upper bound on the value of the speculative margin used by the collidable.
+        /// </summary>
+        /// <remarks><see cref="float.MaxValue"/> 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.
+        /// <para>Smaller values can be useful for improving performance in chaotic situations where missing a collision is acceptable. When using <see cref="ContinuousDetectionMode.Continuous"/>, 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.</para>
+        /// </remarks>
+        public float MaximumSpeculativeMargin;
+
         /// <summary>
-        /// 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. 
-        /// <para>For continuous collision detection with less chance of ghost collisions, use the dedicated continuous collision detection modes.</para>
+        /// 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. 
+        /// <para>This is automatically set by bounding box prediction each frame, and is bound by the collidable's <see cref="MinimumSpeculativeMargin"/> and <see cref="MaximumSpeculativeMargin"/> values.
+        /// The effective speculative margin for a collision pair can also be modified from <see cref="INarrowPhaseCallbacks"/> callbacks.</para>
+        /// <para>This should be positive to avoid jittering.</para>
+        /// <para>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 <see cref="ContinuousDetectionMode.Continuous"/>.</para>
+        /// <para>If using <see cref="ContinuousDetectionMode.Continuous"/>, consider setting <see cref="MaximumSpeculativeMargin"/> to a smaller value to help filter ghost collisions.</para>
+        /// <para>For more information, see the ContinuousCollisionDetection.md documentation.</para>
         /// </summary>
         public float SpeculativeMargin;
         /// <summary>
diff --git a/BepuPhysics/Collidables/CollidableDescription.cs b/BepuPhysics/Collidables/CollidableDescription.cs
index 06c0b8953..6302a760d 100644
--- a/BepuPhysics/Collidables/CollidableDescription.cs
+++ b/BepuPhysics/Collidables/CollidableDescription.cs
@@ -1,31 +1,117 @@
-namespace BepuPhysics.Collidables
+using System.Runtime.InteropServices;
+
+namespace BepuPhysics.Collidables
 {
+    /// <summary>
+    /// Describes a collidable and how it should handle continuous collision detection.
+    /// </summary>
+    [StructLayout(LayoutKind.Sequential)]
     public struct CollidableDescription
     {
+        /// <summary>
+        /// Shape of the collidable.
+        /// </summary>
         public TypedIndex Shape;
-        public float SpeculativeMargin;
-        public ContinuousDetectionSettings Continuity;
+        /// <summary>
+        /// Continuous collision detection settings used by the collidable.
+        /// </summary>
+        public ContinuousDetection Continuity;
+        /// <summary>
+        /// Lower bound on the value of the speculative margin used by the collidable.
+        /// </summary>
+        /// <remarks>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.</remarks>
+        public float MinimumSpeculativeMargin;
+        /// <summary>
+        /// Upper bound on the value of the speculative margin used by the collidable.
+        /// </summary>
+        /// <remarks><see cref="float.MaxValue"/> 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.
+        /// <para>Smaller values can be useful for improving performance in chaotic situations where missing a collision is acceptable. When using <see cref="ContinuousDetectionMode.Continuous"/>, 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.</para>
+        /// </remarks>
+        public float MaximumSpeculativeMargin;
 
         /// <summary>
         /// Constructs a new collidable description.
         /// </summary>
         /// <param name="shape">Shape used by the collidable.</param>
-        /// <param name="speculativeMargin">Radius of the margin in which to allow speculative contact generation.</param>
+        /// <param name="minimumSpeculativeMargin">Lower bound on the value of the speculative margin used by the collidable.</param>
+        /// <param name="maximumSpeculativeMargin">Upper bound on the value of the speculative margin used by the collidable.</param>
         /// <param name="continuity">Continuous collision detection settings for the collidable.</param>
-        public CollidableDescription(TypedIndex shape, float speculativeMargin, in ContinuousDetectionSettings continuity)
+        public CollidableDescription(TypedIndex shape, float minimumSpeculativeMargin, float maximumSpeculativeMargin, ContinuousDetection continuity)
         {
             Shape = shape;
-            SpeculativeMargin = speculativeMargin;
+            MinimumSpeculativeMargin = minimumSpeculativeMargin;
+            MaximumSpeculativeMargin = maximumSpeculativeMargin;
             Continuity = continuity;
         }
 
         /// <summary>
-        /// Constructs a new collidable description with default discrete continuity.
+        /// Constructs a new collidable description with <see cref="ContinuousDetectionMode.Discrete"/>.
         /// </summary>
         /// <param name="shape">Shape used by the collidable.</param>
-        /// <param name="speculativeMargin">Radius of the margin in which to allow speculative contact generation.</param>
-        public CollidableDescription(TypedIndex shape, float speculativeMargin) : this(shape, speculativeMargin, default)
+        /// <param name="minimumSpeculativeMargin">Lower bound on the value of the speculative margin used by the collidable.</param>
+        /// <param name="maximumSpeculativeMargin">Upper bound on the value of the speculative margin used by the collidable.</param>
+        public CollidableDescription(TypedIndex shape, float minimumSpeculativeMargin, float maximumSpeculativeMargin)
+        {
+            Shape = shape;
+            MinimumSpeculativeMargin = minimumSpeculativeMargin;
+            MaximumSpeculativeMargin = maximumSpeculativeMargin;
+            Continuity = ContinuousDetection.Discrete;
+        }
+
+        /// <summary>
+        /// Constructs a new collidable description. Uses 0 for the <see cref="MinimumSpeculativeMargin"/> .
+        /// </summary>
+        /// <param name="shape">Shape used by the collidable.</param>
+        /// <param name="maximumSpeculativeMargin">Upper bound on the value of the speculative margin used by the collidable.</param>
+        /// <param name="continuity">Continuous collision detection settings for the collidable.</param>
+        public CollidableDescription(TypedIndex shape, float maximumSpeculativeMargin, ContinuousDetection continuity)
+        {
+            Shape = shape;
+            MinimumSpeculativeMargin = 0;
+            MaximumSpeculativeMargin = maximumSpeculativeMargin;
+            Continuity = continuity;
+        }
+
+        /// <summary>
+        /// Constructs a new collidable description. Uses 0 for the <see cref="MinimumSpeculativeMargin"/> and <see cref="float.MaxValue"/> for the <see cref="MaximumSpeculativeMargin"/> .
+        /// </summary>
+        /// <param name="shape">Shape used by the collidable.</param>
+        /// <param name="continuity">Continuous collision detection settings for the collidable.</param>
+        public CollidableDescription(TypedIndex shape, ContinuousDetection continuity)
+        {
+            Shape = shape;
+            MinimumSpeculativeMargin = 0;
+            MaximumSpeculativeMargin = float.MaxValue;
+            Continuity = continuity;
+        }
+
+        /// <summary>
+        /// Constructs a new collidable description with <see cref="ContinuousDetectionMode.Passive"/>. Will use a <see cref="MinimumSpeculativeMargin"/> of 0 and a <see cref="MaximumSpeculativeMargin"/> of <see cref="float.MaxValue"/>.
+        /// </summary>
+        /// <param name="shape">Shape used by the collidable.</param>
+        /// <remarks><see cref="ContinuousDetectionMode.Passive"/> and <see cref="ContinuousDetectionMode.Discrete"/> are equivalent in behavior when the <see cref="MaximumSpeculativeMargin"/>  is <see cref="float.MaxValue"/> since they both result in the same (unbounded) expansion of body bounding boxes in response to velocity.</remarks>
+        public CollidableDescription(TypedIndex shape) : this(shape, 0, float.MaxValue, ContinuousDetection.Passive)
+        {
+        }
+
+        /// <summary>
+        /// Constructs a new collidable description with <see cref="ContinuousDetectionMode.Discrete"/>. Will use a minimum speculative margin of 0 and the given maximumSpeculativeMargin.
+        /// </summary>
+        /// <param name="shape">Shape used by the collidable.</param>
+        /// <param name="maximumSpeculativeMargin">Maximum speculative margin to be used with the discrete continuity configuration.</param>
+        public CollidableDescription(TypedIndex shape, float maximumSpeculativeMargin) : this(shape, 0, maximumSpeculativeMargin, ContinuousDetection.Discrete)
+        {
+        }
+
+        /// <summary>
+        /// Constructs a new collidable description with <see cref="ContinuousDetectionMode.Passive"/>. Will use a minimum speculative margin of 0 and a maximum of <see cref="float.MaxValue"/>.
+        /// </summary>
+        /// <param name="shapeIndex">Shape index to use for the collidable.</param>
+        public static implicit operator CollidableDescription(TypedIndex shapeIndex)
         {
+            return new CollidableDescription(shapeIndex);
         }
     }
 }
diff --git a/BepuPhysics/Collidables/CollidableReference.cs b/BepuPhysics/Collidables/CollidableReference.cs
index d9168f345..c9c329eab 100644
--- a/BepuPhysics/Collidables/CollidableReference.cs
+++ b/BepuPhysics/Collidables/CollidableReference.cs
@@ -2,9 +2,13 @@
 using System;
 using System.Diagnostics;
 using System.Runtime.CompilerServices;
+using System.Runtime.InteropServices;
 
 namespace BepuPhysics.Collidables
 {
+    /// <summary>
+    /// Represents how a collidable can interact and move.
+    /// </summary>
     public enum CollidableMobility
     {
         /// <summary>
@@ -21,8 +25,15 @@ public enum CollidableMobility
         Static = 2
     }
 
+    /// <summary>
+    /// Uses a bitpacked representation to refer to a body or static collidable.
+    /// </summary>
+    [StructLayout(LayoutKind.Sequential, Size = 4)]
     public struct CollidableReference : IEquatable<CollidableReference>
     {
+        /// <summary>
+        /// Bitpacked representation of the collidable reference.
+        /// </summary>
         public uint Packed;
 
         /// <summary>
@@ -102,7 +113,6 @@ public CollidableReference(CollidableMobility mobility, BodyHandle handle)
         /// <summary>
         /// Creates a collidable reference for a static.
         /// </summary>
-        /// <param name="mobility">Mobility type of the owner of the collidable.</param>
         /// <param name="handle">Handle of the owner of the collidable.</param>
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
         public CollidableReference(StaticHandle handle)
diff --git a/BepuPhysics/Collidables/Compound.cs b/BepuPhysics/Collidables/Compound.cs
index 3624a4b7e..dc42ec0ca 100644
--- a/BepuPhysics/Collidables/Compound.cs
+++ b/BepuPhysics/Collidables/Compound.cs
@@ -6,290 +6,404 @@
 using BepuUtilities;
 using BepuPhysics.Trees;
 using BepuPhysics.CollisionDetection.CollisionTasks;
+using System.Runtime.InteropServices;
+using System.Diagnostics.CodeAnalysis;
 
-namespace BepuPhysics.Collidables
+namespace BepuPhysics.Collidables;
+
+/// <summary>
+/// Shape and pose of a child within a compound shape.
+/// </summary>
+[StructLayout(LayoutKind.Sequential)]
+public struct CompoundChild
 {
-    public struct CompoundChild
+    /// <summary>
+    /// Local orientation of the child in the compound.
+    /// </summary>
+    public Quaternion LocalOrientation;
+    /// <summary>
+    /// Local position of the child in the compound.
+    /// </summary>
+    public Vector3 LocalPosition;
+    /// <summary>
+    /// Index of the shape within whatever shape collection holds the compound's child shape data.
+    /// </summary>
+    public TypedIndex ShapeIndex;
+
+    /// <summary>
+    /// Creates a compound child.
+    /// </summary>
+    /// <param name="pose">Pose of the compound child in the local space of the parent shape.</param>
+    /// <param name="shapeIndex">Index of the shape used by the child.</param>
+    public CompoundChild(in RigidPose pose, TypedIndex shapeIndex)
     {
-        public TypedIndex ShapeIndex;
-        public RigidPose LocalPose;
+        LocalOrientation = pose.Orientation;
+        LocalPosition = pose.Position;
+        ShapeIndex = shapeIndex;
     }
 
-    struct CompoundChildShapeTester : IShapeRayHitHandler
+    /// <summary>
+    /// Returns a reference to the memory of the <see cref="CompoundChild"/> as a <see cref="RigidPose"/>.
+    /// </summary>
+    /// <returns>Reference to this compound child as a pose.</returns>
+    [UnscopedRef]
+    public ref RigidPose AsPose()
     {
-        //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;
-        }
+        return ref Unsafe.As<CompoundChild, RigidPose>(ref this);
+    }
+}
 
-        public void OnRayHit(in RayData ray, ref float maximumT, float t, in Vector3 normal, int childIndex)
-        {
-            Debug.Assert(childIndex == 0, "Compounds can contain only convexes, so the child index is always zero.");
-            T = t;
-            Normal = normal;
-        }
+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)
+    {
+        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;
     }
+}
+
+/// <summary>
+/// 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.
+/// </summary>
+public struct Compound : ICompoundShape
+{
+    /// <summary>
+    /// Buffer of children within this compound.
+    /// </summary>
+    public Buffer<CompoundChild> Children;
 
     /// <summary>
-    /// 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.
+    /// Creates a compound shape with no acceleration structure.
     /// </summary>
-    public struct Compound : ICompoundShape
+    /// <param name="children">Set of children in the compound.</param>
+    [MethodImpl(MethodImplOptions.AggressiveInlining)]
+    public Compound(Buffer<CompoundChild> children)
     {
-        /// <summary>
-        /// Buffer of children within this compound.
-        /// </summary>
-        public Buffer<CompoundChild> Children;
+        Debug.Assert(children.Length > 0, "Compounds must have a nonzero number of children.");
+        Children = children;
+    }
 
-        /// <summary>
-        /// Creates a compound shape with no acceleration structure.
-        /// </summary>
-        /// <param name="children">Set of children in the compound.</param>
-        [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public Compound(Buffer<CompoundChild> children)
+    /// <summary>
+    /// Checks if a shape index.
+    /// </summary>
+    /// <param name="shapeIndex">Shape index to analyze.</param>
+    /// <param name="shapeBatches">Shape collection into which the index indexes.</param>
+    /// <returns>True if the index is valid, false otherwise.</returns>
+    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;
         }
-
-        /// <summary>
-        /// Checks if a shape index.
-        /// </summary>
-        /// <param name="shapeIndex">Shape index to analyze.</param>
-        /// <param name="shapeBatches">Shape collection into which the index indexes.</param>
-        /// <returns>True if the index is valid, false otherwise.</returns>
-        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;
         }
-
-        /// <summary>
-        /// Checks if a set of children shape indices are all valid.
-        /// </summary>
-        /// <param name="children">Children to examine.</param>
-        /// <param name="shapeBatches">Shape collection into which the children index.</param>
-        /// <returns>True if all child indices are valid, false otherwise.</returns>
-        public static bool ValidateChildIndices(ref Buffer<CompoundChild> 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, in Quaternion orientation, out RigidPose rotatedChildPose)
-        {
-            QuaternionEx.ConcatenateWithoutOverlap(localPose.Orientation, orientation, out rotatedChildPose.Orientation);
-            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)
-        {
-            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)
-        {
-            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)
+    /// <summary>
+    /// Checks if a set of children shape indices are all valid.
+    /// </summary>
+    /// <param name="children">Children to examine.</param>
+    /// <param name="shapeBatches">Shape collection into which the children index.</param>
+    /// <returns>True if all child indices are valid, false otherwise.</returns>
+    public static bool ValidateChildIndices(Span<CompoundChild> 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, in Quaternion orientation, Shapes shapeBatches, out Vector3 childMin, out Vector3 childMax)
-        {
-            GetRotatedChildPose(child.LocalPose, 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;
+    }
 
-        public void ComputeBounds(in Quaternion orientation, Shapes shapeBatches, out Vector3 min, out Vector3 max)
+    [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)
+    {
+        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<CompoundChild> children, ref BoundingBoxBatcher batcher, in RigidPose pose, in BodyVelocity velocity, int bodyIndex)
+    [MethodImpl(MethodImplOptions.AggressiveInlining)]
+    public static void AddChildBoundsToBatcher(Buffer<CompoundChild> 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.LocalPose, 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(Children, ref batcher, pose, velocity, bodyIndex);
+    }
 
-        public void RayTest<TRayHitHandler>(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<TRayHitHandler>(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;
+        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, pool, 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, child.LocalPose, 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<TRayHitHandler>(in RigidPose pose, ref RaySource rays, Shapes shapeBatches, ref TRayHitHandler hitHandler) where TRayHitHandler : struct, IShapeRayHitHandler
+    public unsafe void RayTest<TRayHitHandler>(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.
+        //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)
         {
-            //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);
-            }
+            rays.GetRay(i, out var ray, out var maximumT);
+            RayTest(pose, *ray, ref *maximumT, shapeBatches, pool, ref hitHandler);
         }
+    }
+
+    [MethodImpl(MethodImplOptions.AggressiveInlining)]
+    public static ShapeBatch CreateShapeBatch(BufferPool pool, int initialCapacity, Shapes shapes)
+    {
+        return new CompoundShapeBatch<Compound>(pool, initialCapacity, shapes);
+    }
 
+    public int ChildCount
+    {
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public ShapeBatch CreateShapeBatch(BufferPool pool, int initialCapacity, Shapes shapes)
-        {
-            return new CompoundShapeBatch<Compound>(pool, initialCapacity, shapes);
-        }
+        get { return Children.Length; }
+    }
 
-        public int ChildCount
-        {
-            [MethodImpl(MethodImplOptions.AggressiveInlining)]
-            get { return Children.Length; }
-        }
+    [MethodImpl(MethodImplOptions.AggressiveInlining)]
+    public ref CompoundChild GetChild(int compoundChildIndex)
+    {
+        return ref Children[compoundChildIndex];
+    }
 
-        [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public ref CompoundChild GetChild(int compoundChildIndex)
+    /// <summary>
+    /// Adds a child to the compound.
+    /// </summary>
+    /// <param name="child">Child to add to the compound.</param>
+    /// <param name="pool">Pool to use to resize the compound's children buffer if necessary.</param>
+    public void Add(CompoundChild child, BufferPool pool)
+    {
+        pool.Resize(ref Children, Children.Length + 1, Children.Length);
+        Children[^1] = child;
+    }
+
+    /// <summary>
+    /// Removes a child from the compound by index. The last child is pulled to fill the gap left by the removed child.
+    /// </summary>
+    /// <param name="childIndex">Index of the child to remove from the compound.</param>
+    /// <param name="pool">Pool to use to resize the compound's children buffer if necessary.</param>
+    public void RemoveAt(int childIndex, BufferPool pool)
+    {
+        var lastIndex = Children.Length - 1;
+        if (childIndex < lastIndex)
         {
-            return ref Children[compoundChildIndex];
+            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<TOverlaps, TSubpairOverlaps>(ref Buffer<OverlapQueryForPair> pairs, BufferPool pool, Shapes shapes, ref TOverlaps overlaps)
-            where TOverlaps : struct, ICollisionTaskOverlaps<TSubpairOverlaps>
-            where TSubpairOverlaps : struct, ICollisionTaskSubpairOverlaps
+    public unsafe void FindLocalOverlaps<TOverlaps, TSubpairOverlaps>(ref Buffer<OverlapQueryForPair> pairs, BufferPool pool, Shapes shapes, ref TOverlaps overlaps)
+        where TOverlaps : struct, ICollisionTaskOverlaps<TSubpairOverlaps>
+        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<Compound>(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<Compound>(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).
-                    shapes[child.ShapeIndex.Type].ComputeBounds(child.ShapeIndex.Index, child.LocalPose.Orientation, out _, out _, out var min, out var max);
-                    min += child.LocalPose.Position;
-                    max += child.LocalPose.Position;
-                    if (BoundingBox.Intersects(min, max, pair.Min, pair.Max))
-                    {
-                        overlapsForPair.Allocate(pool) = i;
-                    }
+                    overlapsForPair.Allocate(pool) = i;
                 }
             }
         }
+    }
 
-        public unsafe void FindLocalOverlaps<TOverlaps>(in Vector3 min, in Vector3 max, in Vector3 sweep, float maximumT, BufferPool pool, Shapes shapes, void* overlapsPointer)
-            where TOverlaps : ICollisionTaskSubpairOverlaps
+    [MethodImpl(MethodImplOptions.AggressiveInlining)]
+    public void FindLocalOverlaps<TEnumerator>(Vector3 min, Vector3 max, BufferPool pool, Shapes shapes, ref TEnumerator enumerator)
+        where TEnumerator : IBreakableForEach<int>
+    {
+        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<TOverlaps>(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 += child.LocalPosition;
+            childMax += child.LocalPosition;
+            if (BoundingBox.Intersects(childMin, childMax, min, max))
             {
-                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;
-                if (Tree.Intersects(childMin, childMax, &ray, out _))
-                {
-                    overlaps.Allocate(pool) = i;
-                }
+                if (!enumerator.LoopBody(i))
+                    return;
             }
-
         }
+    }
 
-        public void Dispose(BufferPool bufferPool)
+    public unsafe void FindLocalOverlaps<TOverlaps>(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<TOverlaps>(overlapsPointer);
+        for (int i = 0; i < Children.Length; ++i)
         {
-            bufferPool.Return(ref Children);
+            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;
+            }
         }
+    }
+
+    /// <summary>
+    /// Computes the inertia of a compound. Does not recenter the child poses.
+    /// </summary>
+    /// <param name="childMasses">Masses of the children.</param>
+    /// <param name="shapes">Shapes collection containing the data for the compound child shapes.</param>
+    /// <returns>Inertia of the compound.</returns>
+    public BodyInertia ComputeInertia(Span<float> childMasses, Shapes shapes)
+    {
+        return CompoundBuilder.ComputeInertia(Children, childMasses, shapes);
+    }
 
-        /// <summary>
-        /// Type id of list based compound shapes.
-        /// </summary>
-        public const int Id = 6;
-        public int TypeId { [MethodImpl(MethodImplOptions.AggressiveInlining)] get { return Id; } }
+    /// <summary>
+    /// Computes the inertia of a compound. Recenters the child poses around the calculated center of mass.
+    /// </summary>
+    /// <param name="shapes">Shapes collection containing the data for the compound child shapes.</param>
+    /// <param name="childMasses">Masses of the children.</param>
+    /// <param name="centerOfMass">Calculated center of mass of the compound. Subtracted from all the compound child poses.</param>
+    /// <returns>Inertia of the compound.</returns>
+    public BodyInertia ComputeInertia(Span<float> childMasses, Shapes shapes, out Vector3 centerOfMass)
+    {
+        return CompoundBuilder.ComputeInertia(Children, childMasses, shapes, out centerOfMass);
     }
 
+    public void Dispose(BufferPool bufferPool)
+    {
+        bufferPool.Return(ref Children);
+    }
 
+    /// <summary>
+    /// Type id of list based compound shapes.
+    /// </summary>
+    public const int Id = 6;
+    public static int TypeId { [MethodImpl(MethodImplOptions.AggressiveInlining)] get { return Id; } }
 }
diff --git a/BepuPhysics/Collidables/CompoundBuilder.cs b/BepuPhysics/Collidables/CompoundBuilder.cs
new file mode 100644
index 000000000..ad0268c7d
--- /dev/null
+++ b/BepuPhysics/Collidables/CompoundBuilder.cs
@@ -0,0 +1,489 @@
+using BepuUtilities;
+using BepuUtilities.Collections;
+using BepuUtilities.Memory;
+using System;
+using System.Diagnostics;
+using System.Numerics;
+using System.Runtime.CompilerServices;
+
+namespace BepuPhysics.Collidables
+{
+    /// <summary>
+    /// Reusable convenience type for incrementally building compound shapes.
+    /// </summary>
+    public struct CompoundBuilder : IDisposable
+    {
+        public BufferPool Pool;
+        public Shapes Shapes;
+
+        public struct Child
+        {
+            public RigidPose LocalPose;
+            public TypedIndex ShapeIndex;
+            /// <summary>
+            /// Weight associated with this child. Acts as the child's mass when interpreted as a dynamic compound.
+            /// When interpreted as kinematic with recentering, it is used as a local pose weight to compute the center of rotation.
+            /// </summary>
+            public float Weight;
+            /// <summary>
+            /// Inverse inertia tensor of the child in its local space.
+            /// </summary>
+            public Symmetric3x3 LocalInverseInertia;
+        }
+
+        public QuickList<Child> Children;
+
+        /// <summary>
+        /// Creates a compound builder.
+        /// </summary>
+        /// <param name="pool">Buffer pool to allocate memory from when necessary.</param>
+        /// <param name="shapes">Shapes collection to access when constructing the compound children.</param>
+        /// <param name="initialBuilderCapacity">Number of children the compound builder can hold without resizing.</param>
+        public CompoundBuilder(BufferPool pool, Shapes shapes, int initialBuilderCapacity)
+        {
+            Pool = pool;
+            Shapes = shapes;
+            Children = new QuickList<Child>(initialBuilderCapacity, Pool);
+        }
+
+        /// <summary>
+        /// Adds a new shape to the accumulator.
+        /// </summary>
+        /// <param name="shape">Index of the shape to add.</param>
+        /// <param name="localPose">Pose of the shape in the compound's local space.</param>
+        /// <param name="weight">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.</param>
+        /// <param name="localInverseInertia">Inverse inertia tensor of the shape being added in its local space. This is assumed to already be scaled as desired by the weight.</param>
+        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);
+            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.");
+        }
+
+        /// <summary>
+        /// Adds a new shape to the accumulator.
+        /// </summary>
+        /// <param name="shape">Index of the shape to add.</param>
+        /// <param name="localPose">Pose of the shape in the compound's local space.</param>
+        /// <param name="childInertia">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.</param>
+        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);
+        }
+
+        /// <summary>
+        /// Adds a new shape to the accumulator, assuming it has infinite inertia.
+        /// </summary>
+        /// <param name="shape">Index of the shape to add.</param>
+        /// <param name="localPose">Pose of the shape in the compound's local space.</param>
+        /// <param name="weight">Weight of the shape used for computing the center of rotation.</param>
+        public void AddForKinematic(TypedIndex shape, in RigidPose localPose, float weight)
+        {
+            Debug.Assert(Compound.ValidateChildIndex(shape, Shapes));
+            ref var child = ref Children.Allocate(Pool);
+            child.LocalPose = localPose;
+            child.ShapeIndex = shape;
+            child.Weight = weight;
+            child.LocalInverseInertia = default;
+        }
+
+        /// <summary>
+        /// 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.
+        /// </summary>
+        /// <typeparam name="TShape">Type of the shape to add to the accumulator and the shapes set.</typeparam>
+        /// <param name="shape">Shape to add.</param>
+        /// <param name="localPose">Pose of the shape in the compound's local space.</param>
+        /// <param name="weight">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.</param>
+        public void Add<TShape>(in TShape shape, in RigidPose localPose, float weight) where TShape : unmanaged, IConvexShape
+        {
+            Add(Shapes.Add(shape), localPose, shape.ComputeInertia(weight).InverseInertiaTensor, weight);
+        }
+
+        /// <summary>
+        /// Adds a new shape to the accumulator, creating a new shape in the shapes set. Inertia is assumed to be infinite.
+        /// </summary>
+        /// <typeparam name="TShape">Type of the shape to add to the accumulator and the shapes set.</typeparam>
+        /// <param name="shape">Shape to add.</param>
+        /// <param name="localPose">Pose of the shape in the compound's local space.</param>
+        /// <param name="weight">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.</param>
+        public void AddForKinematic<TShape>(in TShape shape, in RigidPose localPose, float weight) where TShape : unmanaged, IConvexShape
+        {
+            AddForKinematic(Shapes.Add(shape), localPose, weight);
+        }
+
+
+        /// <summary>
+        /// Gets the contribution to an inertia tensor of a point mass at the given offset from the center of mass.
+        /// </summary>
+        /// <param name="offset">Offset from the center of mass.</param>
+        /// <param name="mass">Mass of the point.</param>
+        /// <param name="contribution">Contribution to the inertia tensor.</param>
+        [MethodImpl(MethodImplOptions.AggressiveInlining)]
+        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);
+            contribution.YX = -mass * (offset.Y * offset.X);
+            contribution.YY = mass * (innerProduct - offset.Y * offset.Y);
+            contribution.ZX = -mass * (offset.Z * offset.X);
+            contribution.ZY = -mass * (offset.Z * offset.Y);
+            contribution.ZZ = mass * (innerProduct - offset.Z * offset.Z);
+        }
+
+        /// <summary>
+        /// Builds a buffer of compound children from the accumulated set for a dynamic compound.
+        /// Computes a center of mass and recenters child shapes relative to it. Does not reset the accumulator.
+        /// </summary>
+        /// <param name="children">List of children created from the accumulated set.</param>
+        /// <param name="inertia">Combined inertia of the compound.</param>
+        /// <param name="center">Computed center of rotation based on the poses and weights of accumulated children.</param>
+        public void BuildDynamicCompound(out Buffer<CompoundChild> children, out BodyInertia inertia, out Vector3 center)
+        {
+            center = new Vector3();
+            float totalWeight = 0;
+            for (int i = 0; i < Children.Count; ++i)
+            {
+                center += Children[i].LocalPose.Position * Children[i].Weight;
+                totalWeight += Children[i].Weight;
+            }
+            Debug.Assert(totalWeight > 0, "The compound as a whole must have nonzero weight when using a recentering build. The center is undefined.");
+
+            inertia.InverseMass = 1f / totalWeight;
+            center *= inertia.InverseMass;
+            Pool.Take(Children.Count, out children);
+            Symmetric3x3 summedInertia = default;
+            for (int i = 0; i < Children.Count; ++i)
+            {
+                ref var sourceChild = ref Children[i];
+                ref var targetChild = ref children[i];
+                targetChild.LocalPosition = sourceChild.LocalPose.Position - center;
+                targetChild.LocalOrientation = sourceChild.LocalPose.Orientation;
+                targetChild.ShapeIndex = sourceChild.ShapeIndex;
+                Symmetric3x3.Add(ComputeInertiaForChild(targetChild.LocalPosition, targetChild.LocalOrientation, sourceChild.LocalInverseInertia, sourceChild.Weight), summedInertia, out summedInertia);
+            }
+            Symmetric3x3.Invert(summedInertia, out inertia.InverseInertiaTensor);
+        }
+
+        /// <summary>
+        /// Builds a buffer of compound children from the accumulated set for a dynamic compound. Does not recenter the children. Does not reset the accumulator.
+        /// </summary>
+        /// <param name="children">List of children created from the accumulated set.</param>
+        /// <param name="inertia">Combined inertia of the compound.</param>
+        public void BuildDynamicCompound(out Buffer<CompoundChild> children, out BodyInertia inertia)
+        {
+            float totalWeight = 0;
+            for (int i = 0; i < Children.Count; ++i)
+            {
+                totalWeight += Children[i].Weight;
+            }
+            Debug.Assert(totalWeight > 0, "The compound as a whole must have nonzero weight when creating a dynamic compound.");
+
+            inertia.InverseMass = 1f / totalWeight;
+            Pool.Take(Children.Count, out children);
+            Symmetric3x3 summedInertia = default;
+            for (int i = 0; i < Children.Count; ++i)
+            {
+                ref var sourceChild = ref Children[i];
+                ref var targetChild = ref children[i];
+                targetChild.LocalPosition = sourceChild.LocalPose.Position;
+                targetChild.LocalOrientation = sourceChild.LocalPose.Orientation;
+                targetChild.ShapeIndex = sourceChild.ShapeIndex;
+                Symmetric3x3.Add(ComputeInertiaForChild(sourceChild.LocalPose.Position, sourceChild.LocalPose.Orientation, sourceChild.LocalInverseInertia, sourceChild.Weight), summedInertia, out summedInertia);
+            }
+            Symmetric3x3.Invert(summedInertia, out inertia.InverseInertiaTensor);
+        }
+
+        /// <summary>
+        /// Computes the uninverted inertia contribution of a child.
+        /// </summary>
+        /// <param name="pose">Pose of the child.</param>
+        /// <param name="inverseLocalInertia">Inverse inertia tensor of the child in its local space.</param>
+        /// <param name="mass">Mass of the child.</param>
+        /// <returns>Inertia contribution of the child to a compound given its relative pose.</returns>
+        [MethodImpl(MethodImplOptions.AggressiveInlining)]
+        public static Symmetric3x3 ComputeInertiaForChild(in RigidPose pose, Symmetric3x3 inverseLocalInertia, float mass)
+        {
+            return ComputeInertiaForChild(pose.Position, pose.Orientation, inverseLocalInertia, mass);
+        }
+        /// <summary>
+        /// Computes the uninverted inertia contribution of a child.
+        /// </summary>
+        /// <param name="position">Position of the child.</param>
+        /// <param name="orientation">Orientation of the child.</param>
+        /// <param name="inverseLocalInertia">Inverse inertia tensor of the child in its local space.</param>
+        /// <param name="mass">Mass of the child.</param>
+        /// <returns>Inertia contribution of the child to a compound given its relative pose.</returns>
+        [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, orientation, out var rotatedInverseInertia);
+            Symmetric3x3.Invert(rotatedInverseInertia, out var inertia);
+            Symmetric3x3.Add(offsetContribution, inertia, out inertia);
+            return inertia;
+        }
+
+        /// <summary>
+        /// Computes the inertia for a set of compound children based on their poses and the provided inverse inertias. Does not recenter the children.
+        /// </summary>
+        /// <param name="children">Children and their associated poses.</param>
+        /// <param name="inverseLocalInertias">Inverse inertias of the children, each in the child's local space. Assumed to have already been premultiplied by the mass of the child.</param>
+        /// <param name="childMasses">Masses of each child in the compound.</param>
+        /// <returns><see cref="BodyInertia"/> of the compound.</returns>
+        public static BodyInertia ComputeInverseInertia(Span<CompoundChild> children, Span<Symmetric3x3> inverseLocalInertias, Span<float> childMasses)
+        {
+            Symmetric3x3 summedInertia = default;
+            float massSum = 0;
+            for (int i = 0; i < children.Length; ++i)
+            {
+                ref var child = ref children[i];
+                summedInertia += ComputeInertiaForChild(child.LocalPosition, child.LocalOrientation, inverseLocalInertias[i], childMasses[i]);
+                massSum += childMasses[i];
+            }
+            BodyInertia inertia;
+            Symmetric3x3.Invert(summedInertia, out inertia.InverseInertiaTensor);
+            inertia.InverseMass = 1f / massSum;
+            return inertia;
+        }
+
+        /// <summary>
+        /// Computes the inverse inertia for a set of compound children based on their poses and the provided inverse inertias. Does not recenter the children.
+        /// </summary>
+        /// <param name="childPoses">Poses of the compound's children.</param>
+        /// <param name="inverseLocalInertias">Inverse inertias of the children, each in the child's local space. Assumed to have already been premultiplied by the mass of the child.</param>
+        /// <param name="childMasses">Masses of each child in the compound.</param>
+        /// <returns><see cref="BodyInertia"/> of the compound.</returns>
+        public static BodyInertia ComputeInverseInertia(Span<RigidPose> childPoses, Span<Symmetric3x3> inverseLocalInertias, Span<float> 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;
+        }
+        /// <summary>
+        /// Computes the center of mass of a compound.
+        /// </summary>
+        /// <param name="children">Children of the compound.</param>
+        /// <param name="childMasses">Masses of the children in the compound.</param>
+        /// <param name="inverseMass">Inverse of the sum of all child masses.</param>
+        /// <returns>The compound's center of mass.</returns>
+        public static Vector3 ComputeCenterOfMass(Span<CompoundChild> children, Span<float> childMasses, out float inverseMass)
+        {
+            Vector3 sum = default;
+            float massSum = 0;
+            for (int i = 0; i < children.Length; ++i)
+            {
+                sum += childMasses[i] * children[i].LocalPosition;
+                massSum += childMasses[i];
+            }
+            inverseMass = 1f / massSum;
+            return sum * inverseMass;
+        }
+        /// <summary>
+        /// Computes the center of mass of a compound.
+        /// </summary>
+        /// <param name="childPoses">Poses of the children in the compound.</param>
+        /// <param name="childMasses">Masses of the children in the compound.</param>
+        /// <param name="inverseMass">Inverse of the sum of all child masses.</param>
+        /// <returns>The compound's center of mass.</returns>
+        public static Vector3 ComputeCenterOfMass(Span<RigidPose> childPoses, Span<float> 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;
+        }
+        /// <summary>
+        /// Computes the center of mass of a compound.
+        /// </summary>
+        /// <param name="children">Children of the compound.</param>
+        /// <param name="childMasses">Masses of the children in the compound.</param>
+        /// <returns>The compound's center of mass.</returns>
+        public static Vector3 ComputeCenterOfMass(Span<CompoundChild> children, Span<float> childMasses) => ComputeCenterOfMass(children, childMasses, out _);
+
+        /// <summary>
+        /// Computes the center of mass of a compound.
+        /// </summary>
+        /// <param name="childPoses">Poses of the children in the compound.</param>
+        /// <param name="childMasses">Masses of the children in the compound.</param>
+        /// <returns>The compound's center of mass.</returns>
+        public static Vector3 ComputeCenterOfMass(Span<RigidPose> childPoses, Span<float> childMasses) => ComputeCenterOfMass(childPoses, childMasses, out _);
+
+        /// <summary>
+        /// 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.
+        /// </summary>
+        /// <param name="children">Children and their associated poses. Center of mass will be subtracted from the child position.</param>
+        /// <param name="inverseLocalInertias">Inverse inertias of the children, each in the child's local space. Assumed to have already been premultiplied by the mass of the child.</param>
+        /// <param name="childMasses">Masses of each child in the compound.</param>
+        /// <param name="centerOfMass">Computed center of mass that was subtracted from the child positions.</param>
+        /// <returns><see cref="BodyInertia"/> of the compound.</returns>
+        public static BodyInertia ComputeInverseInertia(Span<CompoundChild> children, Span<Symmetric3x3> inverseLocalInertias, Span<float> childMasses, out Vector3 centerOfMass)
+        {
+            Symmetric3x3 summedInertia = default;
+            BodyInertia inertia;
+            centerOfMass = ComputeCenterOfMass(children, childMasses, out inertia.InverseMass);
+            for (int i = 0; i < children.Length; ++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;
+        }
+        /// <summary>
+        /// 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.
+        /// </summary>
+        /// <param name="childPoses">Poses of the compound's children. Center of mass will be subtracted from the child position.</param>
+        /// <param name="inverseLocalInertias">Inverse inertias of the children, each in the child's local space. Assumed to have already been premultiplied by the mass of the child.</param>
+        /// <param name="childMasses">Masses of each child in the compound.</param>
+        /// <param name="centerOfMass">Computed center of mass that was subtracted from the child positions.</param>
+        /// <returns><see cref="BodyInertia"/> of the compound.</returns>
+        public static BodyInertia ComputeInverseInertia(Span<RigidPose> childPoses, Span<Symmetric3x3> inverseLocalInertias, Span<float> 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;
+        }
+
+        /// <summary>
+        /// Computes the inertia of a compound. Does not recenter the child poses.
+        /// </summary>
+        /// <param name="children">Children of the compound.</param>
+        /// <param name="shapes">Shapes collection containing the data for the compound child shapes.</param>
+        /// <param name="childMasses">Masses of the children.</param>
+        /// <returns>Inertia of the compound.</returns>
+        public static BodyInertia ComputeInertia(Span<CompoundChild> children, Span<float> childMasses, Shapes shapes)
+        {
+            Span<Symmetric3x3> 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);
+        }
+
+        /// <summary>
+        /// Computes the inertia of a compound. Recenters the child poses around the calculated center of mass.
+        /// </summary>
+        /// <param name="children">Children of the compound. Child local positions will have the calculated center of mass subtracted from them.</param>
+        /// <param name="shapes">Shapes collection containing the data for the compound child shapes.</param>
+        /// <param name="childMasses">Masses of the children.</param>
+        /// <param name="centerOfMass">Calculated center of mass of the compound. Subtracted from all the compound child poses.</param>
+        /// <returns>Inertia of the compound.</returns>
+        public static BodyInertia ComputeInertia(Span<CompoundChild> children, Span<float> childMasses, Shapes shapes, out Vector3 centerOfMass)
+        {
+            Span<Symmetric3x3> 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);
+        }
+
+        /// <summary>
+        /// 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.
+        /// </summary>
+        /// <param name="children">List of children created from the accumulated set.</param>
+        /// <param name="center">Computed center of rotation based on the poses and weights of accumulated children.</param>
+        public void BuildKinematicCompound(out Buffer<CompoundChild> children, out Vector3 center)
+        {
+            center = new Vector3();
+            float totalWeight = 0;
+            for (int i = 0; i < Children.Count; ++i)
+            {
+                center += Children[i].LocalPose.Position * Children[i].Weight;
+                totalWeight += Children[i].Weight;
+            }
+            Debug.Assert(totalWeight > 0, "The compound as a whole must have nonzero weight when using a recentering build. The center is undefined.");
+
+            var inverseWeight = 1f / totalWeight;
+            center *= inverseWeight;
+            Pool.Take(Children.Count, out children);
+            for (int i = 0; i < Children.Count; ++i)
+            {
+                ref var sourceChild = ref Children[i];
+                ref var targetChild = ref children[i];
+                targetChild.LocalPosition = sourceChild.LocalPose.Position - center;
+                targetChild.LocalOrientation = sourceChild.LocalPose.Orientation;
+                targetChild.ShapeIndex = sourceChild.ShapeIndex;
+            }
+        }
+
+        /// <summary>
+        /// Builds a buffer of compound children from the accumulated set for a kinematic compound. Does not recenter children. Does not reset the accumulator.
+        /// </summary>
+        /// <param name="children">List of children created from the accumulated set.</param>
+        public void BuildKinematicCompound(out Buffer<CompoundChild> children)
+        {
+            Pool.Take(Children.Count, out children);
+            for (int i = 0; i < Children.Count; ++i)
+            {
+                ref var sourceChild = ref Children[i];
+                ref var targetChild = ref children[i];
+                targetChild.LocalPosition = sourceChild.LocalPose.Position;
+                targetChild.LocalOrientation = sourceChild.LocalPose.Orientation;
+                targetChild.ShapeIndex = sourceChild.ShapeIndex;
+            }
+        }
+
+        /// <summary>
+        /// Empties out the accumulated children.
+        /// </summary>
+        public void Reset()
+        {
+            Children.Count = 0;
+        }
+
+        /// <summary>
+        /// Returns internal resources to the pool, rendering the builder unusable.
+        /// </summary>
+        public void Dispose()
+        {
+            Children.Dispose(Pool);
+        }
+    }
+}
diff --git a/BepuPhysics/Collidables/CompoundHelpers.cs b/BepuPhysics/Collidables/CompoundHelpers.cs
deleted file mode 100644
index d300b86c7..000000000
--- a/BepuPhysics/Collidables/CompoundHelpers.cs
+++ /dev/null
@@ -1,262 +0,0 @@
-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.Collidables
-{
-    /// <summary>
-    /// Reusable convenience type for incrementally building compound shapes.
-    /// </summary>
-    public struct CompoundBuilder : IDisposable
-    {
-        public BufferPool Pool;
-        public Shapes Shapes;
-
-        public struct Child
-        {
-            public RigidPose LocalPose;
-            public TypedIndex ShapeIndex;
-            /// <summary>
-            /// Weight associated with this child. Acts as the child's mass when interpreted as a dynamic compound.
-            /// When interpreted as kinematic with recentering, it is used as a local pose weight to compute the center of rotation.
-            /// </summary>
-            public float Weight;
-            /// <summary>
-            /// Inertia tensor associated with the child. If inertia is all zeroes, it is interpreted as infinite.
-            /// </summary>
-            public Symmetric3x3 Inertia;
-        }
-
-        public QuickList<Child> Children;
-
-        public CompoundBuilder(BufferPool pool, Shapes shapes, int builderCapacity)
-        {
-            Pool = pool;
-            Shapes = shapes;
-            Children = new QuickList<Child>(builderCapacity, Pool);
-        }
-
-        /// <summary>
-        /// 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.
-        /// </summary>
-        /// <typeparam name="TShape">Type of the shape to add to the accumulator and the shapes set.</typeparam>
-        /// <param name="shape">Shape to add.</param>
-        /// <param name="localPose">Pose of the shape in the compound's local space.</param>
-        /// <param name="weight">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.</param>
-        public void Add<TShape>(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;
-            shape.ComputeInertia(weight, out var inertia);
-            Symmetric3x3.Invert(inertia.InverseInertiaTensor, out child.Inertia);
-        }
-
-        /// <summary>
-        /// Adds a new shape to the accumulator, creating a new shape in the shapes set. Inertia is assumed to be infinite.
-        /// </summary>
-        /// <typeparam name="TShape">Type of the shape to add to the accumulator and the shapes set.</typeparam>
-        /// <param name="shape">Shape to add.</param>
-        /// <param name="localPose">Pose of the shape in the compound's local space.</param>
-        /// <param name="weight">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.</param>
-        public void AddForKinematic<TShape>(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;
-            child.Inertia = default;
-        }
-
-        /// <summary>
-        /// Adds a new shape to the accumulator.
-        /// </summary>
-        /// <param name="shape">Index of the shape to add.</param>
-        /// <param name="localPose">Pose of the shape in the compound's local space.</param>
-        /// <param name="weight">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.</param>
-        /// <param name="inverseInertia">Inverse inertia tensor of the shape being added. This is assumed to already be scaled as desired by the weight.</param>
-        public void Add(TypedIndex shape, in RigidPose localPose, in Symmetric3x3 inverseInertia, float weight)
-        {
-            Debug.Assert(Compound.ValidateChildIndex(shape, Shapes));
-            ref var child = ref Children.Allocate(Pool);
-            child.LocalPose = localPose;
-            child.ShapeIndex = shape;
-            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. " +
-                "If making a kinematic, consider using the overload which takes no inverse inertia.");
-            Symmetric3x3.Invert(inverseInertia, out child.Inertia);
-        }
-
-        /// <summary>
-        /// Adds a new shape to the accumulator, assuming it has infinite inertia.
-        /// </summary>
-        /// <param name="shape">Index of the shape to add.</param>
-        /// <param name="localPose">Pose of the shape in the compound's local space.</param>
-        /// <param name="weight">Weight of the shape used for computing the center of rotation.</param>
-        public void AddForKinematic(TypedIndex shape, in RigidPose localPose, float weight)
-        {
-            Debug.Assert(Compound.ValidateChildIndex(shape, Shapes));
-            ref var child = ref Children.Allocate(Pool);
-            child.LocalPose = localPose;
-            child.ShapeIndex = shape;
-            child.Weight = weight;
-            child.Inertia = default;
-        }
-
-        [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public static void GetOffsetInertiaContribution(in Vector3 offset, float mass, out Symmetric3x3 contribution)
-        {
-            var innerProduct = Vector3.Dot(offset, offset);
-            contribution.XX = mass * (innerProduct - offset.X * offset.X);
-            contribution.YX = -mass * (offset.Y * offset.X);
-            contribution.YY = mass * (innerProduct - offset.Y * offset.Y);
-            contribution.ZX = -mass * (offset.Z * offset.X);
-            contribution.ZY = -mass * (offset.Z * offset.Y);
-            contribution.ZZ = mass * (innerProduct - offset.Z * offset.Z);
-        }
-
-        /// <summary>
-        /// Builds a buffer of compound children from the accumulated set for a dynamic compound.
-        /// Computes a center of mass and recenters child shapes relative to it. Does not reset the accumulator.
-        /// </summary>
-        /// <param name="children">List of children created from the accumulated set.</param>
-        /// <param name="inertia">Combined inertia of the compound.</param>
-        /// <param name="center">Computed center of rotation based on the poses and weights of accumulated children.</param>
-        public void BuildDynamicCompound(out Buffer<CompoundChild> children, out BodyInertia inertia, out Vector3 center)
-        {
-            center = new Vector3();
-            float totalWeight = 0;
-            for (int i = 0; i < Children.Count; ++i)
-            {
-                center += Children[i].LocalPose.Position * Children[i].Weight;
-                totalWeight += Children[i].Weight;
-            }
-            Debug.Assert(totalWeight > 0, "The compound as a whole must have nonzero weight when using a recentering build. The center is undefined.");
-
-            inertia.InverseMass = 1f / totalWeight;
-            center *= inertia.InverseMass;
-            Pool.Take(Children.Count, out children);
-            Symmetric3x3 summedInertia = default;
-            for (int i = 0; i < Children.Count; ++i)
-            {
-                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.Invert(summedInertia, out inertia.InverseInertiaTensor);
-        }
-
-        /// <summary>
-        /// Builds a buffer of compound children from the accumulated set for a dynamic compound. Does not recenter the children. Does not reset the accumulator.
-        /// </summary>
-        /// <param name="children">List of children created from the accumulated set.</param>
-        /// <param name="inertia">Combined inertia of the compound.</param>
-        public void BuildDynamicCompound(out Buffer<CompoundChild> children, out BodyInertia inertia)
-        {
-            float totalWeight = 0;
-            for (int i = 0; i < Children.Count; ++i)
-            {
-                totalWeight += Children[i].Weight;
-            }
-            Debug.Assert(totalWeight > 0, "The compound as a whole must have nonzero weight when creating a dynamic compound.");
-
-            inertia.InverseMass = 1f / totalWeight;
-            Pool.Take(Children.Count, out children);
-            Symmetric3x3 summedInertia = default;
-            for (int i = 0; i < Children.Count; ++i)
-            {
-                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.ShapeIndex = sourceChild.ShapeIndex;
-            }
-            Symmetric3x3.Invert(summedInertia, out inertia.InverseInertiaTensor);
-        }
-
-        /// <summary>
-        /// 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.
-        /// </summary>
-        /// <param name="children">List of children created from the accumulated set.</param>
-        /// <param name="inertia">Combined inertia of the compound.</param>
-        /// <param name="center">Computed center of rotation based on the poses and weights of accumulated children.</param>
-        public void BuildKinematicCompound(out Buffer<CompoundChild> children, out Vector3 center)
-        {
-            center = new Vector3();
-            float totalWeight = 0;
-            for (int i = 0; i < Children.Count; ++i)
-            {
-                center += Children[i].LocalPose.Position * Children[i].Weight;
-                totalWeight += Children[i].Weight;
-            }
-            Debug.Assert(totalWeight > 0, "The compound as a whole must have nonzero weight when using a recentering build. The center is undefined.");
-
-            var inverseWeight = 1f / totalWeight;
-            center *= inverseWeight;
-            Pool.Take(Children.Count, out children);
-            for (int i = 0; i < Children.Count; ++i)
-            {
-                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.ShapeIndex = sourceChild.ShapeIndex;
-            }
-        }
-
-        /// <summary>
-        /// Builds a buffer of compound children from the accumulated set for a kinematic compound. Does not recenter children. Does not reset the accumulator.
-        /// </summary>
-        /// <param name="children">List of children created from the accumulated set.</param>
-        /// <param name="inertia">Combined inertia of the compound.</param>
-        /// <param name="center">Computed center of rotation based on the poses and weights of accumulated children.</param>
-        public void BuildKinematicCompound(out Buffer<CompoundChild> children)
-        {
-            Pool.Take(Children.Count, out children);
-            for (int i = 0; i < Children.Count; ++i)
-            {
-                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.ShapeIndex = sourceChild.ShapeIndex;
-            }
-        }
-
-        /// <summary>
-        /// Empties out the accumulated children.
-        /// </summary>
-        public void Reset()
-        {
-            Children.Count = 0;
-        }
-
-        /// <summary>
-        /// Returns internal resources to the pool, rendering the builder unusable.
-        /// </summary>
-        public void Dispose()
-        {
-            Children.Dispose(Pool);
-        }
-    }
-}
diff --git a/BepuPhysics/Collidables/ConvexHull.cs b/BepuPhysics/Collidables/ConvexHull.cs
index 74178b415..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
     {
         /// <summary>
         /// Bundled points of the convex hull.
@@ -60,7 +60,8 @@ public struct ConvexHull : IConvexShape
         /// <param name="center">Computed center of the convex hull before the hull was recentered.</param>
         public ConvexHull(Span<Vector3> 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)]
@@ -137,7 +138,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);
@@ -183,26 +184,30 @@ public bool GetNextTriangle(out Vector3 a, out Vector3 b, out Vector3 c)
             }
         }
 
-        /// <summary>
-        /// Computes the inertia of the convex hull.
-        /// </summary>
-        /// <param name="mass">Mass to scale the inertia tensor with.</param>
-        /// <param name="inertia">Inertia of the convex hull.</param>
-        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);
+            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);
+            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);
         }
 
-        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)
         {
+            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);
@@ -285,7 +290,7 @@ public void Dispose(BufferPool bufferPool)
         /// Type id of convex hull shapes.
         /// </summary>
         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<ConvexHull>
@@ -294,12 +299,12 @@ public struct ConvexHullWide : IShapeWide<ConvexHull>
         //The "wide" variant is simply a collection of convex hull instances.
         public Buffer<ConvexHull> 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<float>.Count * Unsafe.SizeOf<ConvexHull>();
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public void Initialize(in RawBuffer memory)
+        public void Initialize(in Buffer<byte> memory)
         {
             Debug.Assert(memory.Length == InternalAllocationSize);
             Hulls = memory.As<ConvexHull>();
@@ -358,7 +363,7 @@ public void GetBounds(ref QuaternionWide orientations, int countInBundle, out Ve
             maximumAngularExpansion = maximumRadius;
         }
 
-        public void RayTest(ref RigidPoses poses, ref RayWide rayWide, out Vector<int> intersected, out Vector<float> t, out Vector3Wide normal)
+        public void RayTest(ref RigidPoseWide poses, ref RayWide rayWide, out Vector<int> intersected, out Vector<float> t, out Vector3Wide normal)
         {
             Unsafe.SkipInit(out intersected);
             Unsafe.SkipInit(out t);
@@ -366,7 +371,7 @@ public void RayTest(ref RigidPoses poses, ref RayWide rayWide, out Vector<int> i
             Debug.Assert(Hulls.Length > 0 && Hulls.Length <= Vector<float>.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);
@@ -406,6 +411,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/Collidables/ConvexHullHelper.cs b/BepuPhysics/Collidables/ConvexHullHelper.cs
index 29d54acda..0acc332c7 100644
--- a/BepuPhysics/Collidables/ConvexHullHelper.cs
+++ b/BepuPhysics/Collidables/ConvexHullHelper.cs
@@ -1,955 +1,1238 @@
-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.Collidables
-{
-    /// <summary>
-    /// Stores references to the points composing one of a convex hull's faces.
-    /// </summary>
-    public struct HullFace
-    {
-        public Buffer<int> OriginalVertexMapping;
-        public Buffer<int> VertexIndices;
-
-        /// <summary>
-        /// Gets the number of vertices in the face.
-        /// </summary>
-        public int VertexCount
-        {
-            [MethodImpl(MethodImplOptions.AggressiveInlining)]
-            get { return VertexIndices.Length; }
-        }
-
-        /// <summary>
-        /// Gets the index of the vertex associated with the given face vertex index in the source point set.
-        /// </summary>
-        /// <param name="index">Index into the face's vertex list.</param>
-        /// <returns>Index of the vertex associated with the given face vertex index in the source point set.</returns>
-        public int this[int index]
-        {
-            [MethodImpl(MethodImplOptions.AggressiveInlining)]
-            get { return OriginalVertexMapping[VertexIndices[index]]; }
-        }
-    }
-
-    /// <summary>
-    /// Raw data representing a convex hull.
-    /// </summary>
-    /// <remarks>This is not yet transformed into a runtime format. It requires additional processing to be used in a ConvexHull shape; see ConvexHullHelper.ProcessHull.</remarks>
-    public struct HullData
-    {
-        /// <summary>
-        /// Mapping of points on the convex hull back to the original point set.
-        /// </summary>
-        public Buffer<int> OriginalVertexMapping;
-        /// <summary>
-        /// List of indices composing the faces of the hull. Individual faces indexed by the FaceIndices.
-        /// </summary>
-        public Buffer<int> FaceVertexIndices;
-        /// <summary>
-        /// Starting index in the FaceVertexIndices for each face.
-        /// </summary>
-        public Buffer<int> 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);
-            pool.Return(ref FaceVertexIndices);
-            pool.Return(ref FaceStartIndices);
-        }
-    }
-
-    /// <summary>
-    /// Helper methods to create and process convex hulls from point clouds.
-    /// </summary>
-    public static class ConvexHullHelper
-    {
-        static void FindExtremeFace(in Vector3Wide basisX, in Vector3Wide basisY, in Vector3Wide basisOrigin, in EdgeEndpoints sourceEdgeEndpoints, ref Buffer<Vector3Wide> pointBundles, in Vector<int> indexOffsets, int pointCount,
-            ref Buffer<Vector<float>> projectedOnX, ref Buffer<Vector<float>> projectedOnY, in Vector<float> planeEpsilon, ref QuickList<int> vertexIndices, out Vector3 faceNormal)
-        {
-            Debug.Assert(projectedOnX.Length >= pointBundles.Length && projectedOnY.Length >= pointBundles.Length && vertexIndices.Count == 0 && vertexIndices.Span.Length >= pointBundles.Length * Vector<float>.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.
-            x = Vector.Max(Vector<float>.Zero, x);
-            Vector3Wide.Dot(basisY, toCandidate, out y);
-            var bestY = y;
-            var bestX = x;
-            //Ignore the source edge.
-            var edgeIndexA = new Vector<int>(sourceEdgeEndpoints.A);
-            var edgeIndexB = new Vector<int>(sourceEdgeEndpoints.B);
-            var pointCountBundle = new Vector<int>(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)));
-            bestX = Vector.ConditionalSelect(ignoreSlot, Vector<float>.One, bestX);
-            bestY = Vector.ConditionalSelect(ignoreSlot, new Vector<float>(float.MinValue), bestY);
-            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<float>.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<int>(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)));
-                var useCandidate = Vector.AndNot(Vector.GreaterThan(y * bestX, bestY * x), ignoreSlot);
-                bestY = Vector.ConditionalSelect(useCandidate, y, bestY);
-                bestX = Vector.ConditionalSelect(useCandidate, x, bestX);
-            }
-            var bestYNarrow = bestY[0];
-            var bestXNarrow = bestX[0];
-            for (int i = 1; i < Vector<float>.Count; ++i)
-            {
-                var candidateNumerator = bestY[i];
-                var candidateDenominator = bestX[i];
-                if (candidateNumerator * bestXNarrow > bestYNarrow * candidateDenominator)
-                {
-                    bestYNarrow = candidateNumerator;
-                    bestXNarrow = candidateDenominator;
-                }
-            }
-            //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.
-            var projectedPlaneNormalNarrow = Vector2.Normalize(new Vector2(-bestYNarrow, bestXNarrow));
-            Vector2Wide.Broadcast(projectedPlaneNormalNarrow, out var projectedPlaneNormal);
-            for (int i = 0; i < pointBundles.Length; ++i)
-            {
-                var dot = projectedOnX[i] * projectedPlaneNormal.X + projectedOnY[i] * projectedPlaneNormal.Y;
-                var coplanar = Vector.LessThanOrEqual(Vector.Abs(dot), planeEpsilon);
-                if (Vector.LessThanAny(coplanar, Vector<int>.Zero))
-                {
-                    var bundleBaseIndex = i << BundleIndexing.VectorShift;
-                    var localIndexMaximum = pointCount - bundleBaseIndex;
-                    if (localIndexMaximum > Vector<int>.Count)
-                        localIndexMaximum = Vector<int>.Count;
-                    for (int j = 0; j < localIndexMaximum; ++j)
-                    {
-                        if (coplanar[j] < 0)
-                        {
-                            vertexIndices.AllocateUnsafely() = bundleBaseIndex + j;
-                        }
-                    }
-                }
-            }
-            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<Vector2> 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<int> faceVertexIndices, in Vector3 faceNormal, Span<Vector3> points, float planeEpsilon, ref QuickList<Vector2> facePoints, ref Buffer<bool> allowVertex, ref QuickList<int> 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]])
-                    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]] = false;
-                            reducedIndices.FastRemoveAt(2);
-                        }
-                        else if (ac.LengthSquared() > 1e-14f)
-                        {
-                            allowVertex[reducedIndices[1]] = false;
-                            reducedIndices.FastRemoveAt(1);
-                        }
-                        else
-                        {
-                            allowVertex[reducedIndices[1]] = false;
-                            allowVertex[reducedIndices[2]] = false;
-                            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]] = false;
-                }
-                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] = false;
-                }
-            }
-        }
-
-        [StructLayout(LayoutKind.Explicit)]
-        public struct EdgeEndpoints : IEqualityComparerRef<EdgeEndpoints>
-        {
-            [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<int, long>(ref a.A) == Unsafe.As<int, long>(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})";
-            }
-        }
-        struct EdgeToTest
-        {
-            public EdgeEndpoints Endpoints;
-            public Vector3 FaceNormal;
-            public int FaceIndex;
-        }
-
-        //public struct DebugStep
-        //{
-        //    public EdgeEndpoints SourceEdge;
-        //    public List<int> Raw;
-        //    public List<int> Reduced;
-        //    public bool[] AllowVertex;
-        //    public Vector3 FaceNormal;
-        //    public Vector3 BasisX;
-        //    public Vector3 BasisY;
-
-        //    public DebugStep(EdgeEndpoints sourceEdge, ref QuickList<int> raw, in Vector3 faceNormal, in Vector3 basisX, in Vector3 basisY)
-        //    {
-        //        SourceEdge = sourceEdge;
-        //        FaceNormal = faceNormal;
-        //        BasisX = basisX;
-        //        BasisY = basisY;
-        //        Raw = new List<int>();
-        //        for (int i = 0; i < raw.Count; ++i)
-        //        {
-        //            Raw.Add(raw[i]);
-        //        }
-        //        Reduced = default;
-        //        AllowVertex = default;
-        //    }
-
-        //    public void AddReduced(ref QuickList<int> reduced, ref Buffer<bool> allowVertex)
-        //    {
-        //        Reduced = new List<int>();
-        //        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];
-        //        }
-        //    }
-
-
-        //}
-        ///// <summary>
-        ///// Computes the convex hull of a set of points.
-        ///// </summary>
-        ///// <param name="points">Point set to compute the convex hull of.</param>
-        ///// <param name="pool">Buffer pool to pull memory from when creating the hull.</param>
-        ///// <param name="hullData">Convex hull of the input point set.</param>
-        //public static void ComputeHull(Span<Vector3> points, BufferPool pool, out HullData hullData)
-        //{
-        //    ComputeHull(points, pool, out hullData, out _);
-        //}
-        /// <summary>
-        /// Computes the convex hull of a set of points.
-        /// </summary>
-        /// <param name="points">Point set to compute the convex hull of.</param>
-        /// <param name="pool">Buffer pool to pull memory from when creating the hull.</param>
-        /// <param name="hullData">Convex hull of the input point set.</param>
-        public static void ComputeHull(Span<Vector3> points, BufferPool pool, out HullData hullData)
-        {
-            if (points.Length <= 0)
-            {
-                hullData = default;
-                //steps = new List<DebugStep>();
-                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;
-                }
-                //steps = new List<DebugStep>();
-                return;
-            }
-            var pointBundleCount = BundleIndexing.GetBundleCount(points.Length);
-            pool.Take<Vector3Wide>(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<float>.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<int>(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<int>.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;
-                //steps = new List<DebugStep>();
-                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<Vector<float>>(pointBundles.Length, out var projectedOnX);
-            pool.Take<Vector<float>>(pointBundles.Length, out var projectedOnY);
-            var planeEpsilonNarrow = MathF.Sqrt(bestDistanceSquared) * 1e-6f;
-            var planeEpsilon = new Vector<float>(planeEpsilonNarrow);
-            var rawFaceVertexIndices = new QuickList<int>(pointBundles.Length * Vector<float>.Count, pool);
-            var initialSourceEdge = new EdgeEndpoints { A = initialIndex, B = initialIndex };
-            FindExtremeFace(initialBasisX, initialBasisY, initialVertexBundle, initialSourceEdge, ref pointBundles, indexOffsetBundle, points.Length,
-               ref projectedOnX, ref projectedOnY, planeEpsilon, ref rawFaceVertexIndices, out var initialFaceNormal);
-            Debug.Assert(rawFaceVertexIndices.Count >= 2);
-            var facePoints = new QuickList<Vector2>(points.Length, pool);
-            var reducedFaceIndices = new QuickList<int>(points.Length, pool);
-            //Points found to not be on the face hull are ignored by future executions. 
-            pool.Take<bool>(points.Length, out var allowVertex);
-            for (int i = 0; i < points.Length; ++i)
-                allowVertex[i] = true;
-
-            Vector3Wide.ReadFirst(initialBasisX, out var debugInitialBasisX);
-            Vector3Wide.ReadFirst(initialBasisY, out var debugInitialBasisY);
-            //steps = new List<DebugStep>();
-            //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);
-
-            var earlyFaceIndices = new QuickList<int>(points.Length, pool);
-            var earlyFaceStartIndices = new QuickList<int>(points.Length, pool);
-
-            var edgesToTest = new QuickList<EdgeToTest>(points.Length, pool);
-            var edgeFaceCounts = new QuickDictionary<EdgeEndpoints, int, EdgeEndpoints>(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.
-                //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;
-                    edgeFaceCounts.AddRef(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;
-                earlyFaceIndices.AddRange(reducedFaceIndices.Span, 0, reducedFaceIndices.Count, pool);
-            }
-            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);
-            }
-
-            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)
-                    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, 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;
-                ReduceFace(ref rawFaceVertexIndices, faceNormal, points, planeEpsilonNarrow, ref facePoints, ref allowVertex, ref reducedFaceIndices);
-                if (reducedFaceIndices.Count < 3)
-                {
-                    //Degenerate face found; don't bother creating work for it.
-                    continue;
-                }
-                //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);
-                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 (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." + 
-                        //    "This tends to happen when there are many near-coplanar vertices, so numerical tolerances across different faces cannot consistently agree.");
-                        ++edgeFaceCount;
-                    }
-                    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;
-                        //Use the encoding- all indices are offset by 1 since 0 represents 'empty'.
-                        edgeFaceCounts.Table[tableIndex] = ++edgeFaceCounts.Count;
-                        edgesToTest.Allocate(pool) = nextEdgeToTest;
-                    }
-                }
-            }
-
-            edgesToTest.Dispose(pool);
-            edgeFaceCounts.Dispose(pool);
-            facePoints.Dispose(pool);
-            reducedFaceIndices.Dispose(pool);
-            rawFaceVertexIndices.Dispose(pool);
-            pool.Return(ref allowVertex);
-            pool.Return(ref projectedOnX);
-            pool.Return(ref projectedOnY);
-            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);
-            pool.Take<int>(points.Length, out var originalToHullIndexMapping);
-            var hullToOriginalIndexMapping = new QuickList<int>(points.Length, pool);
-            for (int i = 0; i < points.Length; ++i)
-            {
-                originalToHullIndexMapping[i] = -1;
-            }
-            for (int i = 0; i < earlyFaceStartIndices.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 originalVertexIndex = earlyFaceIndices[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[j] = originalToHull;
-                }
-            }
-
-            pool.Take(hullToOriginalIndexMapping.Count, out hullData.OriginalVertexMapping);
-            hullToOriginalIndexMapping.Span.CopyTo(0, hullData.OriginalVertexMapping, 0, hullToOriginalIndexMapping.Count);
-
-            pool.Return(ref originalToHullIndexMapping);
-            hullToOriginalIndexMapping.Dispose(pool);
-            earlyFaceIndices.Dispose(pool);
-            earlyFaceStartIndices.Dispose(pool);
-        }
-
-        /// <summary>
-        /// Processes hull data into a runtime usable convex hull shape. Recenters the convex hull's points around its center of mass.
-        /// </summary>
-        /// <param name="points">Point array into which the hull data indexes.</param>
-        /// <param name="hullData">Raw input data to process.</param>
-        /// <param name="pool">Pool used to allocate resources for the hullShape.</param>
-        /// <param name="hullShape">Convex hull shape created from the input data.</param>
-        /// <param name="center">Computed center of mass of the convex hull before its points were recentered onto the origin.</param>
-        public static void CreateShape(Span<Vector3> points, HullData hullData, BufferPool pool, out Vector3 center, out ConvexHull hullShape)
-        {
-            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);
-
-            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;
-                    center += (a + b + c) * volumeContribution;
-                }
-            }
-            //Division by 4 since we accumulated (a + b + c), rather than the actual tetrahedral center (a + b + c + 0) / 4.
-            center /= volume * 4;
-
-            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<float>.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<float>.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;
-            }
-        }
-
-        /// <summary>
-        /// Creates a convex hull shape out of an input point set. Recenters the convex hull's points around its center of mass.
-        /// </summary>
-        /// <param name="points">Points to use to create the hull.</param>
-        /// <param name="pool">Buffer pool used for temporary allocations and the output data structures.</param>
-        /// <param name="hullData">Intermediate hull data that got processed into the convex hull.</param>
-        /// <param name="center">Computed center of mass of the convex hull before its points were recentered onto the origin.</param>
-        /// <param name="convexHull">Convex hull shape of the input point set.</param>
-        public static void CreateShape(Span<Vector3> 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);
-        }
-
-        /// <summary>
-        /// Creates a convex hull shape out of an input point set. Recenters the convex hull's points around its center of mass.
-        /// </summary>
-        /// <param name="points">Points to use to create the hull.</param>
-        /// <param name="pool">Buffer pool used for temporary allocations and the output data structures.</param>
-        /// <param name="center">Computed center of mass of the convex hull before its points were recentered onto the origin.</param>
-        /// <param name="convexHull">Convex hull shape of the input point set.</param>
-        public static void CreateShape(Span<Vector3> 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.
-            if (hullData.OriginalVertexMapping.Allocated)
-                hullData.Dispose(pool);
-        }
-
-
-        /// <summary>
-        /// Creates a transformed copy of a convex hull.
-        /// </summary>
-        /// <param name="source">Source convex hull to copy.</param>
-        /// <param name="transform">Transform to apply to the hull points.</param>
-        /// <param name="targetPoints">Transformed points in the copy target hull.</param>
-        /// <param name="targetBoundingPlanes">Transformed bounding planes in the copy target hull.</param>
-        public static void CreateTransformedCopy(in ConvexHull source, in Matrix3x3 transform, Buffer<Vector3Wide> targetPoints, Buffer<HullBoundingPlanes> 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<float>.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;
-            }
-        }
-
-        /// <summary>
-        /// 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.
-        /// </summary>
-        /// <param name="source">Source convex hull to copy.</param>
-        /// <param name="transform">Transform to apply to the hull points.</param>
-        /// <param name="pool">Pool from which to allocate the new hull's points and bounding planes buffers.</param>
-        /// <param name="target">Target convex hull to copy into. FaceVertexIndices and FaceToVertexIndicesStart buffers are reused from the source.</param>
-        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;
-        }
-
-        /// <summary>
-        /// Creates a transformed copy of a convex hull. Unique FaceVertexIndices and FaceToVertexIndicesStart buffers are allocated for the copy target.
-        /// </summary>
-        /// <param name="source">Source convex hull to copy.</param>
-        /// <param name="transform">Transform to apply to the hull points.</param>
-        /// <param name="pool">Pool from which to allocate the new hull's buffers.</param>
-        /// <param name="target">Target convex hull to copy into.</param>
-        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);
-        }
-
-    }
-}
+//#define DEBUG_STEPS
+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
+{
+    /// <summary>
+    /// Stores references to the points composing one of a convex hull's faces.
+    /// </summary>
+    public struct HullFace
+    {
+        public Buffer<int> OriginalVertexMapping;
+        public Buffer<int> VertexIndices;
+
+        /// <summary>
+        /// Gets the number of vertices in the face.
+        /// </summary>
+        public int VertexCount
+        {
+            [MethodImpl(MethodImplOptions.AggressiveInlining)]
+            get { return VertexIndices.Length; }
+        }
+
+        /// <summary>
+        /// Gets the index of the vertex associated with the given face vertex index in the source point set.
+        /// </summary>
+        /// <param name="index">Index into the face's vertex list.</param>
+        /// <returns>Index of the vertex associated with the given face vertex index in the source point set.</returns>
+        public int this[int index]
+        {
+            [MethodImpl(MethodImplOptions.AggressiveInlining)]
+            get { return OriginalVertexMapping[VertexIndices[index]]; }
+        }
+    }
+
+    /// <summary>
+    /// Raw data representing a convex hull.
+    /// </summary>
+    /// <remarks>This is not yet transformed into a runtime format. It requires additional processing to be used in a ConvexHull shape; see ConvexHullHelper.ProcessHull.</remarks>
+    public struct HullData
+    {
+        /// <summary>
+        /// Mapping of points on the convex hull back to the original point set.
+        /// </summary>
+        public Buffer<int> OriginalVertexMapping;
+        /// <summary>
+        /// List of indices composing the faces of the hull. Individual faces indexed by the FaceIndices.
+        /// </summary>
+        public Buffer<int> FaceVertexIndices;
+        /// <summary>
+        /// Starting index in the FaceVertexIndices for each face.
+        /// </summary>
+        public Buffer<int> 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);
+        }
+    }
+
+    /// <summary>
+    /// Helper methods to create and process convex hulls from point clouds.
+    /// </summary>
+    public static class ConvexHullHelper
+    {
+        static void FindExtremeFace(
+            in Vector3Wide basisX, in Vector3Wide basisY, in Vector3Wide basisOrigin, in EdgeEndpoints sourceEdgeEndpoints, ref Buffer<Vector3Wide> pointBundles, in Vector<int> indexOffsets, Buffer<int> allowVertices, int pointCount,
+            ref Buffer<Vector<float>> projectedOnX, ref Buffer<Vector<float>> projectedOnY, Vector<float> planeEpsilon, ref QuickList<int> vertexIndices, out Vector3 faceNormal)
+        {
+            Debug.Assert(projectedOnX.Length >= pointBundles.Length && projectedOnY.Length >= pointBundles.Length && vertexIndices.Count == 0 && vertexIndices.Span.Length >= pointBundles.Length * Vector<float>.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<float>.Zero, x);
+            Vector3Wide.Dot(basisY, toCandidate, out y);
+            var bestY = y;
+            var bestX = x;
+            //Ignore the source edge.
+            var edgeIndexA = new Vector<int>(sourceEdgeEndpoints.A);
+            var edgeIndexB = new Vector<int>(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<Vector<int>>();
+            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<float>.One, bestX);
+            bestY = Vector.ConditionalSelect(ignoreSlot, new Vector<float>(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<float>.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<int>(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<float>.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);
+            var negatedPlaneEpsilon = -planeEpsilon;
+            for (int i = 0; i < pointBundles.Length; ++i)
+            {
+                var dot = projectedOnX[i] * projectedPlaneNormal.X + projectedOnY[i] * projectedPlaneNormal.Y;
+                var coplanar = Vector.GreaterThan(dot, negatedPlaneEpsilon);
+                if (Vector.LessThanAny(coplanar, Vector<int>.Zero))
+                {
+                    var bundleBaseIndex = i << BundleIndexing.VectorShift;
+                    var localIndexMaximum = pointCount - bundleBaseIndex;
+                    if (localIndexMaximum > Vector<int>.Count)
+                        localIndexMaximum = Vector<int>.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;
+        }
+
+        /// <summary>
+        /// Finds the next index in the 2D hull of a face on the 3D hull using gift wrapping.
+        /// </summary>
+        /// <param name="start">Start location of the next edge to identify.</param>
+        /// <param name="previousEdgeDirection">2D direction of the previously identified edge.</param>
+        /// <param name="planeEpsilon">Epsilon within which to consider points to be coplanar (or here, in the 2D case, collinear).</param>
+        /// <param name="facePoints">Points composing the hull face projected onto the face's 2D basis.</param>
+        /// <returns>Index of the point in facePoints which is the end point for the next edge segment as identified by gift wrapping.</returns>
+        static int FindNextIndexForFaceHull(Vector2 start, Vector2 previousEdgeDirection, float planeEpsilon, ref QuickList<Vector2> 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;
+            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;
+                if (useCandidate)
+                {
+                    bestY = y;
+                    bestX = x;
+                    bestIndex = i;
+                }
+            }
+            //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.
+            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);
+                if (alongNormal > -planeEpsilon)
+                {
+                    var alongEdge = Vector2.Dot(toCandidate, edgeDirection);
+                    if (alongEdge > distance)
+                    {
+                        distance = alongEdge;
+                        mostDistantIndex = i;
+                    }
+                }
+            }
+            return mostDistantIndex == -1 ? bestIndex : mostDistantIndex;
+
+
+        }
+
+        static void ReduceFace(ref QuickList<int> faceVertexIndices, Vector3 faceNormal, Span<Vector3> points, float planeEpsilon, ref QuickList<Vector2> facePoints, ref Buffer<int> allowVertex, ref QuickList<int> 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;
+            for (int i = 0; i < facePoints.Count; ++i)
+            {
+                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]))
+                {
+                    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;
+            }
+
+            //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<EdgeEndpoints>
+        {
+            [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<int, long>(ref a.A) == Unsafe.As<int, long>(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})";
+            }
+        }
+
+        internal struct EarlyFace
+        {
+            public QuickList<int> VertexIndices;
+            public Vector3 Normal;
+        }
+
+        struct EdgeToTest
+        {
+            public EdgeEndpoints Endpoints;
+            public Vector3 FaceNormal;
+        }
+
+
+        static void AddFace(ref QuickList<EarlyFace> faces, BufferPool pool, Vector3 normal, QuickList<int> vertexIndices)
+        {
+            ref var face = ref faces.Allocate(pool);
+            face = new EarlyFace { Normal = normal, VertexIndices = new QuickList<int>(vertexIndices.Count, pool) };
+            face.VertexIndices.AddRangeUnsafely(vertexIndices);
+        }
+
+        static void AddFaceEdgesToTestList(BufferPool pool,
+            ref QuickList<int> reducedFaceIndices,
+            ref QuickList<EdgeToTest> edgesToTest,
+            ref QuickDictionary<EdgeEndpoints, int, EdgeEndpoints> edgeFaceCounts,
+            Vector3 faceNormal, int newFaceIndex)
+        {
+            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 (!edgeFaceCounts.FindOrAllocateSlot(ref endpoints, pool, out var slotIndex))
+                {
+                    EdgeToTest nextEdgeToTest;
+                    nextEdgeToTest.Endpoints = endpoints;
+                    nextEdgeToTest.FaceNormal = faceNormal;
+                    edgesToTest.Allocate(pool) = nextEdgeToTest;
+                    edgeFaceCounts.Values[slotIndex] = 1;
+                }
+                else
+                {
+                    //No need to test this edge; it's already been submitted by a different face.
+                    edgeFaceCounts.Values[slotIndex]++;
+                }
+            }
+        }
+
+#if DEBUG_STEPS
+        public struct DebugStep
+        {
+            public EdgeEndpoints SourceEdge;
+            public int[] Raw;
+            public int[] Reduced;
+            public int[] OverwrittenOriginal;
+            public List<int[]> DeletedFaces;
+            public bool[] AllowVertex;
+            public Vector3 FaceNormal;
+            public Vector3 BasisX;
+            public Vector3 BasisY;
+            public List<int> FaceStarts;
+            public List<int> FaceIndices;
+            public int FaceIndex;
+            public Vector3[] FaceNormals;
+
+
+            internal DebugStep(EdgeEndpoints sourceEdge, QuickList<int> rawVertexIndices, Vector3 faceNormal, Vector3 basisX, Vector3 basisY, QuickList<int> reducedVertexIndices, int faceIndex)
+            {
+                SourceEdge = sourceEdge;
+                FaceNormal = faceNormal;
+                BasisX = basisX;
+                BasisY = basisY;
+                Raw = ((Span<int>)rawVertexIndices).ToArray();
+                Reduced = ((Span<int>)reducedVertexIndices).ToArray();
+                OverwrittenOriginal = null;
+                FaceIndex = faceIndex;
+                DeletedFaces = new List<int[]>();
+            }
+
+            internal DebugStep FillHistory(Buffer<int> allowVertex, QuickList<EarlyFace> faces)
+            {
+                FaceStarts = new List<int>(faces.Count);
+                FaceIndices = new List<int>();
+                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]);
+                    FaceNormals[i] = face.Normal;
+                }
+                AllowVertex = new bool[allowVertex.Length];
+                for (int i = 0; i < allowVertex.Length; ++i)
+                {
+                    AllowVertex[i] = allowVertex[i] != 0;
+                }
+                return this;
+            }
+
+            /// <summary>
+            /// 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.
+            /// </summary>
+            /// <param name="faceVertexIndices">Face vertex indices of the original face that's being overwritten.</param>
+            internal void RecordOverwrittenFace(QuickList<int> faceVertexIndices)
+            {
+                OverwrittenOriginal = ((Span<int>)faceVertexIndices).ToArray();
+            }
+
+            /// <summary>
+            /// Records the vertex indices corresponding to a face that was deleted for being associated with now-disallowed vertices downstream of a face merge.
+            /// </summary>
+            /// <param name="faceVertexIndices">Vertices of the face that was deleted.</param>
+            internal void RecordDeletedFace(QuickList<int> faceVertexIndices)
+            {
+                DeletedFaces.Add(((Span<int>)faceVertexIndices).ToArray());
+            }
+
+            internal void UpdateForFaceMerge(QuickList<int> rawFaceVertexIndices, QuickList<int> reducedVertexIndices, Buffer<int> allowVertex, int mergedFaceIndex)
+            {
+                Raw = ((Span<int>)rawFaceVertexIndices).ToArray();
+                Reduced = ((Span<int>)reducedVertexIndices).ToArray();
+                FaceIndex = mergedFaceIndex;
+            }
+        }
+        /// <summary>
+        /// Computes the convex hull of a set of points.
+        /// </summary>
+        /// <param name="points">Point set to compute the convex hull of.</param>
+        /// <param name="pool">Buffer pool to pull memory from when creating the hull.</param>
+        /// <param name="hullData">Convex hull of the input point set.</param>
+        public static void ComputeHull(Span<Vector3> points, BufferPool pool, out HullData hullData)
+        {
+            ComputeHull(points, pool, out hullData, out _);
+        }
+#endif
+
+        /// <summary>
+        /// Computes the convex hull of a set of points.
+        /// </summary>
+        /// <param name="points">Point set to compute the convex hull of.</param>
+        /// <param name="pool">Buffer pool to pull memory from when creating the hull.</param>
+        /// <param name="hullData">Convex hull of the input point set.</param>
+#if DEBUG_STEPS
+        public static void ComputeHull(Span<Vector3> points, BufferPool pool, out HullData hullData, out List<DebugStep> steps)
+#else
+        public static void ComputeHull(Span<Vector3> points, BufferPool pool, out HullData hullData)
+#endif
+        {
+#if DEBUG_STEPS
+            steps = new List<DebugStep>();
+#endif
+            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<Vector3Wide>(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<float>.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<int>(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<int>.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<Vector<float>>(pointBundles.Length, out var projectedOnX);
+            pool.Take<Vector<float>>(pointBundles.Length, out var projectedOnY);
+            // 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-4f;
+            var normalCoplanarityEpsilon = 1f - 1e-6f;
+            var planeSlabEpsilon = new Vector<float>(planeSlabEpsilonNarrow);
+            var rawFaceVertexIndices = new QuickList<int>(pointBundles.Length * Vector<float>.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<int>(pointBundleCount * Vector<int>.Count, out var allowVertices);
+            ((Span<int>)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, planeSlabEpsilon, ref rawFaceVertexIndices, out var initialFaceNormal);
+            Debug.Assert(rawFaceVertexIndices.Count >= 2);
+            var facePoints = new QuickList<Vector2>(points.Length, pool);
+            var reducedFaceIndices = new QuickList<int>(points.Length, pool);
+
+
+            ReduceFace(ref rawFaceVertexIndices, initialFaceNormal, points, planeSlabEpsilonNarrow, ref facePoints, ref allowVertices, ref reducedFaceIndices);
+
+            var faces = new QuickList<EarlyFace>(points.Length, pool);
+            var edgesToTest = new QuickList<EdgeToTest>(points.Length, pool);
+            var edgeFaceCounts = new QuickDictionary<EdgeEndpoints, int, EdgeEndpoints>(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.
+                //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;
+                }
+                //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;
+                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);
+            }
+#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)
+            {
+                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];
+                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, planeSlabEpsilon, ref rawFaceVertexIndices, out var faceNormal);
+                reducedFaceIndices.Count = 0;
+                facePoints.Count = 0;
+                ReduceFace(ref rawFaceVertexIndices, faceNormal, points, planeSlabEpsilonNarrow, ref facePoints, ref allowVertices, ref reducedFaceIndices);
+
+                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)
+                {
+                    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;
+                        reducedFaceIndices.Span.CopyTo(0, rawFaceVertexIndices.Span, 0, reducedFaceIndices.Count);
+                        for (int j = 0; j < face.VertexIndices.Count; ++j)
+                        {
+                            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))
+                            {
+                                rawFaceVertexIndices.AllocateUnsafely() = vertexIndex;
+                            }
+                        }
+                        // 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, planeSlabEpsilonNarrow, 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.
+                        break;
+                    }
+                }
+                if (!mergedFace)
+                {
+                    var faceCountPriorToAdd = faces.Count;
+                    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)
+                    {
+#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)
+                        {
+                            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;
+#if DEBUG_STEPS
+                step.FillHistory(allowVertices, faces);
+                steps.Add(step);
+                if (steps.Count > 500)
+                    break;
+#endif
+            }
+
+            edgesToTest.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}");
+            //        }
+            //    }
+            //}
+
+            //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<int>(points.Length, out var originalToHullIndexMapping);
+            var hullToOriginalIndexMapping = new QuickList<int>(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);
+        }
+
+
+        /// <summary>
+        /// Processes hull data into a runtime usable convex hull shape. Recenters the convex hull's points around its center of mass.
+        /// </summary>
+        /// <param name="points">Point array into which the hull data indexes.</param>
+        /// <param name="hullData">Raw input data to process.</param>
+        /// <param name="pool">Pool used to allocate resources for the hullShape.</param>
+        /// <param name="hullShape">Convex hull shape created from the input data.</param>
+        /// <param name="center">Computed center of mass of the convex hull before its points were recentered onto the origin.</param>
+        /// <returns>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.</returns>
+        public static bool CreateShape(Span<Vector3> 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<float>.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<float>.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;
+        }
+
+        /// <summary>
+        /// Creates a convex hull shape out of an input point set. Recenters the convex hull's points around its center of mass.
+        /// </summary>
+        /// <param name="points">Points to use to create the hull.</param>
+        /// <param name="pool">Buffer pool used for temporary allocations and the output data structures.</param>
+        /// <param name="hullData">Intermediate hull data that got processed into the convex hull.</param>
+        /// <param name="center">Computed center of mass of the convex hull before its points were recentered onto the origin.</param>
+        /// <param name="convexHull">Convex hull shape of the input point set.</param>
+        /// <returns>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.</returns>
+        public static bool CreateShape(Span<Vector3> 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);
+        }
+
+        /// <summary>
+        /// Creates a convex hull shape out of an input point set. Recenters the convex hull's points around its center of mass.
+        /// </summary>
+        /// <param name="points">Points to use to create the hull.</param>
+        /// <param name="pool">Buffer pool used for temporary allocations and the output data structures.</param>
+        /// <param name="center">Computed center of mass of the convex hull before its points were recentered onto the origin.</param>
+        /// <param name="convexHull">Convex hull shape of the input point set.</param>
+        /// <returns>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.</returns>
+        public static bool CreateShape(Span<Vector3> 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;
+        }
+
+
+        /// <summary>
+        /// Creates a transformed copy of a convex hull.
+        /// </summary>
+        /// <param name="source">Source convex hull to copy.</param>
+        /// <param name="transform">Transform to apply to the hull points.</param>
+        /// <param name="targetPoints">Transformed points in the copy target hull.</param>
+        /// <param name="targetBoundingPlanes">Transformed bounding planes in the copy target hull.</param>
+        public static void CreateTransformedCopy(in ConvexHull source, in Matrix3x3 transform, Buffer<Vector3Wide> targetPoints, Buffer<HullBoundingPlanes> 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<float>.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;
+            }
+        }
+
+        /// <summary>
+        /// 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.
+        /// </summary>
+        /// <param name="source">Source convex hull to copy.</param>
+        /// <param name="transform">Transform to apply to the hull points.</param>
+        /// <param name="pool">Pool from which to allocate the new hull's points and bounding planes buffers.</param>
+        /// <param name="target">Target convex hull to copy into. FaceVertexIndices and FaceToVertexIndicesStart buffers are reused from the source.</param>
+        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;
+        }
+
+        /// <summary>
+        /// Creates a transformed copy of a convex hull. Unique FaceVertexIndices and FaceToVertexIndicesStart buffers are allocated for the copy target.
+        /// </summary>
+        /// <param name="source">Source convex hull to copy.</param>
+        /// <param name="transform">Transform to apply to the hull points.</param>
+        /// <param name="pool">Pool from which to allocate the new hull's buffers.</param>
+        /// <param name="target">Target convex hull to copy into.</param>
+        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);
+        }
+
+    }
+}
diff --git a/BepuPhysics/Collidables/Cylinder.cs b/BepuPhysics/Collidables/Cylinder.cs
index d94b189c1..0eadc9c06 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);
@@ -86,9 +86,7 @@ public readonly bool RayTest(in RigidPose pose, in Vector3 origin, in Vector3 di
             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, in Vector3 origin, in Vector3 di
                     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)
                 {
@@ -139,14 +136,15 @@ public readonly bool RayTest(in RigidPose pose, in Vector3 origin, in Vector3 di
             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).
             //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;
@@ -165,8 +163,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,9 +174,10 @@ 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)
+        public static ShapeBatch CreateShapeBatch(BufferPool pool, int initialCapacity, Shapes shapeBatches)
         {
             return new ConvexShapeBatch<Cylinder, CylinderWide>(pool, initialCapacity);
         }
@@ -186,7 +186,7 @@ public readonly ShapeBatch CreateShapeBatch(BufferPool pool, int initialCapacity
         /// Type id of cylinder shapes.
         /// </summary>
         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<Cylinder>
@@ -210,7 +210,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<byte> memory) { }
 
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
         public void WriteSlot(int index, in Cylinder source)
@@ -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<float> maximumRadius, out Vector<float> 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<float>.One, yy, out var squared);
             max.X = Vector.Abs(HalfLength * y.X) + Vector.SquareRoot(Vector.Max(Vector<float>.Zero, squared.X)) * Radius;
@@ -234,7 +234,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
@@ -243,7 +243,7 @@ public int MinimumWideRayCount
             }
         }
 
-        public void RayTest(ref RigidPoses pose, ref RayWide ray, out Vector<int> intersected, out Vector<float> t, out Vector3Wide normal)
+        public void RayTest(ref RigidPoseWide pose, ref RayWide ray, out Vector<int> intersected, out Vector<float> 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);
@@ -289,15 +289,15 @@ public void RayTest(ref RigidPoses pose, ref RayWide ray, out Vector<int> inters
 
             //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<float>.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<float>.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<float>.Zero))) * inverseDLength;
             var capUsesUpwardFacingNormal = Vector.LessThan(d.Y, Vector<float>.Zero);
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
+	{
+		/// <summary>
+		/// Returns all resources used by the shape instance to the given pool.
+		/// </summary>
+		/// <param name="pool">Pool to return shape resources to.</param>
+		void Dispose(BufferPool pool);
+	}
+}
diff --git a/BepuPhysics/Collidables/IShape.cs b/BepuPhysics/Collidables/IShape.cs
index 2f26b26a3..7bcb5a183 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
@@ -14,8 +11,19 @@ namespace BepuPhysics.Collidables
     /// </summary>
     public interface IShape
     {
-        int TypeId { get; }
-        ShapeBatch CreateShapeBatch(BufferPool pool, int initialCapacity, Shapes shapeBatches);
+        /// <summary>
+        /// Unique type id for this shape type.
+        /// </summary>
+        static abstract int TypeId { get; }
+        /// <summary>
+        /// Creates a shape batch for this type of shape.
+        /// </summary>
+        /// <param name="pool">Buffer pool used to create the batch.</param>
+        /// <param name="initialCapacity">Initial capacity to allocate within the batch.</param>
+        /// <param name="shapeBatches">The set of shapes to contain this batch.</param>
+        /// <returns>Shape batch for the shape type.</returns>
+        /// <remarks>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.</remarks>
+        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.
@@ -26,34 +34,106 @@ 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...
 
-
+    /// <summary>
+    /// 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.
+    /// </summary>
     public interface IConvexShape : IShape
     {
-        void ComputeBounds(in Quaternion orientation, out Vector3 min, out Vector3 max);
+        /// <summary>
+        /// Computes the bounding box of a shape given an orientation.
+        /// </summary>
+        /// <param name="orientation">Orientation of the shape to use when computing the bounding box.</param>
+        /// <param name="min">Minimum corner of the bounding box.</param>
+        /// <param name="max">Maximum corner of the bounding box.</param>
+        void ComputeBounds(Quaternion orientation, out Vector3 min, out Vector3 max);
+
+        /// <summary>
+        /// Computes information about how the bounding box should be expanded in response to angular velocity.
+        /// </summary>
+        /// <param name="maximumRadius"></param>
+        /// <param name="maximumAngularExpansion"></param>
+        /// <remarks>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.</remarks>
         void ComputeAngularExpansionData(out float maximumRadius, out float maximumAngularExpansion);
 
-        void ComputeInertia(float mass, out BodyInertia inertia);
+        /// <summary>
+        /// Computes the inertia for a body given a mass.
+        /// </summary>
+        /// <param name="mass">Mass to use to compute the body's inertia.</param>
+        /// <returns>Inertia for the body.</returns>
+        /// <remarks>Note that the <see cref="BodyInertia"/> 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.</remarks>
+        BodyInertia ComputeInertia(float mass);
 
-        bool RayTest(in RigidPose pose, in Vector3 origin, in Vector3 direction, out float t, out Vector3 normal);
+        /// <summary>
+        /// Tests a ray against the shape.
+        /// </summary>
+        /// <param name="pose">Pose of the shape during the ray test.</param>
+        /// <param name="origin">Origin of the ray to test against the shape relative to the shape.</param>
+        /// <param name="direction">Direction of the ray to test against the shape.</param>
+        /// <param name="t">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.</param>
+        /// <param name="normal">Normal of the impact surface, if any.</param>
+        /// <returns>True if the ray intersected the shape, false otherwise.</returns>
+        bool RayTest(in RigidPose pose, Vector3 origin, Vector3 direction, out float t, out Vector3 normal);
     }
 
     /// <summary>
     /// Defines a compound shape type that has children of potentially different types.
     /// </summary>
-    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.
-        void ComputeBounds(in Quaternion orientation, Shapes shapeBatches, out Vector3 min, out Vector3 max);
+        /// <summary>
+        /// Computes the bounding box of a compound shape.
+        /// </summary>
+        /// <param name="orientation">Orientation of the compound.</param>
+        /// <param name="shapeBatches">Shape batches to look up child shape information in.</param>
+        /// <param name="min">Minimum of the compound's bounding box.</param>
+        /// <param name="max">Maximum of the compound's bounding box.</param>
+        void ComputeBounds(Quaternion orientation, Shapes shapeBatches, out Vector3 min, out Vector3 max);
+        /// <summary>
+        /// Submits child shapes to a bounding box batcher for vectorized bounds calculation.
+        /// </summary>
+        /// <remarks>This is used internally for bounding box calculation, but it is unlikely to be useful externally.</remarks>
+        /// <param name="batcher">Batcher to accumulate children in.</param>
+        /// <param name="pose">Pose of the compound.</param>
+        /// <param name="velocity">Velocity of the compound used to expand child bounds.</param>
+        /// <param name="bodyIndex">Index of the body in the active body set; used to accumulate child bounds results.</param>
         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.
-        void RayTest<TRayHitHandler>(in RigidPose pose, in RayData ray, ref float maximumT, Shapes shapeBatches, ref TRayHitHandler hitHandler) where TRayHitHandler : struct, IShapeRayHitHandler;
-        void RayTest<TRayHitHandler>(in RigidPose pose, ref RaySource rays, Shapes shapeBatches, ref TRayHitHandler hitHandler) where TRayHitHandler : struct, IShapeRayHitHandler;
+        /// <summary>
+        /// Tests a ray against the shape.
+        /// </summary>
+        /// <param name="pose">Pose of the shape during the ray test.</param>
+        /// <param name="ray">Ray to test against the shape.</param>
+        /// <param name="maximumT">Maximum distance along the ray, in units of the ray direction's length, that the ray will test.</param>
+        /// <param name="shapeBatches">Shape batches to look up child shapes in if necessary.</param>
+        /// <param name="pool">Buffer pool used for any temporary allocations required by the test.</param>
+        /// <param name="hitHandler">Callbacks called when the ray interacts with a test candidate.</param>
+        void RayTest<TRayHitHandler>(in RigidPose pose, in RayData ray, ref float maximumT, Shapes shapeBatches, BufferPool pool, ref TRayHitHandler hitHandler) where TRayHitHandler : struct, IShapeRayHitHandler;
+
+        /// <summary>
+        /// Tests multiple rays against the shape.
+        /// </summary>
+        /// <param name="pose">Pose of the shape during the ray test.</param>
+        /// <param name="rays">Rays to test against the shape.</param>
+        /// <param name="shapeBatches">Shape batches to look up child shapes in if necessary.</param>
+        /// <param name="hitHandler">Callbacks called when the ray interacts with a test candidate.</param>
+        /// <param name="pool">Buffer pool used for any temporary allocations required by the test.</param>
+        void RayTest<TRayHitHandler>(in RigidPose pose, ref RaySource rays, Shapes shapeBatches, BufferPool pool, ref TRayHitHandler hitHandler) where TRayHitHandler : struct, IShapeRayHitHandler;
+        /// <summary>
+        /// Gets the number of children in the compound shape.
+        /// </summary>
         int ChildCount { get; }
+        /// <summary>
+        /// Gets a child from the compound by index.
+        /// </summary>
+        /// <param name="compoundChildIndex">Index of the child to look up.</param>
+        /// <returns>Reference to the requested compound child.</returns>
         ref CompoundChild GetChild(int compoundChildIndex);
-        void Dispose(BufferPool pool);
     }
 
     /// <summary>
@@ -61,25 +141,68 @@ public interface ICompoundShape : IShape, IBoundsQueryableCompound
     /// </summary>
     /// <typeparam name="TChildShape">Type of the child shapes.</typeparam>
     /// <typeparam name="TChildShapeWide">Type of the child shapes, formatted in AOSOA layout.</typeparam>
-    public interface IHomogeneousCompoundShape<TChildShape, TChildShapeWide> : IShape, IBoundsQueryableCompound
-        where TChildShape : IConvexShape
-        where TChildShapeWide : IShapeWide<TChildShape>
+    public interface IHomogeneousCompoundShape<TChildShape, TChildShapeWide> : IDisposableShape, IBoundsQueryableCompound
+        where TChildShape : unmanaged, IConvexShape
+        where TChildShapeWide : unmanaged, IShapeWide<TChildShape>
     {
-        void ComputeBounds(in Quaternion orientation, out Vector3 min, out Vector3 max);
+        /// <summary>
+        /// Computes the bounding box of a compound shape.
+        /// </summary>
+        /// <param name="orientation">Orientation of the compound.</param>
+        /// <param name="min">Minimum of the compound's bounding box.</param>
+        /// <param name="max">Maximum of the compound's bounding box.</param>
+        void ComputeBounds(Quaternion orientation, out Vector3 min, out Vector3 max);
 
-        void RayTest<TRayHitHandler>(in RigidPose pose, in RayData ray, ref float maximumT, ref TRayHitHandler hitHandler) where TRayHitHandler : struct, IShapeRayHitHandler;
-        void RayTest<TRayHitHandler>(in RigidPose pose, ref RaySource rays, ref TRayHitHandler hitHandler) where TRayHitHandler : struct, IShapeRayHitHandler;
+        /// <summary>
+        /// Tests a ray against the shape.
+        /// </summary>
+        /// <param name="pose">Pose of the shape during the ray test.</param>
+        /// <param name="ray">Ray to test against the shape.</param>
+        /// <param name="maximumT">Maximum distance along the ray, in units of the ray direction's length, that the ray will test.</param>
+        /// <param name="pool">Buffer pool used for any temporary allocations required by the test.</param>
+        /// <param name="hitHandler">Callbacks called when the ray interacts with a test candidate.</param>
+        void RayTest<TRayHitHandler>(in RigidPose pose, in RayData ray, ref float maximumT, BufferPool pool, ref TRayHitHandler hitHandler) where TRayHitHandler : struct, IShapeRayHitHandler;
 
+        /// <summary>
+        /// Tests multiple rays against the shape.
+        /// </summary>
+        /// <param name="pose">Pose of the shape during the ray test.</param>
+        /// <param name="rays">Rays to test against the shape.</param>
+        /// <param name="pool">Buffer pool used for any temporary allocations required by the test.</param>
+        /// <param name="hitHandler">Callbacks called when the ray interacts with a test candidate.</param>
+        void RayTest<TRayHitHandler>(in RigidPose pose, ref RaySource rays, BufferPool pool, ref TRayHitHandler hitHandler) where TRayHitHandler : struct, IShapeRayHitHandler;
+        /// <summary>
+        /// Gets the number of children in the compound shape.
+        /// </summary>
         int ChildCount { get; }
+        /// <summary>
+        /// Gets a child shape as it appears in the compound's local space.
+        /// </summary>
+        /// <param name="childIndex">Index of the child in the compound parent.</param>
+        /// <param name="childData">Data representing the child.</param>
         void GetLocalChild(int childIndex, out TChildShape childData);
+        /// <summary>
+        /// 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.
+        /// </summary>
+        /// <param name="childIndex">Index of the child.</param>
+        /// <param name="childData">Shape of the child.</param>
+        /// <param name="childPose">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.</param>
         void GetPosedLocalChild(int childIndex, out TChildShape childData, out RigidPose childPose);
+        /// <summary>
+        /// Gets a child shape as it appears in the compound's local space.
+        /// </summary>
+        /// <param name="childIndex">Index of the child in the compound parent.</param>
+        /// <param name="childData">Reference to an AOSOA slot.</param>
         void GetLocalChild(int childIndex, ref TChildShapeWide childData);
-        void Dispose(BufferPool pool);
     }
 
+    /// <summary>
+    /// Defines a widely vectorized bundle representation of a shape.
+    /// </summary>
+    /// <typeparam name="TShape">Scalar type of the shape.</typeparam>
     public interface IShapeWide<TShape> where TShape : IShape
     {
-
         /// <summary>
         /// Gets whether this type supports accessing its memory by lane offsets. If false, WriteSlot must be used instead of WriteFirst.
         /// </summary>
@@ -93,7 +216,7 @@ public interface IShapeWide<TShape> where TShape : IShape
         /// Memory should be assumed to be stack allocated.
         /// </summary>
         /// <param name="memory">Memory to use for internal allocations in the wide shape.</param>
-        void Initialize(in RawBuffer memory);
+        void Initialize(in Buffer<byte> memory);
 
         /// <summary>
         /// Places the specified AOS-formatted shape into the first lane of the wide 'this' reference.
@@ -108,14 +231,35 @@ public interface IShapeWide<TShape> where TShape : IShape
         /// <param name="index">Index of the slot to put the data into.</param>
         /// <param name="source">Source of the data to insert.</param>
         void WriteSlot(int index, in TShape source);
+        /// <summary>
+        /// Broadcasts a scalar shape into a bundle containing the same shape in every lane.
+        /// </summary>
+        /// <param name="shape">Scalar shape to broadcast.</param>
         void Broadcast(in TShape shape);
-
+        /// <summary>
+        /// Computes the bounds of all shapes in the bundle.
+        /// </summary>
+        /// <param name="orientations">Orientations of the shapes in the bundle.</param>
+        /// <param name="countInBundle">Number of lanes filled in the bundle.</param>
+        /// <param name="maximumRadius">Computed maximum radius of the shapes in the bundle.</param>
+        /// <param name="maximumAngularExpansion">Computed maximum bounds expansion that can be caused by angular motion.</param>
+        /// <param name="min">Minimum bounds of the shapes.</param>
+        /// <param name="max">Maximum bounds of the shapes.</param>
         void GetBounds(ref QuaternionWide orientations, int countInBundle, out Vector<float> maximumRadius, out Vector<float> maximumAngularExpansion, out Vector3Wide min, out Vector3Wide max);
         /// <summary>
         /// 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.
         /// </summary>
-        int MinimumWideRayCount { get; }
-        void RayTest(ref RigidPoses poses, ref RayWide rayWide, out Vector<int> intersected, out Vector<float> t, out Vector3Wide normal);
+        static abstract int MinimumWideRayCount { get; }
+
+        /// <summary>
+        /// Tests a ray against the shape.
+        /// </summary>
+        /// <param name="poses">Poses of the shape bundle during the ray test.</param>
+        /// <param name="rayWide">Ray to test against the shape bundle.</param>
+        /// <param name="intersected">Mask representing hit state in each lane. -1 means the ray in that lane hit, 0 means a miss.</param>
+        /// <param name="t">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.</param>
+        /// <param name="normal">Normal of the impact surface, if any.</param>
+        void RayTest(ref RigidPoseWide poses, ref RayWide rayWide, out Vector<int> intersected, out Vector<float> t, out Vector3Wide normal);
     }
 
 }
diff --git a/BepuPhysics/Collidables/Mesh.cs b/BepuPhysics/Collidables/Mesh.cs
index 5381c6851..d5d078e1e 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;
@@ -66,27 +65,92 @@ public Vector3 Scale
         }
 
         /// <summary>
-        /// Creates a mesh shape.
+        /// Fills a buffer of subtrees according to a buffer of triangles.
+        /// </summary>
+        /// <remarks>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.</remarks>
+        /// <param name="triangles">Triangles to build subtrees from.</param>
+        /// <param name="subtrees">Subtrees created for the triangles.</param>
+        public static void FillSubtreesForTriangles(Span<Triangle> triangles, Span<NodeChild> 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);
+            }
+        }
+
+        /// <summary>
+        /// Creates a mesh shape instance, but leaves the Tree in an unbuilt state. The tree must be built before the mesh can be used.
         /// </summary>
         /// <param name="triangles">Triangles to use in the mesh.</param>
         /// <param name="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.</param>
         /// <param name="pool">Pool used to allocate acceleration structures.</param>
-        public Mesh(Buffer<Triangle> triangles, in Vector3 scale, BufferPool pool) : this()
+        /// <returns>Created mesh shape.</returns>
+        /// <remarks>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.</remarks>
+        public static Mesh CreateWithoutTreeBuild(Buffer<Triangle> triangles, Vector3 scale, BufferPool pool)
         {
-            Triangles = triangles;
-            Tree = new Tree(pool, triangles.Length);
-            pool.Take<BoundingBox>(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.)
+                //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;
+            return mesh;
+        }
+
+        /// <summary>
+        /// Creates a mesh shape instance and builds an acceleration structure using a sweep builder.
+        /// </summary>
+        /// <param name="triangles">Triangles to use in the mesh.</param>
+        /// <param name="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.</param>
+        /// <param name="pool">Pool used to allocate acceleration structures.</param>
+        /// <returns>Created mesh shape.</returns>
+        /// <remarks>The sweep builder is significantly slower than the binned builder, but can sometimes create higher quality trees.
+        /// <para>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.</para></remarks>
+        public unsafe static Mesh CreateWithSweepBuild(Buffer<Triangle> triangles, Vector3 scale, BufferPool pool)
+        {
+            var mesh = CreateWithoutTreeBuild(triangles, scale, pool);
+            pool.Take<NodeChild>(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<BoundingBox>());
+            pool.Return(ref subtrees);
+            return mesh;
+        }
+
+        /// <summary>
+        /// Creates a mesh shape.
+        /// </summary>
+        /// <param name="triangles">Triangles to use in the mesh.</param>
+        /// <param name="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.</param>
+        /// <param name="pool">Pool used to allocate acceleration structures.</param>
+        /// <param name="dispatcher">Dispatcher to use to multithread the execution of the mesh build process. If null, the build will be single threaded.</param>
+        public unsafe Mesh(Buffer<Triangle> triangles, Vector3 scale, BufferPool pool, IThreadDispatcher dispatcher = null)
+        {
+            this = CreateWithoutTreeBuild(triangles, scale, pool);
+            pool.Take<NodeChild>(triangles.Length, out var subtrees);
+            FillSubtreesForTriangles(triangles, subtrees);
+            Tree.BinnedBuild(subtrees, pool, dispatcher);
+            pool.Return(ref subtrees);
         }
 
         /// <summary>
@@ -112,7 +176,6 @@ public unsafe Mesh(Span<byte> data, BufferPool pool)
         /// <summary>
         /// Gets the number of bytes it would take to store the given mesh in a byte buffer.
         /// </summary>
-        /// <param name="mesh">Mesh to measure.</param>
         /// <returns>Number of bytes it would take to store the mesh.</returns>
         public readonly unsafe int GetSerializedByteCount()
         {
@@ -122,7 +185,6 @@ public readonly unsafe int GetSerializedByteCount()
         /// <summary>
         /// Writes a mesh's data to a byte buffer.
         /// </summary>
-        /// <param name="mesh">Mesh to write into the byte buffer.</param>
         /// <param name="data">Byte buffer to store the mesh in.</param>
         public readonly unsafe void Serialize(Span<byte> data)
         {
@@ -139,7 +201,7 @@ public readonly unsafe void Serialize(Span<byte> 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;
@@ -148,17 +210,17 @@ 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 = 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;
         }
 
         [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];
@@ -167,7 +229,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);
@@ -192,7 +254,7 @@ public readonly void ComputeBounds(in Quaternion orientation, out Vector3 min, o
             }
         }
 
-        public readonly ShapeBatch CreateShapeBatch(BufferPool pool, int initialCapacity, Shapes shapeBatches)
+        public static ShapeBatch CreateShapeBatch(BufferPool pool, int initialCapacity, Shapes shapeBatches)
         {
             return new HomogeneousCompoundShapeBatch<Mesh, Triangle, TriangleWide>(pool, initialCapacity);
         }
@@ -206,7 +268,7 @@ unsafe struct HitLeafTester<T> : 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, 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)
@@ -227,7 +289,8 @@ public unsafe void TestLeaf(int leafIndex, RayData* rayData, float* maximumT)
         /// <param name="ray">Ray to test against the mesh.</param>
         /// <param name="maximumT">Maximum length of the ray in units of the ray direction length.</param>
         /// <param name="hitHandler">Callback to execute for every hit.</param>
-        public readonly unsafe void RayTest<TRayHitHandler>(in RigidPose pose, in RayData ray, ref float maximumT, ref TRayHitHandler hitHandler) where TRayHitHandler : struct, IShapeRayHitHandler
+        /// <param name="pool">Pool to use for any temporary allocations required during the ray test.</param>
+        public readonly unsafe void RayTest<TRayHitHandler>(in RigidPose pose, in RayData ray, ref float maximumT, BufferPool pool, ref TRayHitHandler hitHandler) where TRayHitHandler : struct, IShapeRayHitHandler
         {
             HitLeafTester<TRayHitHandler> leafTester;
             leafTester.Triangles = Triangles.Memory;
@@ -239,7 +302,7 @@ public readonly unsafe void RayTest<TRayHitHandler>(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;
         }
@@ -251,7 +314,8 @@ public readonly unsafe void RayTest<TRayHitHandler>(in RigidPose pose, in RayDat
         /// <param name="pose">Pose of the mesh during the ray test.</param>
         /// <param name="rays">Set of rays to cast against the mesh.</param>
         /// <param name="hitHandler">Callbacks to execute.</param>
-        public readonly unsafe void RayTest<TRayHitHandler>(in RigidPose pose, ref RaySource rays, ref TRayHitHandler hitHandler) where TRayHitHandler : struct, IShapeRayHitHandler
+        /// <param name="pool">Pool to use for any temporary allocations required during the ray test.</param>
+        public readonly unsafe void RayTest<TRayHitHandler>(in RigidPose pose, ref RaySource rays, BufferPool pool, ref TRayHitHandler hitHandler) where TRayHitHandler : struct, IShapeRayHitHandler
         {
             HitLeafTester<TRayHitHandler> leafTester;
             leafTester.Triangles = Triangles.Memory;
@@ -267,7 +331,7 @@ public readonly unsafe void RayTest<TRayHitHandler>(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;
@@ -290,11 +354,11 @@ public readonly unsafe void FindLocalOverlaps<TOverlaps, TSubpairOverlaps>(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);
             }
         }
 
-        public readonly unsafe void FindLocalOverlaps<TOverlaps>(in Vector3 min, in Vector3 max, in Vector3 sweep, float maximumT, BufferPool pool, Shapes shapes, void* overlaps)
+        public readonly unsafe void FindLocalOverlaps<TOverlaps>(Vector3 min, Vector3 max, Vector3 sweep, float maximumT, BufferPool pool, Shapes shapes, void* overlaps)
             where TOverlaps : ICollisionTaskSubpairOverlaps
         {
             var scaledMin = min * inverseScale;
@@ -304,7 +368,18 @@ public readonly unsafe void FindLocalOverlaps<TOverlaps>(in Vector3 min, in Vect
             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);
+        }
+
+        [MethodImpl(MethodImplOptions.AggressiveInlining)]
+        public readonly void FindLocalOverlaps<TEnumerator>(Vector3 min, Vector3 max, BufferPool pool, Shapes shapes, ref TEnumerator enumerator)
+            where TEnumerator : IBreakableForEach<int>
+        {
+            //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
@@ -340,7 +415,7 @@ public bool GetNextTriangle(out Vector3 a, out Vector3 b, out Vector3 c)
         /// Subtracts the newCenter from all points in the mesh hull.
         /// </summary>
         /// <param name="newCenter">New center that all points will be made relative to.</param>
-        public unsafe void Recenter(in Vector3 newCenter)
+        public void Recenter(Vector3 newCenter)
         {
             var scaledOffset = newCenter * inverseScale;
             for (int i = 0; i < Triangles.Length; ++i)
@@ -365,17 +440,19 @@ public unsafe void Recenter(in Vector3 newCenter)
         /// Assumes the mesh is closed and should be treated as solid.
         /// </summary>
         /// <param name="mass">Mass to scale the inertia tensor with.</param>
-        /// <param name="inertia">Inertia tensor of the closed mesh.</param>
         /// <param name="center">Center of the closed mesh.</param>
-        public void ComputeClosedInertia(float mass, out BodyInertia inertia, out Vector3 center)
+        /// <returns>Inertia tensor of the closed mesh.</returns>
+        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;
         }
 
         /// <summary>
@@ -383,13 +460,15 @@ public void ComputeClosedInertia(float mass, out BodyInertia inertia, out Vector
         /// Assumes the mesh is closed and should be treated as solid.
         /// </summary>
         /// <param name="mass">Mass to scale the inertia tensor with.</param>
-        /// <param name="inertia">Inertia of the closed mesh.</param>
-        public readonly void ComputeClosedInertia(float mass, out BodyInertia inertia)
+        /// <returns>Inertia tensor of the closed mesh.</returns>
+        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;
         }
 
         /// <summary>
@@ -420,17 +499,19 @@ public readonly Vector3 ComputeClosedCenterOfMass()
         /// Assumes the mesh is open and should be treated as a triangle soup.
         /// </summary>
         /// <param name="mass">Mass to scale the inertia tensor with.</param>
-        /// <param name="inertia">Inertia tensor of the closed mesh.</param>
         /// <param name="center">Center of the open mesh.</param>
-        public void ComputeOpenInertia(float mass, out BodyInertia inertia, out Vector3 center)
+        /// <returns>Inertia tensor of the closed mesh.</returns>
+        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;
         }
 
         /// <summary>
@@ -438,13 +519,15 @@ public void ComputeOpenInertia(float mass, out BodyInertia inertia, out Vector3
         /// Assumes the mesh is open and should be treated as a triangle soup.
         /// </summary>
         /// <param name="mass">Mass to scale the inertia tensor with.</param>
-        /// <param name="inertia">Inertia of the open mesh.</param>
-        public readonly void ComputeOpenInertia(float mass, out BodyInertia inertia)
+        /// <returns>Inertia of the open mesh.</returns>
+        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;
         }
 
         /// <summary>
@@ -473,7 +556,7 @@ public void Dispose(BufferPool bufferPool)
         /// Type id of mesh shapes.
         /// </summary>
         public const int Id = 8;
-        public readonly int TypeId => Id;
+        public static int TypeId => Id;
 
     }
 }
diff --git a/BepuPhysics/Collidables/MeshInertiaHelper.cs b/BepuPhysics/Collidables/MeshInertiaHelper.cs
index 9f1a3e814..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
 {
@@ -27,7 +24,7 @@ public interface ITriangleSource
     /// </summary>
     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 +106,7 @@ public static void ComputeTetrahedronContribution(in Vector3 a, in Vector3 b, in
         /// <param name="c">Third vertex of the tetrahedron.</param>
         /// <returns>Volume of the tetrahedron.</returns>
         [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 +119,7 @@ public static float ComputeTetrahedronVolume(in Vector3 a, in Vector3 b, in Vect
         /// <param name="c">Third vertex of the tetrahedron.</param>
         /// <param name="volume">Volume of the tetrahedron.</param>
         /// <param name="inertiaTensor">Inertia tensor of this tetrahedron assuming a density of 1.</param>
-        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 +202,7 @@ public static void ComputeClosedCenterOfMass<TTriangleSource>(ref TTriangleSourc
         /// <param name="c">Third vertex in the triangle.</param>
         /// <param name="mass">Mass of the triangle.</param>
         /// <param name="inertiaTensor">Inertia tensor of the triangle.</param>
-        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 +251,7 @@ public static void ComputeTriangleContribution(in Vector3 a, in Vector3 b, in Ve
         /// <param name="b">Second vertex in the triangle.</param>
         /// <param name="c">Third vertex in the triangle.</param>
         /// <returns>Area of the triangle.</returns>
-        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 +264,7 @@ public static float ComputeTriangleArea(in Vector3 a, in Vector3 b, in Vector3 c
         /// <param name="c">Third vertex in the triangle.</param>
         /// <param name="area">Area of the triangle.</param>
         /// <param name="inertiaTensor">Inertia tensor of the triangle assuming that the density is 1.</param>
-        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 +341,7 @@ public static Vector3 ComputeOpenCenterOfMass<TTriangleSource>(ref TTriangleSour
         /// <param name="mass">Mass associated with the inertia tensor being moved.</param>
         /// <param name="offset">Offset from the current inertia frame of reference to the new frame of reference.</param>
         /// <param name="inertiaOffset">Modification to add to the inertia tensor to move it into the new reference frame.</param>
-        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 5404b74e8..c00a97822 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;
@@ -12,7 +11,7 @@ namespace BepuPhysics.Collidables
 {
     public abstract class ShapeBatch
     {
-        protected RawBuffer shapesData;
+        protected Buffer<byte> shapesData;
         protected int shapeDataSize;
         /// <summary>
         /// Gets the number of shapes that the batch can currently hold without resizing.
@@ -54,13 +53,46 @@ 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);
-        internal virtual void ComputeBounds(int shapeIndex, in Quaternion orientation, out float maximumRadius, out float maximumAngularExpansion, 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, 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<TRayHitHandler>(int shapeIndex, in RigidPose pose, in RayData ray, ref float maximumT, ref TRayHitHandler hitHandler) where TRayHitHandler : struct, IShapeRayHitHandler;
-        public abstract void RayTest<TRayHitHandler>(int shapeIndex, in RigidPose rigidPose, ref RaySource rays, ref TRayHitHandler hitHandler) where TRayHitHandler : struct, IShapeRayHitHandler;
+        /// <summary>
+        /// Tests a ray against a shape in the batch.
+        /// </summary>
+        /// <typeparam name="TRayHitHandler">Type of the hit handler that will have results reported to it.</typeparam>
+        /// <param name="shapeIndex">Index of the shape in the batch to test.</param>
+        /// <param name="pose">Pose of the shape to use for the test.</param>
+        /// <param name="ray">Ray to test against the shape.</param>
+        /// <param name="maximumT">The maximum parametric distance along the line. May be mutated by the hit handler.</param>
+        /// <param name="hitHandler">Hit handler that will process the reported hits.</param>
+        /// <param name="pool">Pool used for temporary allocations required by the test, if any.</param>
+        public abstract void RayTest<TRayHitHandler>(int shapeIndex, in RigidPose pose, in RayData ray, ref float maximumT, BufferPool pool, ref TRayHitHandler hitHandler) where TRayHitHandler : struct, IShapeRayHitHandler;
+        /// <summary>
+        /// Tests a bunch of rays against a shape in the batch.
+        /// </summary>
+        /// <typeparam name="TRayHitHandler">Type of the hit handler that will have results reported to it.</typeparam>
+        /// <param name="shapeIndex">Index of the shape in the batch to test.</param>
+        /// <param name="pose">Pose of the shape to use for the test.</param>
+        /// <param name="rays">Rays to test against the shape.</param>
+        /// <param name="hitHandler">Hit handler that will process the reported hits.</param>
+        /// <param name="pool">Pool used for temporary allocations required by the test, if any.</param>
+        public abstract void RayTest<TRayHitHandler>(int shapeIndex, in RigidPose pose, ref RaySource rays, BufferPool pool, ref TRayHitHandler hitHandler) where TRayHitHandler : struct, IShapeRayHitHandler;
 
         /// <summary>
         /// Gets a raw untyped pointer to a shape's data.
@@ -122,7 +154,7 @@ public abstract class ShapeBatch<TShape> : 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);
         }
@@ -146,7 +178,7 @@ void InternalResize(int shapeCount, int oldCopyLength)
         {
             shapeDataSize = Unsafe.SizeOf<TShape>();
             var requiredSizeInBytes = shapeCount * Unsafe.SizeOf<TShape>();
-            pool.TakeAtLeast(requiredSizeInBytes, out var newShapesData);
+            pool.TakeAtLeast<byte>(requiredSizeInBytes, out var newShapesData);
             var newShapes = newShapesData.As<TShape>();
 #if DEBUG
             //In debug mode, unused slots are kept at the default value. This helps catch misuse.
@@ -197,8 +229,23 @@ public override void Dispose()
         }
     }
 
+    /// <summary>
+    /// Defines a shape batch containing convex objects that support simple inertia calculations.
+    /// </summary>
+    /// <remarks>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.</remarks>
+    public interface IConvexShapeBatch
+    {
+        /// <summary>
+        /// Computes the inertia of a shape.
+        /// </summary>
+        /// <param name="shapeIndex">Index of the shape to compute the inertia of.</param>
+        /// <param name="mass">Mass to use to compute the inertia.</param>
+        /// <returns>Inertia of the shape.</returns>
+        BodyInertia ComputeInertia(int shapeIndex, float mass);
+    }
 
-    public class ConvexShapeBatch<TShape, TShapeWide> : ShapeBatch<TShape>
+    public class ConvexShapeBatch<TShape, TShapeWide> : ShapeBatch<TShape>, IConvexShapeBatch
         where TShape : unmanaged, IConvexShape
         where TShapeWide : unmanaged, IShapeWide<TShape>
     {
@@ -216,26 +263,29 @@ 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);
         }
 
-        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)
+        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);
             shape.ComputeAngularExpansionData(out maximumRadius, out angularExpansion);
         }
 
-        public override void RayTest<TRayHitHandler>(int shapeIndex, in RigidPose pose, in RayData ray, ref float maximumT, ref TRayHitHandler hitHandler)
+        public override void RayTest<TRayHitHandler>(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)
             {
@@ -243,7 +293,7 @@ public override void RayTest<TRayHitHandler>(int shapeIndex, in RigidPose pose,
             }
         }
 
-        public unsafe override void RayTest<TRayHitHandler>(int index, in RigidPose pose, ref RaySource rays, ref TRayHitHandler hitHandler)
+        public override void RayTest<TRayHitHandler>(int index, in RigidPose pose, ref RaySource rays, BufferPool pool, ref TRayHitHandler hitHandler)
         {
             WideRayTester.Test<RaySource, TShape, TShapeWide, TRayHitHandler>(ref shapes[index], pose, ref rays, ref hitHandler);
         }
@@ -263,8 +313,8 @@ protected override void Dispose(int index, BufferPool pool)
 
 
     public class HomogeneousCompoundShapeBatch<TShape, TChildShape, TChildShapeWide> : ShapeBatch<TShape> where TShape : unmanaged, IHomogeneousCompoundShape<TChildShape, TChildShapeWide>
-        where TChildShape : IConvexShape
-        where TChildShapeWide : IShapeWide<TChildShape>
+        where TChildShape : unmanaged, IConvexShape
+        where TChildShapeWide : unmanaged, IShapeWide<TChildShape>
     {
         public HomogeneousCompoundShapeBatch(BufferPool pool, int initialShapeCount) : base(pool, initialShapeCount)
         {
@@ -286,20 +336,18 @@ 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<TRayHitHandler>(int shapeIndex, in RigidPose pose, in RayData ray, ref float maximumT, ref TRayHitHandler hitHandler)
+        public override void RayTest<TRayHitHandler>(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<TRayHitHandler>(int shapeIndex, in RigidPose pose, ref RaySource rays, ref TRayHitHandler hitHandler)
+        public override void RayTest<TRayHitHandler>(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);
         }
     }
 
@@ -333,21 +381,19 @@ 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<TRayHitHandler>(int shapeIndex, in RigidPose pose, in RayData ray, ref float maximumT, ref TRayHitHandler hitHandler)
+        public override void RayTest<TRayHitHandler>(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<TRayHitHandler>(int shapeIndex, in RigidPose pose, ref RaySource rays, ref TRayHitHandler hitHandler)
+        public override void RayTest<TRayHitHandler>(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);
         }
     }
 
@@ -383,23 +429,36 @@ public Shapes(BufferPool pool, int initialCapacityPerTypeBatch)
         /// <param name="shapeIndex">Index of the shape.</param>
         /// <param name="bounds">Bounding box of the specified shape with the specified pose.</param>
         [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);
         }
+        /// <summary>
+        /// Computes a bounding box for a single shape.
+        /// </summary>
+        /// <param name="position">Position of the shape.</param>
+        /// <param name="orientation">Orientation of the shape.</param>
+        /// <param name="shapeIndex">Index of the shape.</param>
+        /// <param name="bounds">Bounding box of the specified shape with the specified pose.</param>
+        [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<TShape>(int shapeIndex) where TShape : unmanaged, IShape
         {
-            var typeId = default(TShape).TypeId;
+            var typeId = TShape.TypeId;
             return ref Unsafe.As<ShapeBatch, ShapeBatch<TShape>>(ref batches[typeId])[shapeIndex];
         }
 
 
         public TypedIndex Add<TShape>(in TShape shape) where TShape : unmanaged, IShape
         {
-            var typeId = default(TShape).TypeId;
+            var typeId = TShape.TypeId;
             if (RegisteredTypeSpan <= typeId)
             {
                 registeredTypeSpan = typeId + 1;
@@ -410,7 +469,7 @@ public TypedIndex Add<TShape>(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<TShape>);
diff --git a/BepuPhysics/Collidables/Sphere.cs b/BepuPhysics/Collidables/Sphere.cs
index 0d893963d..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;
@@ -51,12 +48,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();
@@ -95,8 +92,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,9 +102,10 @@ 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)
+        public static ShapeBatch CreateShapeBatch(BufferPool pool, int initialCapacity, Shapes shapes)
         {
             return new ConvexShapeBatch<Sphere, SphereWide>(pool, initialCapacity);
         }
@@ -116,7 +115,7 @@ public readonly ShapeBatch CreateShapeBatch(BufferPool pool, int initialCapacity
         /// Type id of sphere shapes.
         /// </summary>
         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<Sphere>
@@ -138,7 +137,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<byte> memory) { }
 
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
         public void WriteSlot(int index, in Sphere source)
@@ -161,7 +160,7 @@ public void GetBounds(ref QuaternionWide orientations, int countInBundle, out Ve
         }
 
 
-        public int MinimumWideRayCount
+        public static int MinimumWideRayCount
         {
             [MethodImpl(MethodImplOptions.AggressiveInlining)]
             get
@@ -170,7 +169,7 @@ public int MinimumWideRayCount
             }
         }
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public void RayTest(ref RigidPoses pose, ref RayWide rayWide, out Vector<int> intersected, out Vector<float> t, out Vector3Wide normal)
+        public void RayTest(ref RigidPoseWide pose, ref RayWide rayWide, out Vector<int> intersected, out Vector<float> 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 5574f3cd1..7378ed7b9 100644
--- a/BepuPhysics/Collidables/Triangle.cs
+++ b/BepuPhysics/Collidables/Triangle.cs
@@ -31,7 +31,7 @@ public struct Triangle : IConvexShape
         /// <param name="a">First vertex of the triangle in local space.</param>
         /// <param name="b">Second vertex of the triangle in local space.</param>
         /// <param name="c">Third vertex of the triangle in local space.</param>
-        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);
@@ -109,14 +109,16 @@ 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)
+        public static ShapeBatch CreateShapeBatch(BufferPool pool, int initialCapacity, Shapes shapeBatches)
         {
             return new ConvexShapeBatch<Triangle, TriangleWide>(pool, initialCapacity);
         }
@@ -125,7 +127,7 @@ public readonly ShapeBatch CreateShapeBatch(BufferPool pool, int initialCapacity
         /// Type id of triangle shapes.
         /// </summary>
         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; } }
     }
 
 
@@ -153,30 +155,49 @@ 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<byte> memory) { }
 
-        /// <summary>
-        /// Provides an estimate of the scale of a shape. 
-        /// </summary>
-        /// <param name="epsilonScale">Approximate scale of the shape for use in epsilons.</param>
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public void EstimateEpsilonScale(out Vector<float> 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);
         }
 
 
+        /// <summary>
+        /// Minimum dot product between the detected local normal and the face normal of a triangle necessary to create contacts.
+        /// </summary>
+        public const float BackfaceNormalDotRejectionThreshold = -1e-2f;
+        /// <summary>
+        /// 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 
+        /// </summary>
+        public const float DegenerateTriangleEpsilon = 1e-6f;
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public void WriteSlot(int index, in Triangle source)
+        public static void ComputeTriangleEpsilonScale(in Vector<float> abLengthSquared, in Vector<float> caLengthSquared, out Vector<float> epsilonScale)
         {
-            GatherScatter.GetOffsetInstance(ref this, index).WriteFirst(source);
+            epsilonScale = Vector.SquareRoot(Vector.Max(abLengthSquared, caLengthSquared));
         }
+        [MethodImpl(MethodImplOptions.AggressiveInlining)]
+        public static void ComputeDegenerateTriangleEpsilon(in Vector<float> abLengthSquared, in Vector<float> caLengthSquared, out Vector<float> epsilonScale, out Vector<float> epsilon)
+        {
+            ComputeTriangleEpsilonScale(abLengthSquared, caLengthSquared, out epsilonScale);
+            epsilon = new Vector<float>(DegenerateTriangleEpsilon) * epsilonScale;
+        }
+        [MethodImpl(MethodImplOptions.AggressiveInlining)]
+        public static void ComputeNondegenerateTriangleMask(in Vector3Wide ab, in Vector3Wide ca, in Vector<float> triangleNormalLength, out Vector<float> epsilonScale, out Vector<int> 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<float> abLengthSquared, in Vector<float> caLengthSquared, in Vector<float> triangleNormalLength, out Vector<float> epsilonScale, out Vector<int> 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<float> maximumRadius, out Vector<float> maximumAngularExpansion, out Vector3Wide min, out Vector3Wide max)
@@ -199,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
@@ -234,7 +255,7 @@ public static void RayTest(ref Vector3Wide a, ref Vector3Wide b, ref Vector3Wide
                         Vector.GreaterThanOrEqual(w, Vector<float>.Zero)),
                     Vector.LessThanOrEqual(v + w, dn)));
         }
-        public void RayTest(ref RigidPoses pose, ref RayWide ray, out Vector<int> intersected, out Vector<float> t, out Vector3Wide normal)
+        public void RayTest(ref RigidPoseWide pose, ref RayWide ray, out Vector<int> intersected, out Vector<float> 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/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
 {
+    /// <summary>
+    /// Represents an index with an associated type packed into a single integer.
+    /// </summary>
     public struct TypedIndex : IEquatable<TypedIndex>
     {
         /// <summary>
diff --git a/BepuPhysics/CollisionDetection/BroadPhase.cs b/BepuPhysics/CollisionDetection/BroadPhase.cs
index 5c79f3821..64656be90 100644
--- a/BepuPhysics/CollisionDetection/BroadPhase.cs
+++ b/BepuPhysics/CollisionDetection/BroadPhase.cs
@@ -6,36 +6,64 @@
 using System.Runtime.CompilerServices;
 using System.Numerics;
 using BepuPhysics.Trees;
+using System.Threading;
+using BepuUtilities.Collections;
+using BepuUtilities.TaskScheduling;
 
 namespace BepuPhysics.CollisionDetection
 {
+    /// <summary>
+    /// Manages scene acceleration structures for collision detection and queries.
+    /// </summary>
     public unsafe partial class BroadPhase : IDisposable
     {
-        internal Buffer<CollidableReference> activeLeaves;
-        internal Buffer<CollidableReference> staticLeaves;
+        /// <summary>
+        /// Collidable references contained within the <see cref="ActiveTree"/>. Note that values at or beyond the <see cref="ActiveTree"/>.LeafCount are not defined.
+        /// </summary>
+        public Buffer<CollidableReference> ActiveLeaves;
+        /// <summary>
+        /// Collidable references contained within the <see cref="StaticTree"/>. Note that values at or beyond <see cref="StaticTree"/>.LeafCount are not defined.
+        /// </summary>
+        public Buffer<CollidableReference> StaticLeaves;
+        /// <summary>
+        /// Pool used by the broad phase.
+        /// </summary>
         public BufferPool Pool { get; private set; }
+        /// <summary>
+        /// Tree containing wakeful bodies.
+        /// </summary>
         public Tree ActiveTree;
+        /// <summary>
+        /// Tree containing sleeping bodies and statics.
+        /// </summary>
         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.
         Tree.RefitAndRefineMultithreadedContext staticRefineContext;
 
+        Action<int> executeRefitAndMarkAction, executeRefineAction;
+
         public BroadPhase(BufferPool pool, int initialActiveLeafCapacity = 4096, int initialStaticLeafCapacity = 8192)
         {
             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();
+            executeRefitAndMarkAction = ExecuteRefitAndMark;
+            executeRefineAction = ExecuteRefine;
+
+            ActiveRefinementSchedule = DefaultActiveRefinementScheduler;
+            StaticRefinementSchedule = DefaultStaticRefinementScheduler;
         }
 
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
         private static int Add(CollidableReference collidable, ref BoundingBox bounds, ref Tree tree, BufferPool pool, ref Buffer<CollidableReference> 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);
@@ -60,59 +88,47 @@ public static bool RemoveAt(int index, ref Tree tree, ref Buffer<CollidableRefer
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
         public int AddActive(CollidableReference collidable, ref BoundingBox bounds)
         {
-            return Add(collidable, ref bounds, ref ActiveTree, Pool, ref activeLeaves);
+            return Add(collidable, ref bounds, ref ActiveTree, Pool, ref ActiveLeaves);
         }
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
         public bool RemoveActiveAt(int index, out CollidableReference movedLeaf)
         {
-            return RemoveAt(index, ref ActiveTree, ref activeLeaves, out movedLeaf);
+            return RemoveAt(index, ref ActiveTree, ref ActiveLeaves, out movedLeaf);
         }
 
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
         public int AddStatic(CollidableReference collidable, ref BoundingBox bounds)
         {
-            return Add(collidable, ref bounds, ref StaticTree, Pool, ref staticLeaves);
+            return Add(collidable, ref bounds, ref StaticTree, Pool, ref StaticLeaves);
         }
 
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
         public bool RemoveStaticAt(int index, out CollidableReference movedLeaf)
         {
-            return RemoveAt(index, ref StaticTree, ref staticLeaves, out 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;
-        }
+        /// <summary>
+        /// Gets pointers to the leaf's bounds stored in the broad phase's active tree.
+        /// </summary>
+        /// <param name="index">Index of the active collidable to examine.</param>
+        /// <param name="minPointer">Pointer to the minimum bounds in the tree.</param>
+        /// <param name="maxPointer">Pointer to the maximum bounds in the tree.</param>
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
         public void GetActiveBoundsPointers(int index, out Vector3* minPointer, out Vector3* maxPointer)
         {
-            GetBoundsPointers(index, ref ActiveTree, 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);
+            ActiveTree.GetBoundsPointers(index, out minPointer, out maxPointer);
         }
-
         /// <summary>
-        /// 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.
         /// </summary>
-        /// <param name="broadPhaseIndex">Index of the leaf in the tree to update.</param>
-        /// <param name="tree">Tree containing the leaf to update.</param>
-        /// <param name="min">New minimum bounds for the leaf.</param>
-        /// <param name="max">New maximum bounds for the leaf.</param>
-        public unsafe static void UpdateBounds(int broadPhaseIndex, ref Tree tree, in Vector3 min, in Vector3 max)
+        /// <param name="index">Index of the static to examine.</param>
+        /// <param name="minPointer">Pointer to the minimum bounds in the tree.</param>
+        /// <param name="maxPointer">Pointer to the maximum bounds in the tree.</param>
+        [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);
         }
 
         /// <summary>
@@ -121,9 +137,10 @@ public unsafe static void UpdateBounds(int broadPhaseIndex, ref Tree tree, in Ve
         /// <param name="broadPhaseIndex">Index of the leaf to update.</param>
         /// <param name="min">New minimum bounds for the leaf.</param>
         /// <param name="max">New maximum bounds for the leaf.</param>
-        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);
         }
         /// <summary>
         /// Applies updated bounds to the given active leaf index, refitting the tree to match.
@@ -131,38 +148,296 @@ public void UpdateActiveBounds(int broadPhaseIndex, in Vector3 min, in Vector3 m
         /// <param name="broadPhaseIndex">Index of the leaf to update.</param>
         /// <param name="min">New minimum bounds for the leaf.</param>
         /// <param name="max">New maximum bounds for the leaf.</param>
-        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;
+        int remainingJobCount;
+        IThreadDispatcher threadDispatcher;
+        void ExecuteRefitAndMark(int workerIndex)
+        {
+            var threadPool = threadDispatcher.WorkerPools[workerIndex];
+            while (true)
+            {
+                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);
+                }
+            }
+        }
+
+        void ExecuteRefine(int workerIndex)
+        {
+            var threadPool = threadDispatcher.WorkerPools[workerIndex];
+            var maximumSubtrees = Math.Max(activeRefineContext.MaximumSubtrees, staticRefineContext.MaximumSubtrees);
+            var subtreeReferences = new QuickList<int>(maximumSubtrees, threadPool);
+            var treeletInternalNodes = new QuickList<int>(maximumSubtrees, threadPool);
+            Tree.CreateBinnedResources(threadPool, maximumSubtrees, out var buffer, out var resources);
+            while (true)
+            {
+                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);
+        }
+
         public void Update(IThreadDispatcher threadDispatcher = null)
         {
             if (frameIndex == int.MaxValue)
                 frameIndex = 0;
             if (threadDispatcher != null)
             {
-                activeRefineContext.RefitAndRefine(ref ActiveTree, 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(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!
+                //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, frameIndex, 1f);
+                remainingJobCount = activeRefineContext.RefinementTargets.Count + staticRefineContext.RefinementTargets.Count;
+                threadDispatcher.DispatchWorkers(executeRefineAction, remainingJobCount);
+                activeRefineContext.CleanUpForRefitAndRefine(Pool);
+                staticRefineContext.CleanUpForRefitAndRefine(Pool);
+                this.threadDispatcher = null;
             }
             else
             {
                 ActiveTree.RefitAndRefine(Pool, frameIndex);
+                StaticTree.RefitAndRefine(Pool, frameIndex);
             }
+            ++frameIndex;
+        }
 
-            //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.
-            if (threadDispatcher != null)
+        /// <summary>
+        /// Returns the size and number of refinements to execute during the broad phase.
+        /// </summary>
+        /// <param name="frameIndex">Index of the frame as tracked by the broad phase.</param>
+        /// <param name="tree">Tree being considered for refinement.</param>
+        /// <param name="rootRefinementSize">Size of the root refinement. If zero or negative, no root refinement will be performed.</param>
+        /// <param name="subtreeRefinementCount">Number of subtree refinements to perform. Can be zero.</param>
+        /// <param name="subtreeRefinementSize">Target size of the subtree refinements.</param>
+        /// <param name="usePriorityQueue">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.</param>
+        public delegate void RefinementScheduler(int frameIndex, in Tree tree, out int rootRefinementSize, out int subtreeRefinementCount, out int subtreeRefinementSize, out bool usePriorityQueue);
+
+        /// <summary>
+        /// Gets or sets the refinement schedule to use for the active tree.
+        /// </summary>
+        public RefinementScheduler ActiveRefinementSchedule { get; set; }
+        /// <summary>
+        /// Gets or sets the refinement schedule to use for the static tree.
+        /// </summary>
+        public RefinementScheduler StaticRefinementSchedule { get; set; }
+
+        /// <summary>
+        /// Returns the size and number of refinements to execute for the active tree. Used by default.
+        /// </summary>
+        /// <param name="optimizationFraction">Target fraction of the tree to be optimized.</param>
+        /// <param name="rootRefinementPeriod">Period, in timesteps, of refinements applied to the root.</param>
+        /// <param name="rootRefinementSizeScale">Multiplier to apply to the square root of the leaf count to get the target root refinement size.</param>
+        /// <param name="subtreeRefinementSizeScale">Multiplier to apply to the square root of the leaf count to get the target subtree refinement size.</param>
+        /// <param name="nonpriorityPeriod">The period between non-priority queue based root refinements, measured in units of root refinements.</param>
+        /// <param name="frameIndex">Index of the frame as tracked by the broad phase.</param>
+        /// <param name="tree">Tree being considered for refinement.</param>
+        /// <param name="rootRefinementSize">Size of the root refinement. If zero or negative, no root refinement will be performed.</param>
+        /// <param name="subtreeRefinementCount">Number of subtree refinements to perform. Can be zero.</param>
+        /// <param name="subtreeRefinementSize">Target size of the subtree refinements.</param>
+        /// <param name="usePriorityQueue">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.</param>
+        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);
+            //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);
+
+            //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 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.Round((float)targetOptimizedLeafCount / subtreeRefinementSize - (refineRoot ? subtreeRefinementsPerRootRefinementInCost : 0)));
+            if (!refineRoot)
+                subtreeRefinementCount = int.Max(1, subtreeRefinementCount);
+
+            rootRefinementSize = refineRoot ? targetRootRefinementSize : 0;
+            usePriorityQueue = (frameIndex / rootRefinementPeriod) % nonpriorityPeriod != 0;
+        }
+
+        /// <summary>
+        /// Returns the size and number of refinements to execute for the active tree. Used by default.
+        /// </summary>
+        /// <param name="frameIndex">Index of the frame as tracked by the broad phase.</param>
+        /// <param name="tree">Tree being considered for refinement.</param>
+        /// <param name="rootRefinementSize">Size of the root refinement. If zero or negative, no root refinement will be performed.</param>
+        /// <param name="subtreeRefinementCount">Number of subtree refinements to perform. Can be zero.</param>
+        /// <param name="subtreeRefinementSize">Target size of the subtree refinements.</param>
+        /// <param name="usePriorityQueue">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.</param>
+        public static void DefaultActiveRefinementScheduler(int frameIndex, in Tree tree, out int rootRefinementSize, out int subtreeRefinementCount, out int subtreeRefinementSize, out bool usePriorityQueue)
+        {
+            DefaultRefinementScheduler(1f / 20f, 2, 1, 4, 16, frameIndex, tree, out rootRefinementSize, out subtreeRefinementCount, out subtreeRefinementSize, out usePriorityQueue);
+        }
+
+
+        /// <summary>
+        /// Returns the size and number of refinements to execute for the active tree. Used by default.
+        /// </summary>
+        /// <param name="frameIndex">Index of the frame as tracked by the broad phase.</param>
+        /// <param name="tree">Tree being considered for refinement.</param>
+        /// <param name="rootRefinementSize">Size of the root refinement. If zero or negative, no root refinement will be performed.</param>
+        /// <param name="subtreeRefinementCount">Number of subtree refinements to perform. Can be zero.</param>
+        /// <param name="subtreeRefinementSize">Target size of the subtree refinements.</param>
+        /// <param name="usePriorityQueue">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.</param>
+        public static void DefaultStaticRefinementScheduler(int frameIndex, in Tree tree, out int rootRefinementSize, out int subtreeRefinementCount, out int subtreeRefinementSize, out bool usePriorityQueue)
+        {
+            DefaultRefinementScheduler(1f / 100f, 2, 1, 4, 16, frameIndex, tree, out rootRefinementSize, out subtreeRefinementCount, out subtreeRefinementSize, out usePriorityQueue);
+        }
+
+        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;
+            public bool UsePriorityQueue;
+
+            //Used for active tree. Didn't split the type because whocares.
+            public Buffer<Node> 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, targetTaskCount: context.TargetTaskCount, deterministic: context.Deterministic, usePriorityQueue: context.UsePriorityQueue);
+            //Now refit!
+            var sourceNodes = context.Tree.Nodes;
+            context.Tree.Nodes = context.TargetNodes;
+            context.Tree.Refit2WithCacheOptimization(sourceNodes, pool, dispatcher, context.TaskStack, workerIndex, context.TargetTotalTaskCount);
+        }
+
+        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, 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, 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))
             {
-                staticRefineContext.RefitAndRefine(ref StaticTree, Pool, threadDispatcher, frameIndex);
+                //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 + 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);
+                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,
+                    UsePriorityQueue = usePriorityQueueActive,
+                    TargetNodes = ActiveTree.LeafCount > 2 ? new Buffer<Node>(ActiveTree.Nodes.Length, Pool) : default,
+                };
+                var staticRefineContext = new RefinementContext
+                {
+                    TaskStack = &taskStack,
+                    Tree = StaticTree,
+                    TargetTotalTaskCount = targetTotalTaskCount,
+                    TargetTaskCount = targetTotalTaskCount - targetActiveTaskCount,
+                    RootRefinementSize = staticRootRefinementSize,
+                    SubtreeRefinementCount = staticSubtreeRefinementCount,
+                    SubtreeRefinementSize = staticSubtreeRefinementSize,
+                    SubtreeRefinementStartIndex = staticSubtreeRefinementStartIndex,
+                    Deterministic = deterministic,
+                    UsePriorityQueue = usePriorityQueueStatic,
+                };
+                Span<Task> 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);
+                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;
+
             }
             else
             {
-                StaticTree.RefitAndRefine(Pool, frameIndex);
+                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);
             }
-            ++frameIndex;
+            if (frameIndex == int.MaxValue)
+                frameIndex = 0;
+            else
+                ++frameIndex;
+            //StaticTree.Validate();
+            //ActiveTree.Validate();
         }
 
         /// <summary>
@@ -201,8 +476,8 @@ void Dispose(ref Tree tree, ref Buffer<CollidableReference> leaves)
         /// <param name="staticCapacity">Number of leaves to allocate space for in the static tree.</param>
         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);
         }
 
         /// <summary>
@@ -212,8 +487,8 @@ public void EnsureCapacity(int activeCapacity, int staticCapacity)
         /// <param name="staticCapacity">Number of leaves to allocate space for in the static tree.</param>
         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);
         }
 
         /// <summary>
@@ -221,8 +496,8 @@ public void Resize(int activeCapacity, int staticCapacity)
         /// </summary>
         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..4e4cab498 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
 {
@@ -15,7 +12,7 @@ namespace BepuPhysics.CollisionDetection
     /// </summary>
     public interface IBroadPhaseSweepTester
     {
-        unsafe void Test(CollidableReference collidable, ref float maximumT);
+        void Test(CollidableReference collidable, ref float maximumT);
     }
 
     partial class BroadPhase
@@ -26,9 +23,9 @@ struct RayLeafTester<TRayTester> : IRayLeafTester where TRayTester : IBroadPhase
             public Buffer<CollidableReference> 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);
             }
         }
 
@@ -39,17 +36,18 @@ public unsafe void TestLeaf(int leafIndex, RayData* rayData, float* maximumT)
         /// <param name="origin">Origin of the ray to cast.</param>
         /// <param name="direction">Direction of the ray to cast.</param>
         /// <param name="maximumT">Maximum length of the ray traversal in units of the direction's length.</param>
+        /// <param name="pool">The buffer pool used for any temporary allocations required during the traversal.</param>
         /// <param name="rayTester">Callback to execute on ray-leaf bounding box intersections.</param>
         /// <param name="id">User specified id of the ray.</param>
-        public unsafe void RayCast<TRayTester>(in Vector3 origin, in Vector3 direction, float maximumT, ref TRayTester rayTester, int id = 0) where TRayTester : IBroadPhaseRayTester
+        public unsafe void RayCast<TRayTester>(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<TRayTester> tester;
             tester.LeafTester = rayTester;
-            tester.Leaves = activeLeaves;
-            ActiveTree.RayCast(&treeRay, &rayData, ref tester);
-            tester.Leaves = staticLeaves;
-            StaticTree.RayCast(&treeRay, &rayData, ref tester);
+            tester.Leaves = ActiveLeaves;
+            ActiveTree.RayCast(&treeRay, &rayData, pool, ref tester);
+            tester.Leaves = StaticLeaves;
+            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;
         }
@@ -60,7 +58,7 @@ struct SweepLeafTester<TSweepTester> : ISweepLeafTester where TSweepTester : IBr
             public Buffer<CollidableReference> 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);
             }
@@ -75,17 +73,18 @@ public unsafe void TestLeaf(int leafIndex, ref float maximumT)
         /// <param name="max">Maximum bounds of the box to sweep.</param>
         /// <param name="direction">Direction along which to sweep the bounding box.</param>
         /// <param name="maximumT">Maximum length of the sweep in units of the direction's length.</param>
+        /// <param name="pool">Pool to use for temporary allocations required by the traversal, if any.</param>
         /// <param name="sweepTester">Callback to execute on sweep-leaf bounding box intersections.</param>
-        public unsafe void Sweep<TSweepTester>(in Vector3 min, in Vector3 max, in Vector3 direction, float maximumT, ref TSweepTester sweepTester) where TSweepTester : IBroadPhaseSweepTester
+        public unsafe void Sweep<TSweepTester>(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<TSweepTester> tester;
             tester.LeafTester = sweepTester;
-            tester.Leaves = activeLeaves;
-            ActiveTree.Sweep(expansion, origin, direction, &treeRay, ref tester);
-            tester.Leaves = staticLeaves;
-            StaticTree.Sweep(expansion, origin, direction, &treeRay, ref tester);
+            tester.Leaves = ActiveLeaves;
+            ActiveTree.Sweep(expansion, origin, direction, &treeRay, pool, ref tester);
+            tester.Leaves = StaticLeaves;
+            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;
         }
@@ -97,10 +96,11 @@ public unsafe void Sweep<TSweepTester>(in Vector3 min, in Vector3 max, in Vector
         /// <param name="boundingBox">Bounding box to sweep.</param>
         /// <param name="direction">Direction along which to sweep the bounding box.</param>
         /// <param name="maximumT">Maximum length of the sweep in units of the direction's length.</param>
+        /// <param name="pool">Pool used for temporary allocations required by the test, if any.</param>
         /// <param name="sweepTester">Callback to execute on sweep-leaf bounding box intersections.</param>
-        public unsafe void Sweep<TSweepTester>(in BoundingBox boundingBox, in Vector3 direction, float maximumT, ref TSweepTester sweepTester) where TSweepTester : IBroadPhaseSweepTester
+        public void Sweep<TSweepTester>(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<TInnerEnumerator> : IBreakableForEach<int> where TInnerEnumerator : IBreakableForEach<CollidableReference>
@@ -121,15 +121,16 @@ public bool LoopBody(int i)
         /// <typeparam name="TOverlapEnumerator">Type of the enumerator to call for overlaps.</typeparam>
         /// <param name="min">Minimum bounds of the query box.</param>
         /// <param name="max">Maximum bounds of the query box.</param>
+        /// <param name="pool">Pool used for temporary allocations required by the test, if any.</param>
         /// <param name="overlapEnumerator">Enumerator to call for overlaps.</param>
-        public unsafe void GetOverlaps<TOverlapEnumerator>(in Vector3 min, in Vector3 max, ref TOverlapEnumerator overlapEnumerator) where TOverlapEnumerator : IBreakableForEach<CollidableReference>
+        public void GetOverlaps<TOverlapEnumerator>(Vector3 min, Vector3 max, BufferPool pool, ref TOverlapEnumerator overlapEnumerator) where TOverlapEnumerator : IBreakableForEach<CollidableReference>
         {
             BoxQueryEnumerator<TOverlapEnumerator> enumerator;
             enumerator.Enumerator = overlapEnumerator;
-            enumerator.Leaves = activeLeaves;
-            ActiveTree.GetOverlaps(min, max, ref enumerator);
-            enumerator.Leaves = staticLeaves;
-            StaticTree.GetOverlaps(min, max, ref enumerator);
+            enumerator.Leaves = ActiveLeaves;
+            ActiveTree.GetOverlaps(min, max, pool, ref enumerator);
+            enumerator.Leaves = StaticLeaves;
+            StaticTree.GetOverlaps(min, max, pool, ref enumerator);
             //Enumeration could have mutated the enumerator; preserve those modifications.
             overlapEnumerator = enumerator.Enumerator;
         }
@@ -139,15 +140,16 @@ public unsafe void GetOverlaps<TOverlapEnumerator>(in Vector3 min, in Vector3 ma
         /// </summary>
         /// <typeparam name="TOverlapEnumerator">Type of the enumerator to call for overlaps.</typeparam>
         /// <param name="boundingBox">Query box bounds.</param>
+        /// <param name="pool">Pool used for temporary allocations required by the test, if any.</param>
         /// <param name="overlapEnumerator">Enumerator to call for overlaps.</param>
-        public unsafe void GetOverlaps<TOverlapEnumerator>(in BoundingBox boundingBox, ref TOverlapEnumerator overlapEnumerator) where TOverlapEnumerator : IBreakableForEach<CollidableReference>
+        public void GetOverlaps<TOverlapEnumerator>(in BoundingBox boundingBox, BufferPool pool, ref TOverlapEnumerator overlapEnumerator) where TOverlapEnumerator : IBreakableForEach<CollidableReference>
         {
             BoxQueryEnumerator<TOverlapEnumerator> enumerator;
             enumerator.Enumerator = overlapEnumerator;
-            enumerator.Leaves = activeLeaves;
-            ActiveTree.GetOverlaps(boundingBox, ref enumerator);
-            enumerator.Leaves = staticLeaves;
-            StaticTree.GetOverlaps(boundingBox, ref enumerator);
+            enumerator.Leaves = ActiveLeaves;
+            ActiveTree.GetOverlaps(boundingBox, pool, ref enumerator);
+            enumerator.Leaves = StaticLeaves;
+            StaticTree.GetOverlaps(boundingBox, pool, 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 a2f0fb56f..e714eb9e9 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
 {
@@ -16,7 +17,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<TCallbacks> : CollidableOverlapFinder where TCallbacks : struct, INarrowPhaseCallbacks
+    public unsafe class CollidableOverlapFinder<TCallbacks> : CollidableOverlapFinder where TCallbacks : struct, INarrowPhaseCallbacks
     {
         struct SelfOverlapHandler : IOverlapHandler
         {
@@ -103,9 +104,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.
@@ -117,33 +118,49 @@ 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);
                 intertreeTestContext.PrepareJobs(ref broadPhase.ActiveTree, ref broadPhase.StaticTree, intertreeHandlers, threadDispatcher.ThreadCount);
                 nextJobIndex = -1;
-                threadDispatcher.DispatchWorkers(workerAction);
+                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)
+                {
+                    narrowPhase.overlapWorkers[i].Batcher.Flush();
+                }
+#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
                 selfTestContext.CompleteSelfTest();
                 intertreeTestContext.CompleteTest();
             }
             else
             {
-                var selfTestHandler = new SelfOverlapHandler(broadPhase.activeLeaves, narrowPhase, 0);
+                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);
+                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 8067d4c34..ed5747eec 100644
--- a/BepuPhysics/CollisionDetection/CollisionBatcher.cs
+++ b/BepuPhysics/CollisionDetection/CollisionBatcher.cs
@@ -5,12 +5,13 @@
 using BepuPhysics.CollisionDetection.CollisionTasks;
 using System.Numerics;
 using System;
+using BepuUtilities;
 
 namespace BepuPhysics.CollisionDetection
 {
     unsafe struct UntypedBlob
     {
-        public RawBuffer Buffer;
+        public Buffer<byte> Buffer;
         public int ByteCount;
 
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
@@ -45,7 +46,7 @@ public struct CollisionBatcher<TCallbacks> 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<CollisionBatch> batches;
+        internal Buffer<CollisionBatch> 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.
@@ -53,7 +54,7 @@ public struct CollisionBatcher<TCallbacks> where TCallbacks : struct, ICollision
         public BatcherContinuations<MeshReduction> MeshReductions;
         public BatcherContinuations<CompoundMeshReduction> 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;
@@ -71,7 +72,7 @@ public unsafe CollisionBatcher(BufferPool pool, Shapes shapes, CollisionTaskRegi
         }
 
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        unsafe ref TPair AllocatePair<TPair>(ref CollisionBatch batch, ref CollisionTaskReference reference) where TPair : ICollisionPair<TPair>
+        ref TPair AllocatePair<TPair>(ref CollisionBatch batch, ref CollisionTaskReference reference) where TPair : ICollisionPair<TPair>
         {
             if (!batch.Pairs.Buffer.Allocated)
             {
@@ -85,7 +86,7 @@ unsafe ref TPair AllocatePair<TPair>(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];
@@ -172,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)
@@ -196,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);
@@ -205,15 +206,15 @@ 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);
         }
 
-       
+
         [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,
+            Vector3 offsetB, Quaternion orientationA, Quaternion orientationB, in BodyVelocity velocityA, in BodyVelocity velocityB,
             float speculativeMargin, float maximumExpansion,
             in PairContinuation continuation)
         {
@@ -223,9 +224,9 @@ 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,
+        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);
@@ -266,24 +267,27 @@ 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, TShapeB>(TShapeA shapeA, TShapeB shapeB, in Vector3 offsetB, in Quaternion orientationA, in Quaternion orientationB, float speculativeMargin, int pairId)
+        public unsafe void Add<TShapeA, TShapeB>(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
             //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<TShapeA>(), Unsafe.SizeOf<TShapeB>(), Unsafe.AsPointer(ref shapeA), Unsafe.AsPointer(ref shapeB),
+            Add(TShapeA.TypeId, TShapeB.TypeId, Unsafe.SizeOf<TShapeA>(), Unsafe.SizeOf<TShapeB>(), Unsafe.AsPointer(ref shapeA), Unsafe.AsPointer(ref shapeB),
                 offsetB, orientationA, orientationB, speculativeMargin, pairId);
         }
 
+        /// <summary>
+        /// Forces any remaining partial batches to execute and disposes the batcher.
+        /// </summary>
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
         public void Flush()
         {
@@ -296,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);
@@ -312,8 +325,20 @@ public void Flush()
             CompoundMeshReductions.Dispose(Pool);
         }
 
-        public unsafe void ProcessConvexResult(ref ConvexContactManifold manifold, ref PairContinuation continuation)
+        /// <summary>
+        /// Reports the result of a convex collision test to the callbacks and, if necessary, to any continuations for postprocessing.
+        /// </summary>
+        /// <remarks>Unless you're building custom compound collision pairs or adding new contact processing continuations, you can safely ignore this.</remarks>
+        /// <param name="manifold">Contacts detected for the pair.</param>
+        /// <param name="continuation">Continuation describing the pair and what to do with it.</param>
+        public 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.
@@ -345,5 +370,49 @@ public unsafe void ProcessConvexResult(ref ConvexContactManifold manifold, ref P
 
             }
         }
+
+        /// <summary>
+        /// Reports the zero result of a convex collision test to the callbacks and, if necessary, to any continuations for postprocessing.
+        /// </summary>
+        /// <remarks>Unless you're building custom compound collision pairs or adding new contact processing continuations, you can safely ignore this.</remarks>
+        /// <param name="continuation">Continuation describing the pair and what to do with it.</param>
+        public void ProcessEmptyResult(ref PairContinuation continuation)
+        {
+            Unsafe.SkipInit(out ConvexContactManifold manifold);
+            manifold.Count = 0;
+            ProcessConvexResult(ref manifold, ref continuation);
+        }
+
+
+        /// <summary>
+        /// Submits a subpair whose testing was blocked by user callback as complete to any relevant continuations.
+        /// </summary>
+        /// <remarks>Unless you're building custom compound collision pairs or adding new contact processing continuations, you can safely ignore this.</remarks>
+        /// <param name="continuation">Continuation describing the pair and what to do with it.</param>
+        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.
+            //That's different from the top level pair which should always report.
+            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..f11b8dfbd 100644
--- a/BepuPhysics/CollisionDetection/CollisionBatcherContinuations.cs
+++ b/BepuPhysics/CollisionDetection/CollisionBatcherContinuations.cs
@@ -1,21 +1,47 @@
 using BepuUtilities.Memory;
-using System;
-using System.Collections.Generic;
 using System.Diagnostics;
 using System.Runtime.CompilerServices;
-using System.Text;
 
 namespace BepuPhysics.CollisionDetection
 {
+    /// <summary>
+    /// Defines a type which includes information necessary to apply some form of post processing to a collision test result.
+    /// </summary>
     public interface ICollisionTestContinuation
     {
+        /// <summary>
+        /// Creates a collision test continuation with the given number of slots for subpairs.
+        /// </summary>
+        /// <param name="slots">Number of subpair slots to include in the continuation.</param>
+        /// <param name="pool">Pool to take resources from.</param>
         void Create(int slots, BufferPool pool);
 
-        unsafe void OnChildCompleted<TCallbacks>(ref PairContinuation report, ref ConvexContactManifold manifold, ref CollisionBatcher<TCallbacks> batcher)
+        /// <summary>
+        /// Handles what to do next when the child pair has finished execution and the resulting manifold is available.
+        /// </summary>
+        /// <typeparam name="TCallbacks">Type of the callbacks used in the batcher.</typeparam>
+        /// <param name="report">Continuation instance being considered.</param>
+        /// <param name="manifold">Contact manifold for the child pair.</param>
+        /// <param name="batcher">Collision batcher processing the pair.</param>
+        void OnChildCompleted<TCallbacks>(ref PairContinuation report, ref ConvexContactManifold manifold, ref CollisionBatcher<TCallbacks> batcher)
             where TCallbacks : struct, ICollisionCallbacks;
-        unsafe void OnChildCompletedEmpty<TCallbacks>(ref PairContinuation report, ref CollisionBatcher<TCallbacks> batcher)
+        /// <summary>
+        /// Handles what to do next when the child pair was rejected for testing, and no manifold exists.
+        /// </summary>
+        /// <typeparam name="TCallbacks">Type of the callbacks used in the batcher.</typeparam>
+        /// <param name="report">Continuation instance being considered.</param>
+        /// <param name="batcher">Collision batcher processing the pair.</param>
+        void OnUntestedChildCompleted<TCallbacks>(ref PairContinuation report, ref CollisionBatcher<TCallbacks> batcher)
             where TCallbacks : struct, ICollisionCallbacks;
-        unsafe bool TryFlush<TCallbacks>(int pairId, ref CollisionBatcher<TCallbacks> batcher)
+
+        /// <summary>
+        /// Checks if the parent pair is complete and should be flushed.
+        /// </summary>
+        /// <typeparam name="TCallbacks">Type of the callbacks used in the batcher.</typeparam>
+        /// <param name="pairId">Id of the pair to attempt to flush.</param>
+        /// <param name="batcher">Collision batcher processing the pair.</param>
+        /// <returns>True if the pair was done and got flushed, false otherwise.</returns>
+        bool TryFlush<TCallbacks>(int pairId, ref CollisionBatcher<TCallbacks> batcher)
             where TCallbacks : struct, ICollisionCallbacks;
 
 
@@ -29,19 +55,19 @@ public enum CollisionContinuationType : byte
         /// <summary>
         /// Marks a pair as requiring no further processing before being reported to the user supplied continuations.
         /// </summary>
-        Direct,
+        Direct = 0,
         /// <summary>
         /// Marks a pair as part of a set of a higher (potentially multi-manifold) pair, potentially requiring contact reduction.
         /// </summary>
-        NonconvexReduction,
+        NonconvexReduction = 1,
         /// <summary>
         /// Marks a pair as a part of a set of mesh-convex collisions, potentially requiring mesh boundary smoothing.
         /// </summary>
-        MeshReduction,
+        MeshReduction = 2,
         /// <summary>
         /// Marks a pair as a part of a set of mesh-convex collisions spawned by a mesh-compound pair, potentially requiring mesh boundary smoothing.
         /// </summary>
-        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.
         ///// <summary>
@@ -57,19 +83,40 @@ public struct PairContinuation
         public int ChildA;
         public int ChildB;
         public uint Packed;
+
+        /// <summary>
+        /// Covers bits [0, 20) in the packed representation. Refers to the child pair index in a subtask generating collision task that generated this continuation.
+        /// </summary>
+        public const int ChildIndexBits = 20;
+        /// <summary>
+        /// 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.
+        /// </summary>
+        public const int ContinuationIndexBits = 10;
+        /// <summary>
+        /// Covers bits [30, 32) in the packed representation. Refers to which continuation processor should be used for this subpair.
+        /// </summary>
+        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)
         {
@@ -79,9 +126,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<T> where T : unmanaged, ICollisionTestContinuation
@@ -110,7 +157,7 @@ public ref T CreateContinuation(int slotsInContinuation, BufferPool pool, out in
         }
 
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public unsafe void ContributeChildToContinuation<TCallbacks>(ref PairContinuation continuation, ref ConvexContactManifold manifold, ref CollisionBatcher<TCallbacks> batcher)
+        public void ContributeChildToContinuation<TCallbacks>(ref PairContinuation continuation, ref ConvexContactManifold manifold, ref CollisionBatcher<TCallbacks> batcher)
             where TCallbacks : struct, ICollisionCallbacks
         {
             ref var slot = ref Continuations[continuation.Index];
@@ -122,6 +169,19 @@ public unsafe void ContributeChildToContinuation<TCallbacks>(ref PairContinuatio
             }
         }
 
+        [MethodImpl(MethodImplOptions.AggressiveInlining)]
+        public void ContributeUntestedChildToContinuation<TCallbacks>(ref PairContinuation continuation, ref CollisionBatcher<TCallbacks> 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/CollisionTaskRegistry.cs b/BepuPhysics/CollisionDetection/CollisionTaskRegistry.cs
index e388d55a8..d8353a7fd 100644
--- a/BepuPhysics/CollisionDetection/CollisionTaskRegistry.cs
+++ b/BepuPhysics/CollisionDetection/CollisionTaskRegistry.cs
@@ -5,10 +5,19 @@
 
 namespace BepuPhysics.CollisionDetection
 {
+    /// <summary>
+    /// Callbacks invoked by a <see cref="CollisionBatcher{TCallbacks}"/>.
+    /// </summary>
     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.
-        unsafe void OnPairCompleted<TManifold>(int pairId, ref TManifold manifold) where TManifold : unmanaged, IContactManifold<TManifold>;
+        /// <summary>
+        /// Called when a pair submitted to a collision batcher has finished collision detection.
+        /// </summary>
+        /// <typeparam name="TManifold">Type of the contact manifold generated by collision detection.</typeparam>
+        /// <param name="pairId">Id of the pair that completed.</param>
+        /// <param name="manifold">Contact manifold generated by collision testing.</param>
+        void OnPairCompleted<TManifold>(int pairId, ref TManifold manifold) where TManifold : unmanaged, IContactManifold<TManifold>;
 
         /// <summary>
         /// Provides control over subtask generated results before they are reported to the parent task.
@@ -17,7 +26,7 @@ public interface ICollisionCallbacks
         /// <param name="childA">Index of the child belonging to collidable A in the subpair under consideration.</param>
         /// <param name="childB">Index of the child belonging to collidable B in the subpair under consideration.</param>
         /// <param name="manifold">Manifold of the child pair to configure.</param>
-        unsafe void OnChildPairCompleted(int pairId, int childA, int childB, ref ConvexContactManifold manifold);
+        void OnChildPairCompleted(int pairId, int childA, int childB, ref ConvexContactManifold manifold);
 
         /// <summary>
         /// Checks whether further collision testing should be performed for a given subtask.
@@ -29,7 +38,9 @@ public interface ICollisionCallbacks
         bool AllowCollisionTesting(int pairId, int childA, int childB);
     }
 
-
+    /// <summary>
+    /// Parent type of tasks which handle collision tests between batches of shapes of a particular type.
+    /// </summary>
     public abstract class CollisionTask
     {
         /// <summary>
@@ -63,13 +74,14 @@ public abstract class CollisionTask
         /// <typeparam name="TCallbacks">Type of the callbacks used to handle results of collision tasks.</typeparam>
         /// <param name="batcher">Batcher responsible for the invocation.</param>
         /// <param name="batch">Batch of pairs to test.</param>
-        /// <param name="continuations">Continuations to invoke upon completion of a top level pair.</param>
-        /// <param name="filters">Filters to use to influence execution of the collision tasks.</param>
         public abstract void ExecuteBatch<TCallbacks>(ref UntypedList batch, ref CollisionBatcher<TCallbacks> batcher)
             where TCallbacks : struct, ICollisionCallbacks;
 
     }
 
+    /// <summary>
+    /// Describes the data requirements for a collision pair type in a <see cref="CollisionBatcher{TCallbacks}"/>.
+    /// </summary>
     public enum CollisionTaskPairType
     {
         /// <summary>
@@ -95,20 +107,43 @@ public enum CollisionTaskPairType
 
     }
 
+    /// <summary>
+    /// Metadata about a collision task.
+    /// </summary>
     public struct CollisionTaskReference
     {
+        /// <summary>
+        /// Index of the task in the registry.
+        /// </summary>
         public int TaskIndex;
+        /// <summary>
+        /// Number of pairs to accumulate in a batch before dispatching tests.
+        /// </summary>
         public int BatchSize;
+        /// <summary>
+        /// The type id that is expected to come first in the collision pair.
+        /// </summary>
         public int ExpectedFirstTypeId;
+        /// <summary>
+        /// Data requirements for the collision pair type in a <see cref="CollisionBatcher{TCallbacks}"/>.
+        /// </summary>
         public CollisionTaskPairType PairType;
     }
 
+    /// <summary>
+    /// Registry of collision tasks used to handle various shape pair types.
+    /// </summary>
     public class CollisionTaskRegistry
     {
         CollisionTaskReference[][] topLevelMatrix;
         internal CollisionTask[] tasks;
         int count;
-
+        
+        /// <summary>
+        /// Gets the collision task associated with a task index.
+        /// </summary>
+        /// <param name="taskIndex">Task index to look up.</param>
+        /// <returns>Task associated with the task index.</returns>
         public CollisionTask this[int taskIndex]
         {
             [MethodImpl(MethodImplOptions.AggressiveInlining)]
@@ -118,6 +153,10 @@ public CollisionTask this[int taskIndex]
             }
         }
 
+        /// <summary>
+        /// Creates a new collision task registry.
+        /// </summary>
+        /// <param name="initialShapeCount">Initial number of shape types to allocate space for in the registry.</param>
         public CollisionTaskRegistry(int initialShapeCount = 9)
         {
             ResizeMatrix(initialShapeCount);
@@ -137,6 +176,11 @@ void ResizeMatrix(int newSize)
             }
         }
 
+        /// <summary>
+        /// Registers a collision task.
+        /// </summary>
+        /// <param name="task">Task to register.</param>
+        /// <returns>Index of the task in the registry.</returns>
         public int Register(CollisionTask task)
         {
             //Some tasks can generate tasks. Note that this can only be one level deep; nesting compounds is not allowed.
@@ -208,17 +252,31 @@ public int Register(CollisionTask task)
             return index;
 
         }
+
+        /// <summary>
+        /// Gets metadata about the task associated with a shape type pair.
+        /// </summary>
+        /// <param name="topLevelTypeA">Type index of the first shape.</param>
+        /// <param name="topLevelTypeB">Type index of the second shape.</param>
+        /// <returns>Reference to the metadata for the task.</returns>
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
         public ref CollisionTaskReference GetTaskReference(int topLevelTypeA, int topLevelTypeB)
         {
             return ref topLevelMatrix[topLevelTypeA][topLevelTypeB];
         }
+
+        /// <summary>
+        /// Gets metadata about the task associated with a shape type pair.
+        /// </summary>
+        /// <typeparam name="TShapeA">Type of the first shape.</typeparam>
+        /// <typeparam name="TShapeB">Type of the second shape.</typeparam>
+        /// <returns>Reference to the metadata for the task.</returns>
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
         public ref CollisionTaskReference GetTaskReference<TShapeA, TShapeB>()
             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 349d99aa4..a3dba5c22 100644
--- a/BepuPhysics/CollisionDetection/CollisionTasks/BoxConvexHullTester.cs
+++ b/BepuPhysics/CollisionDetection/CollisionTasks/BoxConvexHullTester.cs
@@ -1,7 +1,7 @@
 using BepuPhysics.Collidables;
 using BepuUtilities;
+using BepuUtilities.Memory;
 using System;
-using System.Diagnostics;
 using System.Numerics;
 using System.Runtime.CompilerServices;
 
@@ -9,9 +9,9 @@ namespace BepuPhysics.CollisionDetection.CollisionTasks
 {
     public struct BoxConvexHullTester : IPairTester<BoxWide, ConvexHullWide, Convex4ContactManifoldWide>
     {
-        public int BatchSize => 16;
+        public static int BatchSize => 16;
 
-        public unsafe void Test(ref BoxWide a, ref ConvexHullWide b, ref Vector<float> 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<float> 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);
@@ -28,7 +28,7 @@ public unsafe void Test(ref BoxWide a, ref ConvexHullWide b, ref Vector<float> s
             initialNormal.Z = Vector.ConditionalSelect(useInitialFallback, Vector<float>.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;
@@ -80,18 +80,39 @@ public unsafe void Test(ref BoxWide a, ref ConvexHullWide b, ref Vector<float> 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<float>.Count];
+            Vector3* slotLocalNormals = stackalloc Vector3[Vector<float>.Count];
+            Buffer<HullVertexIndex>* hullVertexIndices = stackalloc Buffer<HullVertexIndex>[Vector<float>.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 +140,7 @@ public unsafe void Test(ref BoxWide a, ref ConvexHullWide b, ref Vector<float> 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 +258,7 @@ public unsafe void Test(ref BoxWide a, ref ConvexHullWide b, ref Vector<float> 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 +269,7 @@ public unsafe void Test(ref BoxWide a, ref ConvexHullWide b, ref Vector<float> 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 +284,7 @@ public unsafe void Test(ref BoxWide a, ref ConvexHullWide b, ref Vector<float> 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 +320,7 @@ public unsafe void Test(ref BoxWide a, ref ConvexHullWide b, ref Vector<float> s
                         candidate.Y = projectedTangentY.X;
                         candidate.FeatureId = 0;
                     }
-                    if (candidateCount == 8)
+                    if (candidateCount == maximumContactCount)
                         goto SkipVertexCandidates;
                     if (maximumVertexContainmentDots.Y <= 0)
                     {
@@ -308,7 +329,7 @@ public unsafe void Test(ref BoxWide a, ref ConvexHullWide b, ref Vector<float> s
                         candidate.Y = projectedTangentY.Y;
                         candidate.FeatureId = 1;
                     }
-                    if (candidateCount == 8)
+                    if (candidateCount == maximumContactCount)
                         goto SkipVertexCandidates;
                     if (maximumVertexContainmentDots.Z <= 0)
                     {
@@ -317,7 +338,7 @@ public unsafe void Test(ref BoxWide a, ref ConvexHullWide b, ref Vector<float> 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;
@@ -338,12 +359,12 @@ public unsafe void Test(ref BoxWide a, ref ConvexHullWide b, ref Vector<float> s
             Matrix3x3Wide.TransformWithoutOverlap(localNormal, hullOrientation, out manifold.Normal);
         }
 
-        public void Test(ref BoxWide a, ref ConvexHullWide b, ref Vector<float> speculativeMargin, ref Vector3Wide offsetB, ref QuaternionWide orientationB, int pairCount, out Convex4ContactManifoldWide manifold)
+        public static void Test(ref BoxWide a, ref ConvexHullWide b, ref Vector<float> 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<float> speculativeMargin, ref Vector3Wide offsetB, int pairCount, out Convex4ContactManifoldWide manifold)
+        public static void Test(ref BoxWide a, ref ConvexHullWide b, ref Vector<float> 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 4cefb5331..51f214ce2 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;
 
@@ -11,7 +9,7 @@ namespace BepuPhysics.CollisionDetection.CollisionTasks
     using DepthRefiner = DepthRefiner<Cylinder, CylinderWide, CylinderSupportFinder, Box, BoxWide, BoxSupportFinder>;
     public struct BoxCylinderTester : IPairTester<BoxWide, CylinderWide, Convex4ContactManifoldWide>
     {
-        public int BatchSize => 16;
+        public static int BatchSize => 16;
 
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
         internal static void IntersectLineCircle(in Vector2Wide linePosition, in Vector2Wide lineDirection, in Vector<float> radius, out Vector<float> tMin, out Vector<float> tMax, out Vector<int> intersected)
@@ -57,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.
@@ -100,7 +98,7 @@ static void TryAddInteriorPoint(in Vector2Wide point, in Vector<int> featureId,
         }
 
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public unsafe void Test(
+        public static unsafe void Test(
             ref BoxWide a, ref CylinderWide b, ref Vector<float> speculativeMargin,
             ref Vector3Wide offsetB, ref QuaternionWide orientationA, ref QuaternionWide orientationB, int pairCount,
             out Convex4ContactManifoldWide manifold)
@@ -123,7 +121,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));
@@ -386,12 +384,12 @@ public unsafe void Test(
         }
 
 
-        public void Test(ref BoxWide a, ref CylinderWide b, ref Vector<float> speculativeMargin, ref Vector3Wide offsetB, ref QuaternionWide orientationB, int pairCount, out Convex4ContactManifoldWide manifold)
+        public static void Test(ref BoxWide a, ref CylinderWide b, ref Vector<float> 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<float> speculativeMargin, ref Vector3Wide offsetB, int pairCount, out Convex4ContactManifoldWide manifold)
+        public static void Test(ref BoxWide a, ref CylinderWide b, ref Vector<float> 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 af1e6fafb..707df4191 100644
--- a/BepuPhysics/CollisionDetection/CollisionTasks/BoxPairTester.cs
+++ b/BepuPhysics/CollisionDetection/CollisionTasks/BoxPairTester.cs
@@ -4,13 +4,12 @@
 using System.Diagnostics;
 using System.Numerics;
 using System.Runtime.CompilerServices;
-using static BepuUtilities.GatherScatter;
 
 namespace BepuPhysics.CollisionDetection.CollisionTasks
 {
     public struct BoxPairTester : IPairTester<BoxWide, BoxWide, Convex4ContactManifoldWide>
     {
-        public int BatchSize => 32;
+        public static int BatchSize => 32;
 
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
         static void TestEdgeEdge(
@@ -103,7 +102,7 @@ static void Select(
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
         static void AddBoxAVertex(in Vector3Wide vertex, in Vector<int> featureId, in Vector3Wide faceNormalB, in Vector3Wide contactNormal, in Vector<float> inverseContactNormalDotFaceNormalB,
             in Vector3Wide faceCenterB, in Vector3Wide faceTangentBX, in Vector3Wide faceTangentBY, in Vector<float> halfSpanBX, in Vector<float> halfSpanBY,
-            ref ManifoldCandidate candidates, ref Vector<int> candidateCount, int pairCount)
+            ref ManifoldCandidate candidates, ref Vector<int> candidateCount, int pairCount, in Vector<int> 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 +126,7 @@ static void AddBoxAVertex(in Vector3Wide vertex, in Vector<int> 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<int>(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 +135,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<int> f00, in Vector<int> f01, in Vector<int> f10, in Vector<int> f11,
-            ref ManifoldCandidate candidates, ref Vector<int> candidateCount, int pairCount)
+            ref ManifoldCandidate candidates, ref Vector<int> candidateCount, int pairCount, in Vector<int> allowContacts)
         {
             Vector3Wide.Dot(faceNormalB, contactNormal, out var normalDot);
 #if DEBUG
@@ -150,13 +149,13 @@ private static void AddBoxAVertices(in Vector3Wide faceCenterB, in Vector3Wide f
             var inverseContactNormalDotFaceNormalB = Vector.ConditionalSelect(Vector.GreaterThan(Vector.Abs(normalDot), new Vector<float>(1e-10f)), Vector<float>.One / normalDot, new Vector<float>(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 +220,23 @@ private static void ClipBoxBEdgesAgainstBoxAFace(in Vector3Wide edgeStartB0, in
 
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
         private static void AddContactsForEdge(in Vector<float> min, in ManifoldCandidate minCandidate, in Vector<float> max, in ManifoldCandidate maxCandidate, in Vector<float> halfSpanB,
-            in Vector<float> epsilon, ref ManifoldCandidate candidates, ref Vector<int> candidateCount, int pairCount)
+            in Vector<float> epsilon, ref ManifoldCandidate candidates, ref Vector<int> candidateCount, in Vector<int> allowContacts, int pairCount)
         {
             //If -halfSpan<min<halfSpan && (max-min)>epsilon 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 +245,7 @@ private static void CreateEdgeContacts(
             in Vector3Wide faceCenterB, in Vector3Wide faceTangentBX, in Vector3Wide faceTangentBY, in Vector<float> halfSpanBX, in Vector<float> halfSpanBY,
             in Vector3Wide vertexA00, in Vector3Wide vertexA11, in Vector3Wide faceTangentAX, in Vector3Wide faceTangentAY, in Vector3Wide contactNormal,
             in Vector<int> featureIdX0, in Vector<int> featureIdX1, in Vector<int> featureIdY0, in Vector<int> featureIdY1,
-            in Vector<float> epsilonScale, ref ManifoldCandidate candidates, ref Vector<int> candidateCount, int pairCount)
+            in Vector<float> epsilonScale, ref ManifoldCandidate candidates, ref Vector<int> candidateCount, int pairCount, in Vector<int> 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 +287,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 +296,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 +305,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,14 +314,14 @@ 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);
         }
 
 
 
 
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public unsafe void Test(
+        public static unsafe void Test(
             ref BoxWide a, ref BoxWide b, ref Vector<float> speculativeMargin,
             ref Vector3Wide offsetB, ref QuaternionWide orientationA, ref QuaternionWide orientationB, int pairCount,
             out Convex4ContactManifoldWide manifold)
@@ -378,6 +381,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);
 
+            var activeLanes = BundleIndexing.CreateMaskForCountInBundle(pairCount);
+            var minimumDepth = -speculativeMargin;
+            var allowContacts = Vector.BitwiseAnd(activeLanes, Vector.GreaterThanOrEqual(depth, minimumDepth));
+            if (Vector.EqualsAll(allowContacts, Vector<int>.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<float>.Zero);
@@ -486,7 +500,7 @@ public unsafe void Test(
             var edgeIdBY1 = axisIdBX * three + twiceAxisIdBY + axisZEdgeIdContribution;
             var candidateCount = Vector<int>.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 +512,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<float>(-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<float>(-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);
@@ -524,12 +538,12 @@ private static void TransformContactToManifold(
             manifoldFeatureId = rawContact.FeatureId;
         }
 
-        public void Test(ref BoxWide a, ref BoxWide b, ref Vector<float> speculativeMargin, ref Vector3Wide offsetB, ref QuaternionWide orientationB, int pairCount, out Convex4ContactManifoldWide manifold)
+        public static void Test(ref BoxWide a, ref BoxWide b, ref Vector<float> 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<float> speculativeMargin, ref Vector3Wide offsetB, int pairCount, out Convex4ContactManifoldWide manifold)
+        public static void Test(ref BoxWide a, ref BoxWide b, ref Vector<float> 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 3c1af2f06..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<BoxWide, TriangleWide, Convex4ContactManifoldWide>
     {
-        public int BatchSize => 32;
+        public static int BatchSize => 32;
 
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
         static void GetDepthForInterval(in Vector<float> boxExtreme, in Vector<float> a, in Vector<float> b, in Vector<float> c, out Vector<float> 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<float> speculativeMargin,
             ref Vector3Wide offsetB, ref QuaternionWide orientationA, ref QuaternionWide orientationB, int pairCount,
             out Convex4ContactManifoldWide manifold)
@@ -333,10 +333,231 @@ 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);
 
+            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.
+            //var testVertexNormals = Vector.BitwiseAnd(activeLanes, Vector.LessThan(depth, Vector<float>.Zero));
+            //if (Vector.LessThanAny(testVertexNormals, Vector<int>.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<float>.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<float>.One / (ab.X * ab.X + ab.Y * ab.Y + ab.Z * ab.Z);
+            //    var inverseLengthSquaredBC = Vector<float>.One / (bc.X * bc.X + bc.Y * bc.Y + bc.Z * bc.Z);
+            //    var inverseLengthSquaredCA = Vector<float>.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<float>.Zero, Vector.Min(Vector<float>.One, (abToFaceDot - edgeABDotBoxEdgeX - edgeABDotBoxEdgeY) * inverseLengthSquaredAB));
+            //    var tClosestOnAB01 = Vector.Max(Vector<float>.Zero, Vector.Min(Vector<float>.One, (abToFaceDot - edgeABDotBoxEdgeX + edgeABDotBoxEdgeY) * inverseLengthSquaredAB));
+            //    var tClosestOnAB10 = Vector.Max(Vector<float>.Zero, Vector.Min(Vector<float>.One, (abToFaceDot + edgeABDotBoxEdgeX - edgeABDotBoxEdgeY) * inverseLengthSquaredAB));
+            //    var tClosestOnAB11 = Vector.Max(Vector<float>.Zero, Vector.Min(Vector<float>.One, (abToFaceDot + edgeABDotBoxEdgeX + edgeABDotBoxEdgeY) * inverseLengthSquaredAB));
+            //    var tClosestOnBC00 = Vector.Max(Vector<float>.Zero, Vector.Min(Vector<float>.One, (bcToFaceDot - edgeBCDotBoxEdgeX - edgeBCDotBoxEdgeY) * inverseLengthSquaredBC));
+            //    var tClosestOnBC01 = Vector.Max(Vector<float>.Zero, Vector.Min(Vector<float>.One, (bcToFaceDot - edgeBCDotBoxEdgeX + edgeBCDotBoxEdgeY) * inverseLengthSquaredBC));
+            //    var tClosestOnBC10 = Vector.Max(Vector<float>.Zero, Vector.Min(Vector<float>.One, (bcToFaceDot + edgeBCDotBoxEdgeX - edgeBCDotBoxEdgeY) * inverseLengthSquaredBC));
+            //    var tClosestOnBC11 = Vector.Max(Vector<float>.Zero, Vector.Min(Vector<float>.One, (bcToFaceDot + edgeBCDotBoxEdgeX + edgeBCDotBoxEdgeY) * inverseLengthSquaredBC));
+            //    var tClosestOnCA00 = Vector.Max(Vector<float>.Zero, Vector.Min(Vector<float>.One, (caToFaceDot - edgeCADotBoxEdgeX - edgeCADotBoxEdgeY) * inverseLengthSquaredCA));
+            //    var tClosestOnCA01 = Vector.Max(Vector<float>.Zero, Vector.Min(Vector<float>.One, (caToFaceDot - edgeCADotBoxEdgeX + edgeCADotBoxEdgeY) * inverseLengthSquaredCA));
+            //    var tClosestOnCA10 = Vector.Max(Vector<float>.Zero, Vector.Min(Vector<float>.One, (caToFaceDot + edgeCADotBoxEdgeX - edgeCADotBoxEdgeY) * inverseLengthSquaredCA));
+            //    var tClosestOnCA11 = Vector.Max(Vector<float>.Zero, Vector.Min(Vector<float>.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<float>(-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<float>(1e-6f)), depthCandidate, new Vector<float>(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.
             Vector3Wide.Dot(localNormal, triangleNormal, out var normalDot);
-            ManifoldCandidateHelper.CreateActiveMask(pairCount, out var activeLanes);
-            var allowContacts = Vector.BitwiseAnd(Vector.GreaterThanOrEqual(normalDot, new Vector<float>(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<float>(TriangleWide.BackfaceNormalDotRejectionThreshold))),
+                Vector.BitwiseAnd(Vector.GreaterThanOrEqual(depth, minimumDepth), activeLanes));
             if (Vector.EqualsAll(allowContacts, Vector<int>.Zero))
             {
                 //All lanes are inactive; early out.
@@ -407,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.
@@ -447,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<float>.One / faceNormalDotNormal, faceCenterBToFaceCenterA, triangleTangentX, triangleTangentY, epsilonScale, -speculativeMargin, pairCount,
+            ManifoldCandidateHelper.Reduce(ref candidates, candidateCount, 6, boxFaceNormal, Vector<float>.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);
 
@@ -483,12 +701,12 @@ private static void TransformContactToManifold(
             manifoldFeatureId = rawContact.FeatureId;
         }
 
-        public void Test(ref BoxWide a, ref TriangleWide b, ref Vector<float> speculativeMargin, ref Vector3Wide offsetB, ref QuaternionWide orientationB, int pairCount, out Convex4ContactManifoldWide manifold)
+        public static void Test(ref BoxWide a, ref TriangleWide b, ref Vector<float> 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<float> speculativeMargin, ref Vector3Wide offsetB, int pairCount, out Convex4ContactManifoldWide manifold)
+        public static void Test(ref BoxWide a, ref TriangleWide b, ref Vector<float> 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 b860d480c..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<CapsuleWide, BoxWide, Convex2ContactManifoldWide>
     {
-        public int BatchSize => 32;
+        public static int BatchSize => 32;
 
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
         internal static void Prepare(
@@ -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.)
@@ -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<float> 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<float>(1e-7f) * a.HalfLength));
         }
 
-        public void Test(ref CapsuleWide a, ref BoxWide b, ref Vector<float> speculativeMargin, ref Vector3Wide offsetB, ref QuaternionWide orientationB, int pairCount, out Convex2ContactManifoldWide manifold)
+        public static void Test(ref CapsuleWide a, ref BoxWide b, ref Vector<float> 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<float> speculativeMargin, ref Vector3Wide offsetB, int pairCount, out Convex2ContactManifoldWide manifold)
+        public static void Test(ref CapsuleWide a, ref BoxWide b, ref Vector<float> 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 d736224fe..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;
 
@@ -9,9 +8,9 @@ namespace BepuPhysics.CollisionDetection.CollisionTasks
 {
     public struct CapsuleConvexHullTester : IPairTester<CapsuleWide, ConvexHullWide, Convex2ContactManifoldWide>
     {
-        public int BatchSize => 16;
+        public static int BatchSize => 16;
 
-        public void Test(ref CapsuleWide a, ref ConvexHullWide b, ref Vector<float> 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<float> 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);
@@ -28,7 +27,7 @@ public void Test(ref CapsuleWide a, ref ConvexHullWide b, ref Vector<float> spec
             initialNormal.Z = Vector.ConditionalSelect(useInitialFallback, Vector<float>.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;
@@ -179,12 +178,12 @@ public void Test(ref CapsuleWide a, ref ConvexHullWide b, ref Vector<float> spec
 
         }
 
-        public void Test(ref CapsuleWide a, ref ConvexHullWide b, ref Vector<float> speculativeMargin, ref Vector3Wide offsetB, ref QuaternionWide orientationB, int pairCount, out Convex2ContactManifoldWide manifold)
+        public static void Test(ref CapsuleWide a, ref ConvexHullWide b, ref Vector<float> 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<float> speculativeMargin, ref Vector3Wide offsetB, int pairCount, out Convex2ContactManifoldWide manifold)
+        public static void Test(ref CapsuleWide a, ref ConvexHullWide b, ref Vector<float> 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 868f753b9..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;
@@ -9,7 +8,7 @@ namespace BepuPhysics.CollisionDetection.CollisionTasks
 {
     public struct CapsuleCylinderTester : IPairTester<CapsuleWide, CylinderWide, Convex2ContactManifoldWide>
     {
-        public int BatchSize => 32;
+        public static int BatchSize => 32;
 
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
         private static void Bounce(in Vector3Wide lineOrigin, in Vector3Wide lineDirection, in Vector<float> t, in CylinderWide b, in Vector<float> radiusSquared, out Vector3Wide p, out Vector3Wide clamped)
@@ -132,7 +131,7 @@ public static void GetClosestPointsBetweenSegments(in Vector3Wide da, in Vector3
         }
 
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public void Test(ref CapsuleWide a, ref CylinderWide b, ref Vector<float> 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<float> speculativeMargin, ref Vector3Wide offsetB, ref QuaternionWide orientationA, ref QuaternionWide orientationB, int pairCount, out Convex2ContactManifoldWide manifold)
         {
             //Potential normal generators:
             //Capsule endpoint vs cylinder cap plane :
@@ -159,7 +158,7 @@ public void Test(ref CapsuleWide a, ref CylinderWide b, ref Vector<float> 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<float>(1e-12f));
@@ -167,7 +166,8 @@ public void Test(ref CapsuleWide a, ref CylinderWide b, ref Vector<float> 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<float>.One / distanceFromCylinderToLineSegment, out localNormal);
             var depth = Vector.ConditionalSelect(internalLineSegmentIntersected, new Vector<float>(float.MaxValue), -distanceFromCylinderToLineSegment);
-
+            var negativeMargin = -speculativeMargin;
+            inactiveLanes = Vector.BitwiseOr(Vector.LessThan(depth + a.Radius, negativeMargin), inactiveLanes);
             if (Vector.LessThanAny(Vector.AndNot(internalLineSegmentIntersected, inactiveLanes), Vector<int>.Zero))
             {
                 //At least one lane is intersecting deeply, so we need to examine the other possible normals.
@@ -207,7 +207,7 @@ public void Test(ref CapsuleWide a, ref CylinderWide b, ref Vector<float> 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);
+            inactiveLanes = Vector.BitwiseOr(Vector.LessThan(depth, negativeMargin), inactiveLanes);
             if (Vector.LessThanAll(inactiveLanes, Vector<int>.Zero))
             {
                 //All lanes have a depth which cannot create any contacts due to the speculative margin. We can early out.
@@ -223,7 +223,7 @@ public void Test(ref CapsuleWide a, ref CylinderWide b, ref Vector<float> 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<float>(0.70710678118f));
+            var useCapContacts = Vector.AndNot(Vector.GreaterThan(Vector.Abs(localNormal.Y), new Vector<float>(0.70710678118f)), inactiveLanes);
 
             //First, assume non-cap contacts.
             //Phrase the problem as a segment-segment test.
@@ -248,7 +248,7 @@ public void Test(ref CapsuleWide a, ref CylinderWide b, ref Vector<float> specul
 
             var contactCount = Vector.ConditionalSelect(Vector.LessThan(Vector.Abs(contactTMax - contactTMin), b.HalfLength * new Vector<float>(1e-5f)), Vector<int>.One, new Vector<int>(2));
 
-            if (Vector.LessThanAny(Vector.AndNot(useCapContacts, inactiveLanes), Vector<int>.Zero))
+            if (Vector.LessThanAny(useCapContacts, Vector<int>.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 +329,8 @@ public void Test(ref CapsuleWide a, ref CylinderWide b, ref Vector<float> specul
             //In this case, both contact positions should be extremely close together anyway.
             var collapse = Vector.LessThan(Vector.Abs(faceNormalADotLocalNormal), new Vector<float>(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<int>(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<int>(2)), collapse), Vector.GreaterThanOrEqual(manifold.Depth1, negativeMargin)), inactiveLanes);
 
             //Push the contacts into world space.
             Matrix3x3Wide.TransformWithoutOverlap(localNormal, worldRB, out manifold.Normal);
@@ -345,12 +344,12 @@ public void Test(ref CapsuleWide a, ref CylinderWide b, ref Vector<float> specul
         }
 
 
-        public void Test(ref CapsuleWide a, ref CylinderWide b, ref Vector<float> speculativeMargin, ref Vector3Wide offsetB, ref QuaternionWide orientationB, int pairCount, out Convex2ContactManifoldWide manifold)
+        public static void Test(ref CapsuleWide a, ref CylinderWide b, ref Vector<float> 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<float> speculativeMargin, ref Vector3Wide offsetB, int pairCount, out Convex2ContactManifoldWide manifold)
+        public static void Test(ref CapsuleWide a, ref CylinderWide b, ref Vector<float> 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 8cb066fe4..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<CapsuleWide, CapsuleWide, Convex2ContactManifoldWide>
     {
-        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<float> speculativeMargin,
             ref Vector3Wide offsetB, ref QuaternionWide orientationA, ref QuaternionWide orientationB, int pairCount,
             out Convex2ContactManifoldWide manifold)
@@ -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);
@@ -128,12 +128,12 @@ public void Test(
             //Worth looking into later.
         }
 
-        public void Test(ref CapsuleWide a, ref CapsuleWide b, ref Vector<float> speculativeMargin, ref Vector3Wide offsetB, ref QuaternionWide orientationB, int pairCount, out Convex2ContactManifoldWide manifold)
+        public static void Test(ref CapsuleWide a, ref CapsuleWide b, ref Vector<float> 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<float> speculativeMargin, ref Vector3Wide offsetB, int pairCount, out Convex2ContactManifoldWide manifold)
+        public static void Test(ref CapsuleWide a, ref CapsuleWide b, ref Vector<float> 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 08d27c424..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;
 
@@ -9,7 +8,7 @@ namespace BepuPhysics.CollisionDetection.CollisionTasks
 {
     public struct CapsuleTriangleTester : IPairTester<CapsuleWide, TriangleWide, Convex2ContactManifoldWide>
     {
-        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,
@@ -55,22 +54,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<float>.Zero), ref fallbackNormal);
             Vector3Wide.LengthSquared(fallbackNormal, out var fallbackNormalLengthSquared);
-            var useFallbackNormal = Vector.LessThan(normalLengthSquared, new Vector<float>(1e-15f));
+            var useFallbackNormal = Vector.LessThan(normalLengthSquared, new Vector<float>(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<float>(1e-15f));
+            var useSecondFallbackNormal = Vector.LessThan(normalLengthSquared, new Vector<float>(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<float>.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
@@ -106,7 +109,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<float> speculativeMargin,
             ref Vector3Wide offsetB, ref QuaternionWide orientationA, ref QuaternionWide orientationB, int pairCount,
             out Convex2ContactManifoldWide manifold)
@@ -127,7 +130,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:
@@ -151,8 +154,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<float>.One / Vector.SquareRoot(faceNormalLengthSquared), out var faceNormal);
+            Vector3Wide.Length(acxab, out var faceNormalLength);
+            Vector3Wide.Scale(acxab, Vector<float>.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 +195,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<float>(SphereTriangleTester.BackfaceNormalDotRejectionThreshold));
-            if (Vector.EqualsAll(Vector.BitwiseAnd(collidingWithSolidSide, Vector.GreaterThanOrEqual(depth + a.Radius, -speculativeMargin)), Vector<int>.Zero))
+            var collidingWithSolidSide = Vector.GreaterThanOrEqual(localNormalDotFaceNormal, new Vector<float>(TriangleWide.BackfaceNormalDotRejectionThreshold));
+            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));
+            if (Vector.EqualsAll(allowContacts, Vector<int>.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<int>.Zero;
@@ -203,7 +210,8 @@ public void Test(
             Unsafe.SkipInit(out Vector3Wide b0);
             Unsafe.SkipInit(out Vector3Wide b1);
             Vector<int> contactCount;
-            if (Vector.EqualsAny(useEdge, new Vector<int>(-1)))
+            useEdge = Vector.BitwiseAnd(useEdge, allowContacts);
+            if (Vector.LessThanAny(useEdge, Vector<int>.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 +261,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<int>.One))
+            if (Vector.LessThanAny(Vector.BitwiseAnd(Vector.LessThanOrEqual(contactCount, Vector<int>.One), allowContacts), Vector<int>.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 +346,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<float>(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<int>.Zero);
-            manifold.Contact0Exists = Vector.BitwiseAnd(Vector.GreaterThan(contactCount, Vector<int>.Zero), Vector.GreaterThan(manifold.Depth0, negativeMargin));
-            manifold.Contact1Exists = Vector.BitwiseAnd(Vector.AndNot(Vector.Equals(contactCount, new Vector<int>(2)), collapse), Vector.GreaterThan(manifold.Depth1, negativeMargin));
+            manifold.Contact0Exists = Vector.BitwiseAnd(allowContacts, Vector.BitwiseAnd(Vector.GreaterThan(contactCount, Vector<int>.Zero), Vector.GreaterThan(manifold.Depth0, negativeMargin)));
+            manifold.Contact1Exists = Vector.BitwiseAnd(allowContacts, Vector.BitwiseAnd(Vector.AndNot(Vector.Equals(contactCount, new Vector<int>(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,
@@ -373,12 +380,12 @@ public void Test(
         }
 
 
-        public void Test(ref CapsuleWide a, ref TriangleWide b, ref Vector<float> speculativeMargin, ref Vector3Wide offsetB, ref QuaternionWide orientationB, int pairCount, out Convex2ContactManifoldWide manifold)
+        public static void Test(ref CapsuleWide a, ref TriangleWide b, ref Vector<float> 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<float> speculativeMargin, ref Vector3Wide offsetB, int pairCount, out Convex2ContactManifoldWide manifold)
+        public static void Test(ref CapsuleWide a, ref TriangleWide b, ref Vector<float> 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 0620f2874..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<TCompound, TMesh> : ICompoundPairContinuationHandler<CompoundMeshReduction>
-        where TCompound : ICompoundShape
-        where TMesh : IHomogeneousCompoundShape<Triangle, TriangleWide>
+        where TCompound : struct, ICompoundShape
+        where TMesh : struct, IHomogeneousCompoundShape<Triangle, TriangleWide>
     {
         public CollisionContinuationType CollisionContinuationType => CollisionContinuationType.CompoundMeshReduction;
 
@@ -23,6 +23,9 @@ public ref CompoundMeshReduction CreateContinuation<TCallbacks>(
             collisionBatcher.Pool.Take(pairOverlaps.Length, out continuation.QueryBounds);
             continuation.RegionCount = pairOverlaps.Length;
             continuation.MeshOrientation = pair.OrientationB;
+            continuation.Mesh = pair.B;
+            continuation.FindLocalOverlapsThunk = MeshReductionThunks<TMesh>.FindLocalOverlaps;
+            continuation.GetLocalChildThunk = MeshReductionThunks<TMesh>.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;
 
@@ -44,19 +47,19 @@ public ref CompoundMeshReduction CreateContinuation<TCallbacks>(
         }
 
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public unsafe void GetChildAData<TCallbacks>(ref CollisionBatcher<TCallbacks> collisionBatcher, ref CompoundMeshReduction continuation, in BoundsTestedPair pair, int childIndexA,
+        public void GetChildAData<TCallbacks>(ref CollisionBatcher<TCallbacks> collisionBatcher, ref CompoundMeshReduction continuation, in BoundsTestedPair pair, int childIndexA,
             out RigidPose childPoseA, out int childTypeA, out void* childShapeDataA)
             where TCallbacks : struct, ICollisionCallbacks
         {
             ref var compound = ref Unsafe.AsRef<TCompound>(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 _);
         }
 
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public unsafe void ConfigureContinuationChild<TCallbacks>(
+        public void ConfigureContinuationChild<TCallbacks>(
             ref CollisionBatcher<TCallbacks> 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
@@ -65,7 +68,7 @@ public unsafe void ConfigureContinuationChild<TCallbacks>(
             //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<TMesh>(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 6ea2163dd..247ccc1ef 100644
--- a/BepuPhysics/CollisionDetection/CollisionTasks/CompoundPairCollisionTask.cs
+++ b/BepuPhysics/CollisionDetection/CollisionTasks/CompoundPairCollisionTask.cs
@@ -1,17 +1,13 @@
 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
 {
     public interface ICompoundPairOverlapFinder
     {
-        void FindLocalOverlaps<TOverlapTestingOptions>(ref Buffer<BoundsTestedPair> pairs, int pairCount, BufferPool pool, Shapes shapes, float dt, out CompoundPairOverlaps overlaps) where TOverlapTestingOptions : unmanaged, IOverlapTestingOptions;
+        static abstract void FindLocalOverlaps(ref Buffer<BoundsTestedPair> pairs, int pairCount, BufferPool pool, Shapes shapes, float dt, out CompoundPairOverlaps overlaps);
     }
 
     public unsafe interface ICompoundPairContinuationHandler<TContinuation> where TContinuation : struct, ICollisionTestContinuation
@@ -39,9 +35,9 @@ public class CompoundPairCollisionTask<TCompoundA, TCompoundB, TOverlapFinder, T
     {
         public CompoundPairCollisionTask()
         {
-            BatchSize = 32;
-            ShapeTypeIndexA = default(TCompoundA).TypeId;
-            ShapeTypeIndexB = default(TCompoundB).TypeId;
+            BatchSize = 16;
+            ShapeTypeIndexA = TCompoundA.TypeId;
+            ShapeTypeIndexB = TCompoundB.TypeId;
             SubtaskGenerator = true;
             PairType = CollisionTaskPairType.BoundsTestedPair;
         }
@@ -49,15 +45,10 @@ public CompoundPairCollisionTask()
         public unsafe override void ExecuteBatch<TCallbacks>(ref UntypedList batch, ref CollisionBatcher<TCallbacks> batcher)
         {
             var pairs = batch.Buffer.As<BoundsTestedPair>();
-            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.
-            CompoundPairOverlaps overlaps;
-            if (continuationHandler.CollisionContinuationType == CollisionContinuationType.CompoundMeshReduction || continuationHandler.CollisionContinuationType == CollisionContinuationType.MeshReduction)
-                overlapFinder.FindLocalOverlaps<UseEpsilonBoundsExpansion>(ref pairs, batch.Count, batcher.Pool, batcher.Shapes, batcher.Dt, out overlaps);
-            else
-                overlapFinder.FindLocalOverlaps<DontUseEpsilonBoundsExpansion>(ref pairs, batch.Count, batcher.Pool, batcher.Shapes, batcher.Dt, out overlaps);
+            TOverlapFinder.FindLocalOverlaps(ref pairs, batch.Count, batcher.Pool, batcher.Shapes, batcher.Dt, out var overlaps);
 
             for (int pairIndex = 0; pairIndex < batch.Count; ++pairIndex)
             {
@@ -67,9 +58,10 @@ public unsafe override void ExecuteBatch<TCallbacks>(ref UntypedList batch, ref
                 {
                     totalOverlapCountForPair += pairOverlaps[j].Count;
                 }
+                ref var pair = ref pairs[pairIndex];
                 if (totalOverlapCountForPair > 0)
                 {
-                    ref var pair = ref pairs[pairIndex];
+                    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;
@@ -98,6 +90,11 @@ public unsafe override void ExecuteBatch<TCallbacks>(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))
@@ -120,11 +117,15 @@ public unsafe override void ExecuteBatch<TCallbacks>(ref UntypedList batch, ref
                             }
                             else
                             {
-                                continuation.OnChildCompletedEmpty(ref subpairContinuation, ref batcher);
+                                batcher.ProcessUntestedSubpairConvexResult(ref subpairContinuation);
                             }
                         }
                     }
                 }
+                else
+                {
+                    batcher.ProcessEmptyResult(ref pair.Continuation);
+                }
 
 
             }
diff --git a/BepuPhysics/CollisionDetection/CollisionTasks/CompoundPairContinuations.cs b/BepuPhysics/CollisionDetection/CollisionTasks/CompoundPairContinuations.cs
index 004d7bdac..600b7d8bb 100644
--- a/BepuPhysics/CollisionDetection/CollisionTasks/CompoundPairContinuations.cs
+++ b/BepuPhysics/CollisionDetection/CollisionTasks/CompoundPairContinuations.cs
@@ -19,19 +19,19 @@ public ref NonconvexReduction CreateContinuation<TCallbacks>(
         }
 
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public unsafe void GetChildAData<TCallbacks>(ref CollisionBatcher<TCallbacks> collisionBatcher, ref NonconvexReduction continuation, in BoundsTestedPair pair, int childIndexA,
+        public void GetChildAData<TCallbacks>(ref CollisionBatcher<TCallbacks> collisionBatcher, ref NonconvexReduction continuation, in BoundsTestedPair pair, int childIndexA,
             out RigidPose childPoseA, out int childTypeA, out void* childShapeDataA)
             where TCallbacks : struct, ICollisionCallbacks
         {
             ref var compoundA = ref Unsafe.AsRef<TCompoundA>(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 _);
         }
 
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public unsafe void ConfigureContinuationChild<TCallbacks>(
+        public void ConfigureContinuationChild<TCallbacks>(
             ref CollisionBatcher<TCallbacks> 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
@@ -43,7 +43,7 @@ public unsafe void ConfigureContinuationChild<TCallbacks>(
             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 a23fc5d0e..16d7adf46 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
 {
@@ -20,7 +17,7 @@ public struct CompoundPairOverlapFinder<TCompoundA, TCompoundB> : ICompoundPairO
         where TCompoundB : struct, IBoundsQueryableCompound
     {
 
-        public unsafe void FindLocalOverlaps<TOverlapTestingOptions>(ref Buffer<BoundsTestedPair> pairs, int pairCount, BufferPool pool, Shapes shapes, float dt, out CompoundPairOverlaps overlaps) where TOverlapTestingOptions : unmanaged, IOverlapTestingOptions
+        public static unsafe void FindLocalOverlaps(ref Buffer<BoundsTestedPair> pairs, int pairCount, BufferPool pool, Shapes shapes, float dt, out CompoundPairOverlaps overlaps)
         {
             var totalCompoundChildCount = 0;
             for (int i = 0; i < pairCount; ++i)
@@ -29,7 +26,18 @@ public unsafe void FindLocalOverlaps<TOverlapTestingOptions>(ref Buffer<BoundsTe
             }
             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> subpairData;
+            const int stackallocThreshold = 1024;
+            if (totalCompoundChildCount <= stackallocThreshold)
+            {
+                var memory = stackalloc SubpairData[totalCompoundChildCount];
+                subpairData = new Buffer<SubpairData>(memory, totalCompoundChildCount);
+            }
+            else
+            {
+                subpairData = new Buffer<SubpairData>(totalCompoundChildCount, pool);
+            }
             int nextSubpairIndex = 0;
             for (int i = 0; i < pairCount; ++i)
             {
@@ -56,10 +64,9 @@ public unsafe void FindLocalOverlaps<TOverlapTestingOptions>(ref Buffer<BoundsTe
             Unsafe.SkipInit(out Vector<float> maximumAllowedExpansion);
             Unsafe.SkipInit(out Vector<float> maximumRadius);
             Unsafe.SkipInit(out Vector<float> maximumAngularExpansion);
-            Unsafe.SkipInit(out RigidPoses localPosesA);
+            Unsafe.SkipInit(out RigidPoseWide localPosesA);
             Unsafe.SkipInit(out Vector3Wide mins);
             Unsafe.SkipInit(out Vector3Wide maxes);
-            Unsafe.SkipInit(out TOverlapTestingOptions overlapTestingOptions);
             for (int i = 0; i < totalCompoundChildCount; i += Vector<float>.Count)
             {
                 var count = totalCompoundChildCount - i;
@@ -80,7 +87,7 @@ public unsafe void FindLocalOverlaps<TOverlapTestingOptions>(ref Buffer<BoundsTe
                     Vector3Wide.WriteFirst(subpair.Pair->AngularVelocityB, ref GatherScatter.GetOffsetInstance(ref angularVelocityB, j));
                     Unsafe.Add(ref Unsafe.As<Vector<float>, float>(ref maximumAllowedExpansion), j) = subpair.Pair->MaximumExpansion;
 
-                    RigidPoses.WriteFirst(subpair.Child->LocalPose, ref GatherScatter.GetOffsetInstance(ref localPosesA, j));
+                    RigidPoseWide.WriteFirst((*subpair.Child).AsPose(), ref GatherScatter.GetOffsetInstance(ref localPosesA, j));
                 }
 
                 QuaternionWide.Conjugate(orientationB, out var toLocalB);
@@ -106,10 +113,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)
                 {
@@ -120,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<TCompoundB>(pairsToTest[0].Container).FindLocalOverlaps<CompoundPairOverlaps, ChildOverlapsCollection>(ref pairsToTest, pool, shapes, ref overlaps);
-
+            Unsafe.AsRef<TCompoundB>(pairsToTest[0].Container).FindLocalOverlaps<CompoundPairOverlaps, ChildOverlapsCollection>(ref pairsToTest, pool, shapes, ref overlaps);
+
+            if (subpairData.Length > stackallocThreshold)
+                subpairData.Dispose(pool);
         }
 
     }
diff --git a/BepuPhysics/CollisionDetection/CollisionTasks/CompoundPairOverlaps.cs b/BepuPhysics/CollisionDetection/CollisionTasks/CompoundPairOverlaps.cs
index 268d22f86..fb5c170b5 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
 {
@@ -20,14 +15,14 @@ public interface ICollisionTaskOverlaps<TSubpairOverlaps> where TSubpairOverlaps
         ref TSubpairOverlaps GetOverlapsForPair(int subpairIndex);
     }
 
-    public unsafe struct ChildOverlapsCollection : ICollisionTaskSubpairOverlaps
+    public struct ChildOverlapsCollection : ICollisionTaskSubpairOverlaps
     {
         public Buffer<int> 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/ConvexCollisionTask.cs b/BepuPhysics/CollisionDetection/CollisionTasks/ConvexCollisionTask.cs
index 5bdfcedb4..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
@@ -15,13 +13,13 @@ public interface IPairTester<TShapeWideA, TShapeWideB, TManifoldWideType>
         /// <summary>
         /// Gets the nubmer of pairs which would ideally be gathered together before executing a wide test.
         /// </summary>
-        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<float> 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<float> speculativeMargin, ref Vector3Wide offsetB, ref QuaternionWide orientationB, int pairCount, out TManifoldWideType manifold);
-        void Test(ref TShapeWideA a, ref TShapeWideB b, ref Vector<float> speculativeMargin, ref Vector3Wide offsetB, int pairCount, out TManifoldWideType manifold);
+        static abstract void Test(ref TShapeWideA a, ref TShapeWideB b, ref Vector<float> 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<float> speculativeMargin, ref Vector3Wide offsetB, ref QuaternionWide orientationB, int pairCount, out TManifoldWideType manifold);
+        static abstract void Test(ref TShapeWideA a, ref TShapeWideB b, ref Vector<float> speculativeMargin, ref Vector3Wide offsetB, int pairCount, out TManifoldWideType manifold);
     }
 
     public interface IContactManifoldWide
@@ -40,10 +38,10 @@ public class ConvexCollisionTask<TShapeA, TShapeWideA, TShapeB, TShapeWideB, TPa
     {
         public ConvexCollisionTask()
         {
-            BatchSize = default(TPairTester).BatchSize;
-            ShapeTypeIndexA = default(TShapeA).TypeId;
-            ShapeTypeIndexB = default(TShapeB).TypeId;
-            PairType = default(TPair).PairType;
+            BatchSize = TPairTester.BatchSize;
+            ShapeTypeIndexA = TShapeA.TypeId;
+            ShapeTypeIndexB = TShapeB.TypeId;
+            PairType = TPair.PairType;
         }
 
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
@@ -53,20 +51,19 @@ public override unsafe void ExecuteBatch<TCallbacks>(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];
-                aWide.Initialize(new RawBuffer(memory, aWide.InternalAllocationSize));
+                aWide.Initialize(new Buffer<byte>(memory, aWide.InternalAllocationSize));
             }
             if (bWide.InternalAllocationSize > 0)
             {
                 var memory = stackalloc byte[bWide.InternalAllocationSize];
-                bWide.Initialize(new RawBuffer(memory, bWide.InternalAllocationSize));
+                bWide.Initialize(new Buffer<byte>(memory, bWide.InternalAllocationSize));
             }
             TManifoldWide manifoldWide;
-            var defaultPairTester = default(TPairTester);
             var manifold = default(ConvexContactManifold);
 
             for (int i = 0; i < batch.Count; i += Vector<float>.Count)
@@ -82,59 +79,59 @@ public override unsafe void ExecuteBatch<TCallbacks>(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 2a88a7187..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
@@ -31,9 +26,9 @@ public class ConvexCompoundCollisionTask<TConvex, TCompound, TOverlapFinder, TCo
     {
         public ConvexCompoundCollisionTask()
         {
-            BatchSize = 32;
-            ShapeTypeIndexA = default(TConvex).TypeId;
-            ShapeTypeIndexB = default(TCompound).TypeId;
+            BatchSize = 16;
+            ShapeTypeIndexA = TConvex.TypeId;
+            ShapeTypeIndexB = TCompound.TypeId;
             SubtaskGenerator = true;
             PairType = CollisionTaskPairType.BoundsTestedPair;
         }
@@ -45,19 +40,19 @@ public unsafe override void ExecuteBatch<TCallbacks>(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<UseEpsilonBoundsExpansion>(ref pairs, batch.Count, batcher.Pool, batcher.Shapes, batcher.Dt, out overlaps);
-            else
-                overlapFinder.FindLocalOverlaps<DontUseEpsilonBoundsExpansion>(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);
                 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<TCompound>(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;
@@ -100,11 +95,15 @@ public unsafe override void ExecuteBatch<TCallbacks>(ref UntypedList batch, ref
                         }
                         else
                         {
-                            continuation.OnChildCompletedEmpty(ref subpairContinuation, ref batcher);
+                            batcher.ProcessUntestedSubpairConvexResult(ref subpairContinuation);
                         }
 
                     }
                 }
+                else
+                {
+                    batcher.ProcessEmptyResult(ref pair.Continuation);
+                }
             }
             overlaps.Dispose(batcher.Pool);
         }
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<TCallbacks>(
         {
             ref var compoundChild = ref Unsafe.AsRef<TCompound>(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/CollisionTasks/ConvexCompoundOverlapFinder.cs b/BepuPhysics/CollisionDetection/CollisionTasks/ConvexCompoundOverlapFinder.cs
index da5dfe1fd..6c226121c 100644
--- a/BepuPhysics/CollisionDetection/CollisionTasks/ConvexCompoundOverlapFinder.cs
+++ b/BepuPhysics/CollisionDetection/CollisionTasks/ConvexCompoundOverlapFinder.cs
@@ -15,26 +15,28 @@ public unsafe struct OverlapQueryForPair
 
     public unsafe interface IBoundsQueryableCompound
     {
-        unsafe void FindLocalOverlaps<TOverlaps, TSubpairOverlaps>(ref Buffer<OverlapQueryForPair> pairs, BufferPool pool, Shapes shapes, ref TOverlaps overlaps)
+        void FindLocalOverlaps<TOverlaps, TSubpairOverlaps>(ref Buffer<OverlapQueryForPair> pairs, BufferPool pool, Shapes shapes, ref TOverlaps overlaps)
             where TOverlaps : struct, ICollisionTaskOverlaps<TSubpairOverlaps>
             where TSubpairOverlaps : struct, ICollisionTaskSubpairOverlaps;
 
-        unsafe void FindLocalOverlaps<TOverlaps>(in Vector3 min, in Vector3 max, in Vector3 sweep, float maximumT, BufferPool pool, Shapes shapes, void* overlaps)
+        void FindLocalOverlaps<TOverlaps>(Vector3 min, Vector3 max, Vector3 sweep, float maximumT, BufferPool pool, Shapes shapes, void* overlaps)
             where TOverlaps : ICollisionTaskSubpairOverlaps;
-    }
-    public interface IOverlapTestingOptions
-    {
+
         /// <summary>
-        /// 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.
+        /// Finds the indices of all children whose local-space bounding boxes overlap the given local-space AABB.
         /// </summary>
-        bool EpsilonExpandBounds { get; }
+        /// <typeparam name="TEnumerator">Type of the enumerator that receives child indices.</typeparam>
+        /// <param name="min">Minimum corner of the query AABB in the compound's local space.</param>
+        /// <param name="max">Maximum corner of the query AABB in the compound's local space.</param>
+        /// <param name="pool">Pool used for any temporary allocations during traversal.</param>
+        /// <param name="shapes">Shape collection used to look up child bounds for compounds with heterogeneous children. May be null for homogeneous compounds that don't require it.</param>
+        /// <param name="enumerator">Enumerator that receives the indices of overlapping children.</param>
+        void FindLocalOverlaps<TEnumerator>(Vector3 min, Vector3 max, BufferPool pool, Shapes shapes, ref TEnumerator enumerator)
+            where TEnumerator : IBreakableForEach<int>;
     }
-    public struct UseEpsilonBoundsExpansion : IOverlapTestingOptions { public readonly bool EpsilonExpandBounds => true; }
-    public struct DontUseEpsilonBoundsExpansion : IOverlapTestingOptions { public readonly bool EpsilonExpandBounds => false; }
     public interface IConvexCompoundOverlapFinder
     {
-        void FindLocalOverlaps<TOverlapTestingOptions>(ref Buffer<BoundsTestedPair> pairs, int pairCount, BufferPool pool, Shapes shapes, float dt, out ConvexCompoundTaskOverlaps overlaps) where TOverlapTestingOptions : unmanaged, IOverlapTestingOptions;
+        void FindLocalOverlaps(ref Buffer<BoundsTestedPair> pairs, int pairCount, BufferPool pool, Shapes shapes, float dt, out ConvexCompoundTaskOverlaps overlaps);
     }
 
     public struct ConvexCompoundOverlapFinder<TConvex, TConvexWide, TCompound> : IConvexCompoundOverlapFinder
@@ -42,7 +44,7 @@ public struct ConvexCompoundOverlapFinder<TConvex, TConvexWide, TCompound> : ICo
         where TConvexWide : struct, IShapeWide<TConvex>
         where TCompound : struct, IBoundsQueryableCompound
     {
-        public unsafe void FindLocalOverlaps<TOverlapTestingOptions>(ref Buffer<BoundsTestedPair> pairs, int pairCount, BufferPool pool, Shapes shapes, float dt, out ConvexCompoundTaskOverlaps overlaps) where TOverlapTestingOptions : unmanaged, IOverlapTestingOptions
+        public unsafe void FindLocalOverlaps(ref Buffer<BoundsTestedPair> pairs, int pairCount, BufferPool pool, Shapes shapes, float dt, out ConvexCompoundTaskOverlaps overlaps)
         {
             overlaps = new ConvexCompoundTaskOverlaps(pool, pairCount);
             ref var pairsToTest = ref overlaps.subpairQueries;
@@ -54,11 +56,10 @@ public unsafe void FindLocalOverlaps<TOverlapTestingOptions>(ref Buffer<BoundsTe
             Unsafe.SkipInit(out Vector3Wide angularVelocityB);
             Unsafe.SkipInit(out Vector<float> maximumAllowedExpansion);
             Unsafe.SkipInit(out TConvexWide convexWide);
-            Unsafe.SkipInit(out TOverlapTestingOptions overlapTestingOptions);
             if (convexWide.InternalAllocationSize > 0)
             {
                 var memory = stackalloc byte[convexWide.InternalAllocationSize];
-                convexWide.Initialize(new RawBuffer(memory, convexWide.InternalAllocationSize));
+                convexWide.Initialize(new Buffer<byte>(memory, convexWide.InternalAllocationSize));
             }
             for (int i = 0; i < pairCount; i += Vector<float>.Count)
             {
@@ -95,10 +96,6 @@ public unsafe void FindLocalOverlaps<TOverlapTestingOptions>(ref Buffer<BoundsTe
                 Vector3Wide.Negate(localOffsetB, out var localPositionA);
                 BoundingBoxHelpers.ExpandLocalBoundingBoxes(ref min, ref max, Vector<float>.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/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<int> 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/ConvexHullPairTester.cs b/BepuPhysics/CollisionDetection/CollisionTasks/ConvexHullPairTester.cs
index 02f83d997..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;
 
@@ -15,9 +14,9 @@ struct CachedEdge
             public Vector3 EdgePlaneNormal;
             public float MaximumContainmentDot;
         }
-        public int BatchSize => 16;
+        public static int BatchSize => 16;
 
-        public unsafe void Test(ref ConvexHullWide a, ref ConvexHullWide b, ref Vector<float> 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<float> 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);
@@ -33,7 +32,7 @@ public unsafe void Test(ref ConvexHullWide a, ref ConvexHullWide b, ref Vector<f
             initialNormal.Y = Vector.ConditionalSelect(useInitialFallback, Vector<float>.One, initialNormal.Y);
             initialNormal.Z = Vector.ConditionalSelect(useInitialFallback, Vector<float>.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);
@@ -105,7 +104,9 @@ public unsafe void Test(ref ConvexHullWide a, ref ConvexHullWide b, ref Vector<f
                     edge.EdgePlaneNormal = Vector3.Cross(slotLocalNormal, edge.Vertex - previousVertexA);
                     previousVertexA = edge.Vertex;
                 }
-                var maximumCandidateCount = faceVertexIndicesB.Length * 2; //Two contacts per edge.
+                //The number of possible edge intersections is no more than two per edge on either shape due to convexity.
+                //Vertex-face contacts could number as high as the number of vertices in either face.
+                var maximumCandidateCount = Math.Max(Math.Max(faceVertexIndicesA.Length, faceVertexIndicesB.Length), Math.Min(faceVertexIndicesA.Length * 2, faceVertexIndicesB.Length * 2));
                 var candidates = stackalloc ManifoldCandidateScalar[maximumCandidateCount];
                 var candidateCount = 0;
                 var previousIndexB = faceVertexIndicesB[faceVertexIndicesB.Length - 1];
@@ -223,12 +224,12 @@ public unsafe void Test(ref ConvexHullWide a, ref ConvexHullWide b, ref Vector<f
             Matrix3x3Wide.TransformWithoutOverlap(localNormal, rB, out manifold.Normal);
         }
 
-        public void Test(ref ConvexHullWide a, ref ConvexHullWide b, ref Vector<float> speculativeMargin, ref Vector3Wide offsetB, ref QuaternionWide orientationB, int pairCount, out Convex4ContactManifoldWide manifold)
+        public static void Test(ref ConvexHullWide a, ref ConvexHullWide b, ref Vector<float> 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<float> speculativeMargin, ref Vector3Wide offsetB, int pairCount, out Convex4ContactManifoldWide manifold)
+        public static void Test(ref ConvexHullWide a, ref ConvexHullWide b, ref Vector<float> speculativeMargin, ref Vector3Wide offsetB, int pairCount, out Convex4ContactManifoldWide manifold)
         {
             throw new NotImplementedException();
         }
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/ConvexMeshContinuations.cs b/BepuPhysics/CollisionDetection/CollisionTasks/ConvexMeshContinuations.cs
index 3774372e6..7428c1200 100644
--- a/BepuPhysics/CollisionDetection/CollisionTasks/ConvexMeshContinuations.cs
+++ b/BepuPhysics/CollisionDetection/CollisionTasks/ConvexMeshContinuations.cs
@@ -3,12 +3,12 @@
 
 namespace BepuPhysics.CollisionDetection.CollisionTasks
 {
-    public struct ConvexMeshContinuations<TMesh> : IConvexCompoundContinuationHandler<MeshReduction> where TMesh : IHomogeneousCompoundShape<Triangle, TriangleWide>
+    public struct ConvexMeshContinuations<TMesh> : IConvexCompoundContinuationHandler<MeshReduction> where TMesh : struct, IHomogeneousCompoundShape<Triangle, TriangleWide>
     {
         public CollisionContinuationType CollisionContinuationType => CollisionContinuationType.MeshReduction;
 
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public ref MeshReduction CreateContinuation<TCallbacks>(
+        public unsafe ref MeshReduction CreateContinuation<TCallbacks>(
             ref CollisionBatcher<TCallbacks> collisionBatcher, int childCount, in BoundsTestedPair pair, in OverlapQueryForPair pairQuery, out int continuationIndex)
             where TCallbacks : struct, ICollisionCallbacks
         {
@@ -20,6 +20,9 @@ public ref MeshReduction CreateContinuation<TCallbacks>(
             continuation.RequiresFlip = pair.FlipMask == 0;
             continuation.QueryBounds.Min = pairQuery.Min;
             continuation.QueryBounds.Max = pairQuery.Max;
+            continuation.Mesh = pairQuery.Container;
+            continuation.FindLocalOverlapsThunk = MeshReductionThunks<TMesh>.FindLocalOverlaps;
+            continuation.GetLocalChildThunk = MeshReductionThunks<TMesh>.GetLocalChild;
             return ref continuation;
         }
 
@@ -33,7 +36,7 @@ public unsafe void ConfigureContinuationChild<TCallbacks>(
             //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<TMesh>(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 368f3ad17..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;
 
@@ -9,7 +8,7 @@ namespace BepuPhysics.CollisionDetection.CollisionTasks
 {
     public struct CylinderConvexHullTester : IPairTester<CylinderWide, ConvexHullWide, Convex4ContactManifoldWide>
     {
-        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)
@@ -64,9 +63,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<float> contactDepthWide, ref Vector<int> contactFeatureIdWide, ref Vector<int> contactExistsWide)
         {
             //Create max contact.
@@ -81,7 +80,7 @@ static void InsertContact(in Vector3 slotSideEdgeCenter, in Vector3 slotCylinder
             GatherScatter.GetFirst(ref contactExistsWide) = -1;
         }
 
-        public unsafe void Test(ref CylinderWide a, ref ConvexHullWide b, ref Vector<float> 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<float> 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);
@@ -98,7 +97,7 @@ public unsafe void Test(ref CylinderWide a, ref ConvexHullWide b, ref Vector<flo
             initialNormal.Z = Vector.ConditionalSelect(useInitialFallback, Vector<float>.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;
@@ -407,12 +406,12 @@ public unsafe void Test(ref CylinderWide a, ref ConvexHullWide b, ref Vector<flo
             Vector3Wide.Add(manifold.OffsetA3, offset3, out manifold.OffsetA3);
         }
 
-        public void Test(ref CylinderWide a, ref ConvexHullWide b, ref Vector<float> speculativeMargin, ref Vector3Wide offsetB, ref QuaternionWide orientationB, int pairCount, out Convex4ContactManifoldWide manifold)
+        public static void Test(ref CylinderWide a, ref ConvexHullWide b, ref Vector<float> 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<float> speculativeMargin, ref Vector3Wide offsetB, int pairCount, out Convex4ContactManifoldWide manifold)
+        public static void Test(ref CylinderWide a, ref ConvexHullWide b, ref Vector<float> 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 e1e3e8c0d..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;
 
@@ -12,7 +10,7 @@ namespace BepuPhysics.CollisionDetection.CollisionTasks
 
     public struct CylinderPairTester : IPairTester<CylinderWide, CylinderWide, Convex4ContactManifoldWide>
     {
-        public int BatchSize => 16;
+        public static int BatchSize => 16;
 
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
         static void ProjectOntoCapA(in Vector<float> capCenterBY, in Vector3Wide capCenterA, in Matrix3x3Wide rA, in Vector<float> inverseNDotAY, in Vector3Wide localNormal, in Vector2Wide point, out Vector2Wide projected)
@@ -95,7 +93,7 @@ internal static void TransformContact(
         }
         
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public void Test(
+        public static void Test(
             ref CylinderWide a, ref CylinderWide b, ref Vector<float> speculativeMargin,
             ref Vector3Wide offsetB, ref QuaternionWide orientationA, ref QuaternionWide orientationB, int pairCount,
             out Convex4ContactManifoldWide manifold)
@@ -152,7 +150,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));
@@ -447,12 +445,12 @@ public void Test(
             manifold.FeatureId3 = new Vector<int>(3);
         }
 
-        public void Test(ref CylinderWide a, ref CylinderWide b, ref Vector<float> speculativeMargin, ref Vector3Wide offsetB, ref QuaternionWide orientationB, int pairCount, out Convex4ContactManifoldWide manifold)
+        public static void Test(ref CylinderWide a, ref CylinderWide b, ref Vector<float> 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<float> speculativeMargin, ref Vector3Wide offsetB, int pairCount, out Convex4ContactManifoldWide manifold)
+        public static void Test(ref CylinderWide a, ref CylinderWide b, ref Vector<float> speculativeMargin, ref Vector3Wide offsetB, int pairCount, out Convex4ContactManifoldWide manifold)
         {
             throw new NotImplementedException();
         }
diff --git a/BepuPhysics/CollisionDetection/CollisionTasks/ManifoldCandidateHelper.cs b/BepuPhysics/CollisionDetection/CollisionTasks/ManifoldCandidateHelper.cs
index b9a1cd45a..e41a473ae 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
@@ -24,27 +20,6 @@ public struct ManifoldCandidate
     }
     public static class ManifoldCandidateHelper
     {
-        [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public static void CreateInactiveMask(int pairCount, out Vector<int> inactiveLanes)
-        {
-            inactiveLanes = Vector<int>.Zero;
-            ref var laneMasks = ref Unsafe.As<Vector<int>, int>(ref inactiveLanes);
-            for (int i = Vector<int>.Count - 1; i >= pairCount; --i)
-            {
-                Unsafe.Add(ref laneMasks, i) = -1;
-            }
-        }
-        [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public static void CreateActiveMask(int pairCount, out Vector<int> activeLanes)
-        {
-            activeLanes = Vector<int>.Zero;
-            ref var laneMasks = ref Unsafe.As<Vector<int>, 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<int> count, in ManifoldCandidate candidate, in Vector<int> newContactExists, int pairCount)
         {
@@ -213,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>(-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);
@@ -308,9 +283,9 @@ 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)
+        static void PlaceCandidateInSlot(in ManifoldCandidateScalar candidate, int contactIndex,
+            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);
@@ -335,8 +310,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/CollisionTasks/MeshPairContinuations.cs b/BepuPhysics/CollisionDetection/CollisionTasks/MeshPairContinuations.cs
index faa0e3dcf..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<TMeshA, TMeshB> : ICompoundPairContinuationHandler<CompoundMeshReduction>
-        where TMeshA : IHomogeneousCompoundShape<Triangle, TriangleWide>
-        where TMeshB : IHomogeneousCompoundShape<Triangle, TriangleWide>
+        where TMeshA : struct, IHomogeneousCompoundShape<Triangle, TriangleWide>
+        where TMeshB : struct, IHomogeneousCompoundShape<Triangle, TriangleWide>
     {
         public CollisionContinuationType CollisionContinuationType => CollisionContinuationType.CompoundMeshReduction;
 
@@ -29,6 +29,9 @@ public ref CompoundMeshReduction CreateContinuation<TCallbacks>(
             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;
+            continuation.Mesh = pair.B;
+            continuation.FindLocalOverlapsThunk = MeshReductionThunks<TMeshB>.FindLocalOverlaps;
+            continuation.GetLocalChildThunk = MeshReductionThunks<TMeshB>.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.
@@ -48,19 +51,19 @@ public ref CompoundMeshReduction CreateContinuation<TCallbacks>(
         }
 
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public unsafe void GetChildAData<TCallbacks>(ref CollisionBatcher<TCallbacks> collisionBatcher, ref CompoundMeshReduction continuation, in BoundsTestedPair pair, int childIndexA,
+        public void GetChildAData<TCallbacks>(ref CollisionBatcher<TCallbacks> collisionBatcher, ref CompoundMeshReduction continuation, in BoundsTestedPair pair, int childIndexA,
             out RigidPose childPoseA, out int childTypeA, out void* childShapeDataA)
             where TCallbacks : struct, ICollisionCallbacks
         {
             ref var triangle = ref continuation.Triangles[triangleAStartIndex++];
             childShapeDataA = Unsafe.AsPointer(ref triangle);
-            childTypeA = triangle.TypeId;
+            childTypeA = Triangle.TypeId;
             Unsafe.AsRef<TMeshA>(pair.A).GetLocalChild(childIndexA, out triangle);
             childPoseA = new RigidPose(default, pair.OrientationA);
         }
 
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public unsafe void ConfigureContinuationChild<TCallbacks>(
+        public void ConfigureContinuationChild<TCallbacks>(
             ref CollisionBatcher<TCallbacks> 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
@@ -69,7 +72,7 @@ public unsafe void ConfigureContinuationChild<TCallbacks>(
             //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<TMeshB>(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 f1aaeb6aa..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
 {
@@ -15,7 +12,7 @@ public struct MeshPairOverlapFinder<TMeshA, TMeshB> : ICompoundPairOverlapFinder
         where TMeshB : struct, IHomogeneousCompoundShape<Triangle, TriangleWide>
     {
 
-        public unsafe void FindLocalOverlaps<TOverlapTestingOptions>(ref Buffer<BoundsTestedPair> pairs, int pairCount, BufferPool pool, Shapes shapes, float dt, out CompoundPairOverlaps overlaps) where TOverlapTestingOptions : unmanaged, IOverlapTestingOptions
+        public static unsafe void FindLocalOverlaps(ref Buffer<BoundsTestedPair> pairs, int pairCount, BufferPool pool, Shapes shapes, float dt, out CompoundPairOverlaps overlaps)
         {
             var totalCompoundChildCount = 0;
             for (int i = 0; i < pairCount; ++i)
@@ -40,7 +37,6 @@ public unsafe void FindLocalOverlaps<TOverlapTestingOptions>(ref Buffer<BoundsTe
 
             Unsafe.SkipInit(out TriangleWide triangles);
             nextSubpairIndex = 0;
-            Unsafe.SkipInit(out TOverlapTestingOptions overlapTestingOptions);
             for (int i = 0; i < pairCount; ++i)
             {
                 ref var pair = ref pairs[i];
@@ -73,10 +69,6 @@ public unsafe void FindLocalOverlaps<TOverlapTestingOptions>(ref Buffer<BoundsTe
                     Vector3Wide.Length(localOffsetA, out var radiusA);
                     BoundingBoxHelpers.ExpandLocalBoundingBoxes(ref min, ref max, radiusA, localOffsetA, localRelativeLinearVelocityA, angularVelocityA, angularVelocityB, dt,
                         maximumRadius, maximumAngularExpansion, maximumAllowedExpansion);
-                    if (overlapTestingOptions.EpsilonExpandBounds)
-                    {
-                        BoundingBoxHelpers.EpsilonExpandLocalBoundingBoxes(maximumRadius, ref min, ref max);
-                    }
 
                     for (int innerIndex = 0; innerIndex < count; ++innerIndex)
                     {
diff --git a/BepuPhysics/CollisionDetection/CollisionTasks/PairTypes.cs b/BepuPhysics/CollisionDetection/CollisionTasks/PairTypes.cs
index 01d82b4fd..bebfbc9a7 100644
--- a/BepuPhysics/CollisionDetection/CollisionTasks/PairTypes.cs
+++ b/BepuPhysics/CollisionDetection/CollisionTasks/PairTypes.cs
@@ -21,8 +21,8 @@ public interface ICollisionPair<TPair> where TPair : ICollisionPair<TPair>
         /// <summary>
         /// Gets the enumeration type associated with this pair type.
         /// </summary>
-        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<CollisionPair>
@@ -39,10 +39,10 @@ public unsafe struct CollisionPair : ICollisionPair<CollisionPair>
         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<FliplessPair>
         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<SpherePair>
         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<SphereIncludingPair>
         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;
         }
@@ -108,8 +108,6 @@ public ref PairContinuation GetContinuation(ref SphereIncludingPair pair)
     /// <summary>
     /// Pair of objects awaiting collision processing that involves velocities for bounds calculation.
     /// </summary>
-    /// <typeparam name="TA">Type of the first shape in the pair.</typeparam>
-    /// <typeparam name="TB">Type of the second shape in the pair.</typeparam>
     public unsafe struct BoundsTestedPair : ICollisionPair<BoundsTestedPair>
     {
         public void* A;
@@ -125,10 +123,10 @@ public unsafe struct BoundsTestedPair : ICollisionPair<BoundsTestedPair>
         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 +136,16 @@ public interface ICollisionPairWide<TShapeA, TShapeWideA, TShapeB, TShapeWideB,
         where TShapeA : struct, IShape where TShapeB : struct, IShape
         where TShapeWideA : struct, IShapeWide<TShapeA> where TShapeWideB : struct, IShapeWide<TShapeB>
     {
-        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<int> GetFlipMask(ref TPairWide pair);
-        ref Vector<float> 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<int> GetFlipMask(ref TPairWide pair);
+        static abstract ref Vector<float> 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 +164,51 @@ public struct ConvexPairWide<TShapeA, TShapeWideA, TShapeB, TShapeWideB> :
         public QuaternionWide OrientationB;
         public Vector<float> 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<int> GetFlipMask(ref ConvexPairWide<TShapeA, TShapeWideA, TShapeB, TShapeWideB> pair)
+        public static ref Vector<int> GetFlipMask(ref ConvexPairWide<TShapeA, TShapeWideA, TShapeB, TShapeWideB> pair)
         {
             return ref pair.FlipMask;
         }
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public ref Vector<float> GetSpeculativeMargin(ref ConvexPairWide<TShapeA, TShapeWideA, TShapeB, TShapeWideB> pair)
+        public static ref Vector<float> GetSpeculativeMargin(ref ConvexPairWide<TShapeA, TShapeWideA, TShapeB, TShapeWideB> 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<TShapeA, TShapeWideA, TShapeB, TShapeWideB> pair)
+        public static ref TShapeWideA GetShapeA(ref ConvexPairWide<TShapeA, TShapeWideA, TShapeB, TShapeWideB> pair)
         {
             return ref pair.A;
         }
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public ref TShapeWideB GetShapeB(ref ConvexPairWide<TShapeA, TShapeWideA, TShapeB, TShapeWideB> pair)
+        public static ref TShapeWideB GetShapeB(ref ConvexPairWide<TShapeA, TShapeWideA, TShapeB, TShapeWideB> pair)
         {
             return ref pair.B;
         }
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public ref Vector3Wide GetOffsetB(ref ConvexPairWide<TShapeA, TShapeWideA, TShapeB, TShapeWideB> pair)
+        public static ref Vector3Wide GetOffsetB(ref ConvexPairWide<TShapeA, TShapeWideA, TShapeB, TShapeWideB> pair)
         {
             return ref pair.OffsetB;
         }
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public ref QuaternionWide GetOrientationA(ref ConvexPairWide<TShapeA, TShapeWideA, TShapeB, TShapeWideB> pair)
+        public static ref QuaternionWide GetOrientationA(ref ConvexPairWide<TShapeA, TShapeWideA, TShapeB, TShapeWideB> pair)
         {
             return ref pair.OrientationA;
         }
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public ref QuaternionWide GetOrientationB(ref ConvexPairWide<TShapeA, TShapeWideA, TShapeB, TShapeWideB> pair)
+        public static ref QuaternionWide GetOrientationB(ref ConvexPairWide<TShapeA, TShapeWideA, TShapeB, TShapeWideB> pair)
         {
             return ref pair.OrientationB;
         }
@@ -243,50 +241,50 @@ public struct FliplessPairWide<TShape, TShapeWide> : ICollisionPairWide<TShape,
         public QuaternionWide OrientationB;
         public Vector<float> 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<int> GetFlipMask(ref FliplessPairWide<TShape, TShapeWide> pair)
+        public static ref Vector<int> GetFlipMask(ref FliplessPairWide<TShape, TShapeWide> pair)
         {
             throw new NotImplementedException();
         }
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public ref Vector<float> GetSpeculativeMargin(ref FliplessPairWide<TShape, TShapeWide> pair)
+        public static ref Vector<float> GetSpeculativeMargin(ref FliplessPairWide<TShape, TShapeWide> 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<TShape, TShapeWide> pair)
+        public static ref TShapeWide GetShapeA(ref FliplessPairWide<TShape, TShapeWide> pair)
         {
             return ref pair.A;
         }
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public ref TShapeWide GetShapeB(ref FliplessPairWide<TShape, TShapeWide> pair)
+        public static ref TShapeWide GetShapeB(ref FliplessPairWide<TShape, TShapeWide> pair)
         {
             return ref pair.B;
         }
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public ref Vector3Wide GetOffsetB(ref FliplessPairWide<TShape, TShapeWide> pair)
+        public static ref Vector3Wide GetOffsetB(ref FliplessPairWide<TShape, TShapeWide> pair)
         {
             return ref pair.OffsetB;
         }
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public ref QuaternionWide GetOrientationA(ref FliplessPairWide<TShape, TShapeWide> pair)
+        public static ref QuaternionWide GetOrientationA(ref FliplessPairWide<TShape, TShapeWide> pair)
         {
             return ref pair.OrientationA;
         }
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public ref QuaternionWide GetOrientationB(ref FliplessPairWide<TShape, TShapeWide> pair)
+        public static ref QuaternionWide GetOrientationB(ref FliplessPairWide<TShape, TShapeWide> pair)
         {
             return ref pair.OrientationB;
         }
@@ -320,51 +318,51 @@ public struct SphereIncludingPairWide<TShape, TShapeWide> : ICollisionPairWide<S
         public QuaternionWide OrientationB;
         public Vector<float> 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<int> GetFlipMask(ref SphereIncludingPairWide<TShape, TShapeWide> pair)
+        public static ref Vector<int> GetFlipMask(ref SphereIncludingPairWide<TShape, TShapeWide> pair)
         {
             return ref pair.FlipMask;
         }
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public ref Vector<float> GetSpeculativeMargin(ref SphereIncludingPairWide<TShape, TShapeWide> pair)
+        public static ref Vector<float> GetSpeculativeMargin(ref SphereIncludingPairWide<TShape, TShapeWide> 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<TShape, TShapeWide> pair)
+        public static ref SphereWide GetShapeA(ref SphereIncludingPairWide<TShape, TShapeWide> pair)
         {
             return ref pair.A;
         }
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public ref TShapeWide GetShapeB(ref SphereIncludingPairWide<TShape, TShapeWide> pair)
+        public static ref TShapeWide GetShapeB(ref SphereIncludingPairWide<TShape, TShapeWide> pair)
         {
             return ref pair.B;
         }
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public ref Vector3Wide GetOffsetB(ref SphereIncludingPairWide<TShape, TShapeWide> pair)
+        public static ref Vector3Wide GetOffsetB(ref SphereIncludingPairWide<TShape, TShapeWide> pair)
         {
             return ref pair.OffsetB;
         }
-        public ref QuaternionWide GetOrientationA(ref SphereIncludingPairWide<TShape, TShapeWide> pair)
+        public static ref QuaternionWide GetOrientationA(ref SphereIncludingPairWide<TShape, TShapeWide> pair)
         {
             throw new NotImplementedException();
         }
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public ref QuaternionWide GetOrientationB(ref SphereIncludingPairWide<TShape, TShapeWide> pair)
+        public static ref QuaternionWide GetOrientationB(ref SphereIncludingPairWide<TShape, TShapeWide> pair)
         {
             return ref pair.OrientationB;
         }
@@ -390,47 +388,47 @@ public struct SpherePairWide : ICollisionPairWide<Sphere, SphereWide, Sphere, Sp
         public Vector3Wide OffsetB;
         public Vector<float> 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<int> GetFlipMask(ref SpherePairWide pair)
+        public static ref Vector<int> GetFlipMask(ref SpherePairWide pair)
         {
             throw new NotImplementedException();
         }
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public ref Vector<float> GetSpeculativeMargin(ref SpherePairWide pair)
+        public static ref Vector<float> 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<SphereWide, BoxWide, Convex1ContactManifoldWide>
     {
-        public int BatchSize => 32;
+        public static int BatchSize => 32;
 
-        public void Test(ref SphereWide a, ref BoxWide b, ref Vector<float> 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<float> 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<float> speculativeMargin, ref Vector3Wide offsetB, ref QuaternionWide orientationB, int pairCount, out Convex1ContactManifoldWide manifold)
+        public static void Test(ref SphereWide a, ref BoxWide b, ref Vector<float> 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<float> speculativeM
             manifold.ContactExists = Vector.GreaterThan(manifold.Depth, -speculativeMargin);
         }
 
-        public void Test(ref SphereWide a, ref BoxWide b, ref Vector<float> speculativeMargin, ref Vector3Wide offsetB, int pairCount, out Convex1ContactManifoldWide manifold)
+        public static void Test(ref SphereWide a, ref BoxWide b, ref Vector<float> 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<SphereWide, CapsuleWide, Convex1ContactManifoldWide>
     {
-        public int BatchSize => 32;
+        public static int BatchSize => 32;
 
-        public void Test(ref SphereWide a, ref CapsuleWide b, ref Vector<float> 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<float> 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<float> speculativeMargin, ref Vector3Wide offsetB, ref QuaternionWide orientationB, int pairCount, out Convex1ContactManifoldWide manifold)
+        public static void Test(ref SphereWide a, ref CapsuleWide b, ref Vector<float> 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<float> speculat
             manifold.ContactExists = Vector.GreaterThan(manifold.Depth, -speculativeMargin);
         }
 
-        public void Test(ref SphereWide a, ref CapsuleWide b, ref Vector<float> speculativeMargin, ref Vector3Wide offsetB, int pairCount, out Convex1ContactManifoldWide manifold)
+        public static void Test(ref SphereWide a, ref CapsuleWide b, ref Vector<float> 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 4f7628e1b..c57317ade 100644
--- a/BepuPhysics/CollisionDetection/CollisionTasks/SphereConvexHullTester.cs
+++ b/BepuPhysics/CollisionDetection/CollisionTasks/SphereConvexHullTester.cs
@@ -2,20 +2,19 @@
 using BepuUtilities;
 using System;
 using System.Numerics;
-using System.Runtime.CompilerServices;
 
 namespace BepuPhysics.CollisionDetection.CollisionTasks
 {
     public struct SphereConvexHullTester : IPairTester<SphereWide, ConvexHullWide, Convex1ContactManifoldWide>
     {
-        public int BatchSize => 16;
+        public static int BatchSize => 16;
 
-        public void Test(ref SphereWide a, ref ConvexHullWide b, ref Vector<float> 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<float> 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<float> speculativeMargin, ref Vector3Wide offsetB, ref QuaternionWide orientationB, int pairCount, out Convex1ContactManifoldWide manifold)
+        public static void Test(ref SphereWide a, ref ConvexHullWide b, ref Vector<float> 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);
@@ -29,7 +28,7 @@ public void Test(ref SphereWide a, ref ConvexHullWide b, ref Vector<float> specu
             initialNormal.Z = Vector.ConditionalSelect(useInitialFallback, Vector<float>.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<ConvexHull, ConvexHullWide, ConvexHullSupportFinder, Sphere, SphereWide, SphereSupportFinder>.FindMinimumDepth(
@@ -45,7 +44,7 @@ public void Test(ref SphereWide a, ref ConvexHullWide b, ref Vector<float> specu
             manifold.ContactExists = Vector.GreaterThanOrEqual(manifold.Depth, -speculativeMargin);
         }
 
-        public void Test(ref SphereWide a, ref ConvexHullWide b, ref Vector<float> speculativeMargin, ref Vector3Wide offsetB, int pairCount, out Convex1ContactManifoldWide manifold)
+        public static void Test(ref SphereWide a, ref ConvexHullWide b, ref Vector<float> 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 1b3b02255..8b4fba8a0 100644
--- a/BepuPhysics/CollisionDetection/CollisionTasks/SphereCylinderTester.cs
+++ b/BepuPhysics/CollisionDetection/CollisionTasks/SphereCylinderTester.cs
@@ -8,16 +8,16 @@ namespace BepuPhysics.CollisionDetection.CollisionTasks
 {
     public struct SphereCylinderTester : IPairTester<SphereWide, CylinderWide, Convex1ContactManifoldWide>
     {
-        public int BatchSize => 32;
+        public static int BatchSize => 32;
 
-        public void Test(ref SphereWide a, ref CylinderWide b, ref Vector<float> 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<float> speculativeMargin, ref Vector3Wide offsetB, ref QuaternionWide orientationA, ref QuaternionWide orientationB, int pairCount, out Convex1ContactManifoldWide manifold)
         {
             throw new NotImplementedException();
         }
 
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
         public static void ComputeSphereToClosest(in CylinderWide b, in Vector3Wide offsetB, in Matrix3x3Wide orientationMatrixB,
-            out Vector3Wide cylinderLocalOffsetA, out Vector<int> horizontalClampRequired, out Vector<float> horizontalOffsetLength, out Vector<float> inverseHorizontalOffsetLength, 
+            out Vector3Wide cylinderLocalOffsetA, out Vector<float> horizontalOffsetLength, out Vector<float> 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<float>.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));
@@ -37,21 +37,21 @@ public static void ComputeSphereToClosest(in CylinderWide b, in Vector3Wide offs
         }
 
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public void Test(ref SphereWide a, ref CylinderWide b, ref Vector<float> speculativeMargin, ref Vector3Wide offsetB, ref QuaternionWide orientationB, int pairCount, out Convex1ContactManifoldWide manifold)
+        public static void Test(ref SphereWide a, ref CylinderWide b, ref Vector<float> speculativeMargin, ref Vector3Wide offsetB, ref QuaternionWide orientationB, int pairCount, out Convex1ContactManifoldWide manifold)
         {
             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<float>.Zero);
             Vector3Wide localInternalNormal;
+
             var useHorizontalFallback = Vector.LessThanOrEqual(horizontalOffsetLength, b.Radius * new Vector<float>(1e-5f));
             localInternalNormal.X = Vector.ConditionalSelect(useDepthY, Vector<float>.Zero, Vector.ConditionalSelect(useHorizontalFallback, Vector<float>.One, cylinderLocalOffsetA.X * inverseHorizontalOffsetLength));
             localInternalNormal.Y = Vector.ConditionalSelect(useDepthY, Vector.ConditionalSelect(useTopCapNormal, Vector<float>.One, new Vector<float>(-1)), Vector<float>.Zero);
@@ -61,6 +61,8 @@ public void Test(ref SphereWide a, ref CylinderWide b, ref Vector<float> specula
             //Note negation; normal points from B to A by convention.
             Vector3Wide.Scale(sphereToContactLocalB, new Vector<float>(-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<float>(1e-7f));
             Vector3Wide.ConditionalSelect(useInternal, localInternalNormal, localExternalNormal, out var localNormal);
 
             Matrix3x3Wide.TransformWithoutOverlap(localNormal, orientationMatrixB, out manifold.Normal);
@@ -70,7 +72,7 @@ public void Test(ref SphereWide a, ref CylinderWide b, ref Vector<float> specula
             manifold.ContactExists = Vector.GreaterThanOrEqual(manifold.Depth, -speculativeMargin);
         }
 
-        public void Test(ref SphereWide a, ref CylinderWide b, ref Vector<float> speculativeMargin, ref Vector3Wide offsetB, int pairCount, out Convex1ContactManifoldWide manifold)
+        public static void Test(ref SphereWide a, ref CylinderWide b, ref Vector<float> 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<SphereWide, SphereWide, Convex1ContactManifoldWide>
     {
-        public int BatchSize => 32;
+        public static int BatchSize => 32;
 
-        public void Test(ref SphereWide a, ref SphereWide b, ref Vector<float> 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<float> 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<float> speculativeMargin, ref Vector3Wide offsetB, ref QuaternionWide orientationB, int pairCount, out Convex1ContactManifoldWide manifold)
+        public static void Test(ref SphereWide a, ref SphereWide b, ref Vector<float> 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<float> speculativeMargin, ref Vector3Wide offsetB, int pairCount, out Convex1ContactManifoldWide manifold)
+        public static void Test(ref SphereWide a, ref SphereWide b, ref Vector<float> 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 ace11812f..d5382f9a7 100644
--- a/BepuPhysics/CollisionDetection/CollisionTasks/SphereTriangleTester.cs
+++ b/BepuPhysics/CollisionDetection/CollisionTasks/SphereTriangleTester.cs
@@ -9,14 +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<SphereWide, TriangleWide, Convex1ContactManifoldWide>
     {
-        public int BatchSize => 32;
+        public static int BatchSize => 32;
 
-        /// <summary>
-        /// Minimum dot product between the detected local normal and the face normal of a triangle necessary to create contacts.
-        /// </summary>
-        public const float BackfaceNormalDotRejectionThreshold = -1e-2f;
-
-        public void Test(ref SphereWide a, ref TriangleWide b, ref Vector<float> speculativeMargin, ref Vector3Wide offsetB, ref QuaternionWide orientationA, ref QuaternionWide orientationB,
+        public static void Test(ref SphereWide a, ref TriangleWide b, ref Vector<float> speculativeMargin, ref Vector3Wide offsetB, ref QuaternionWide orientationA, ref QuaternionWide orientationB,
             int pairCount, out Convex1ContactManifoldWide manifold)
         {
             throw new NotImplementedException();
@@ -31,7 +26,7 @@ static void Select(ref Vector<float> distanceSquared, ref Vector3Wide localNorma
         }
 
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public void Test(ref SphereWide a, ref TriangleWide b, ref Vector<float> speculativeMargin, ref Vector3Wide offsetB, ref QuaternionWide orientationB, int pairCount,
+        public static void Test(ref SphereWide a, ref TriangleWide b, ref Vector<float> speculativeMargin, ref Vector3Wide offsetB, ref QuaternionWide orientationB, int pairCount,
             out Convex1ContactManifoldWide manifold)
         {
             Unsafe.SkipInit(out manifold);
@@ -72,7 +67,7 @@ public void Test(ref SphereWide a, ref TriangleWide b, ref Vector<float> specula
             var outsideAnyEdge = Vector.BitwiseOr(outsideAB, Vector.BitwiseOr(outsideAC, outsideBC));
             Unsafe.SkipInit(out Vector3Wide localClosestOnTriangle);
             var negativeOne = new Vector<int>(-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.
@@ -115,14 +110,17 @@ public void Test(ref SphereWide a, ref TriangleWide b, ref Vector<float> 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<float>.Zero),
                 Vector.BitwiseAnd(
-                    Vector.LessThanOrEqual(faceNormalDotLocalNormal, new Vector<float>(-BackfaceNormalDotRejectionThreshold)),
+                    Vector.GreaterThan(distance, Vector<float>.Zero),
+                    nondegenerateMask),
+                Vector.BitwiseAnd(
+                    Vector.LessThanOrEqual(faceNormalDotLocalNormal, new Vector<float>(-TriangleWide.BackfaceNormalDotRejectionThreshold)),
                     Vector.GreaterThanOrEqual(manifold.Depth, -speculativeMargin)));
         }
 
-        public void Test(ref SphereWide a, ref TriangleWide b, ref Vector<float> speculativeMargin, ref Vector3Wide offsetB, int pairCount, out Convex1ContactManifoldWide manifold)
+        public static void Test(ref SphereWide a, ref TriangleWide b, ref Vector<float> 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 b77fa11aa..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;
 
@@ -11,9 +10,9 @@ namespace BepuPhysics.CollisionDetection.CollisionTasks
     using DepthRefiner = DepthRefiner<ConvexHull, ConvexHullWide, ConvexHullSupportFinder, Triangle, TriangleWide, PretransformedTriangleSupportFinder>;
     public struct TriangleConvexHullTester : IPairTester<TriangleWide, ConvexHullWide, Convex4ContactManifoldWide>
     {
-        public int BatchSize => 16;
+        public static int BatchSize => 16;
 
-        public unsafe void Test(ref TriangleWide a, ref ConvexHullWide b, ref Vector<float> 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<float> 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);
@@ -59,13 +58,12 @@ public unsafe void Test(ref TriangleWide a, ref ConvexHullWide b, ref Vector<flo
                     Vector.BitwiseAnd(Vector.LessThanOrEqual(bcPlaneTest, Vector<float>.Zero), Vector.LessThanOrEqual(caPlaneTest, Vector<float>.Zero)));
             var hullInsideAndBelowTriangle = Vector.BitwiseAnd(hullBelowPlane, hullInsideTriangleEdgePlanes);
 
-
-            ManifoldCandidateHelper.CreateInactiveMask(pairCount, out var inactiveLanes);
-            a.EstimateEpsilonScale(out var triangleEpsilonScale);
+            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);
             //Note that degenerate triangles will not contribute contacts. They don't have a well defined normal.
-            inactiveLanes = Vector.BitwiseOr(inactiveLanes, Vector.LessThan(triangleNormalLength, epsilonScale * 1e-6f));
+            inactiveLanes = Vector.BitwiseOr(inactiveLanes, Vector.OnesComplement(nondegenerateMask));
             inactiveLanes = Vector.BitwiseOr(inactiveLanes, hullInsideAndBelowTriangle);
             //Not every lane will generate contacts. Rather than requiring every lane to carefully clear all contactExists states, just clear them up front.
             manifold.Contact0Exists = default;
@@ -138,7 +136,7 @@ public unsafe void Test(ref TriangleWide a, ref ConvexHullWide b, ref Vector<flo
 
 
             Vector3Wide.Dot(triangleNormal, localNormal, out var triangleNormalDotLocalNormal);
-            inactiveLanes = Vector.BitwiseOr(inactiveLanes, Vector.BitwiseOr(Vector.GreaterThan(triangleNormalDotLocalNormal, new Vector<float>(-SphereTriangleTester.BackfaceNormalDotRejectionThreshold)), Vector.LessThan(depth, depthThreshold)));
+            inactiveLanes = Vector.BitwiseOr(inactiveLanes, Vector.BitwiseOr(Vector.GreaterThan(triangleNormalDotLocalNormal, new Vector<float>(-TriangleWide.BackfaceNormalDotRejectionThreshold)), Vector.LessThan(depth, depthThreshold)));
             if (Vector.LessThanAll(inactiveLanes, Vector<int>.Zero))
             {
                 //No contacts generated.
@@ -202,6 +200,7 @@ public unsafe void Test(ref TriangleWide a, ref ConvexHullWide b, ref Vector<flo
 
             var inverseTriangleNormalDotLocalNormal = Vector<float>.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
@@ -364,7 +363,7 @@ public unsafe void Test(ref TriangleWide a, ref ConvexHullWide b, ref Vector<flo
                     candidate.FeatureId = 0;
 
                 }
-                if (latestEntryAB < earliestExitAB && latestEntryAB > 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.
@@ -385,7 +384,7 @@ public unsafe void Test(ref TriangleWide a, ref ConvexHullWide b, ref Vector<flo
                     candidate.FeatureId = 2;
 
                 }
-                if (latestEntryBC < earliestExitBC && latestEntryBC > 0 && candidateCount < 6)
+                if (latestEntryBC < earliestExitBC && latestEntryBC > 0 && candidateCount < maximumContactCount)
                 {
                     //Create min contact.
                     var point = slotTriangleBC * latestEntryBC + slotTriangleAB;
@@ -406,7 +405,7 @@ public unsafe void Test(ref TriangleWide a, ref ConvexHullWide b, ref Vector<flo
                     candidate.FeatureId = 4;
 
                 }
-                if (latestEntryCA < earliestExitCA && latestEntryCA > 0 && candidateCount < 6)
+                if (latestEntryCA < earliestExitCA && latestEntryCA > 0 && candidateCount < maximumContactCount)
                 {
                     //Create min contact.
                     var point = slotTriangleCA * latestEntryCA - slotTriangleCA;
@@ -433,12 +432,12 @@ public unsafe void Test(ref TriangleWide a, ref ConvexHullWide b, ref Vector<flo
             manifold.FeatureId0 += faceCollisionFlag;
         }
 
-        public void Test(ref TriangleWide a, ref ConvexHullWide b, ref Vector<float> speculativeMargin, ref Vector3Wide offsetB, ref QuaternionWide orientationB, int pairCount, out Convex4ContactManifoldWide manifold)
+        public static void Test(ref TriangleWide a, ref ConvexHullWide b, ref Vector<float> 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<float> speculativeMargin, ref Vector3Wide offsetB, int pairCount, out Convex4ContactManifoldWide manifold)
+        public static void Test(ref TriangleWide a, ref ConvexHullWide b, ref Vector<float> 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 bc7ec690a..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;
 
@@ -39,7 +37,7 @@ public void GetMargin(in TriangleWide shape, out Vector<float> margin)
 
     public struct TriangleCylinderTester : IPairTester<TriangleWide, CylinderWide, Convex4ContactManifoldWide>
     {
-        public int BatchSize => 16;
+        public static int BatchSize => 16;
 
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
         static void TryAddInteriorPoint(in Vector2Wide point, in Vector<int> featureId,
@@ -99,7 +97,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<float> speculativeMargin,
             ref Vector3Wide offsetB, ref QuaternionWide orientationA, ref QuaternionWide orientationB, int pairCount,
             out Convex4ContactManifoldWide manifold)
@@ -160,9 +158,9 @@ public unsafe void Test(
                     Vector.LessThanOrEqual(caPlaneTest, Vector<float>.Zero)));
             var cylinderInsideAndBelowTriangle = Vector.BitwiseAnd(cylinderInsideTriangleEdgePlanes, cylinderBelowPlane);
 
-            ManifoldCandidateHelper.CreateInactiveMask(pairCount, out var inactiveLanes);
-            var degenerate = Vector.LessThan(triangleNormalLength, new Vector<float>(1e-10f));
-            inactiveLanes = Vector.BitwiseOr(degenerate, 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);
             if (Vector.LessThanAll(inactiveLanes, Vector<int>.Zero))
             {
@@ -218,7 +216,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<float>(-SphereTriangleTester.BackfaceNormalDotRejectionThreshold)), Vector.LessThan(depth, depthThreshold)));
+            inactiveLanes = Vector.BitwiseOr(inactiveLanes, Vector.BitwiseOr(Vector.GreaterThan(faceNormalADotNormal, new Vector<float>(-TriangleWide.BackfaceNormalDotRejectionThreshold)), Vector.LessThan(depth, depthThreshold)));
             if (Vector.LessThanAll(inactiveLanes, Vector<int>.Zero))
             {
                 //All lanes are either inactive or were found to have a depth lower than the speculative margin, so we can just quit early.
@@ -299,7 +297,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<float>.One / faceNormalADotNormal;
+                    var inverseDenominator = new Vector<float>(-1f) / faceNormalADotNormal;
                     var yOffset = localTriangleCenter.Y - capCenterBY;
                     var xOffset0 = localTriangleCenter.X - interiorOnCylinder0.X;
                     var zOffset0 = localTriangleCenter.Z - interiorOnCylinder0.Y;
@@ -531,17 +529,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,8 +547,8 @@ public unsafe void Test(
                     Vector3Wide.ConditionalSelect(useB, triangleB, vertexFallback, out vertexFallback);
 
                     //Bound the interval to the cylinder's extent.
-                    cylinderTMin = Vector.Max(Vector<float>.Zero, Vector.Min(Vector<float>.One, cylinderTMin));
-                    cylinderTMax = Vector.Max(Vector<float>.Zero, Vector.Min(Vector<float>.One, cylinderTMax));
+                    cylinderTMin = Vector.ConditionalSelect(useSideTriangleFace, Vector.Max(Vector<float>.Zero, Vector.Min(Vector<float>.One, sideTriangleCylinderTMin)), cylinderTMin);
+                    cylinderTMax = Vector.ConditionalSelect(useSideTriangleFace, Vector.Max(Vector<float>.Zero, Vector.Min(Vector<float>.One, sideTriangleCylinderTMax)), 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);
@@ -563,8 +561,8 @@ public unsafe void Test(
                     //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<float>.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<int>.Zero, manifold.FeatureId0);
                 manifold.FeatureId1 = Vector.ConditionalSelect(useSide, Vector<int>.One, manifold.FeatureId1);
@@ -594,12 +592,12 @@ public unsafe void Test(
         }
 
 
-        public void Test(ref TriangleWide a, ref CylinderWide b, ref Vector<float> speculativeMargin, ref Vector3Wide offsetB, ref QuaternionWide orientationB, int pairCount, out Convex4ContactManifoldWide manifold)
+        public static void Test(ref TriangleWide a, ref CylinderWide b, ref Vector<float> 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<float> speculativeMargin, ref Vector3Wide offsetB, int pairCount, out Convex4ContactManifoldWide manifold)
+        public static void Test(ref TriangleWide a, ref CylinderWide b, ref Vector<float> 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 cdea34fcb..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<TriangleWide, TriangleWide, Convex4ContactManifoldWide>
     {
-        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<float> min, out Vector<float> max)
@@ -42,6 +42,48 @@ static void TestEdgeEdge(
             //Protect against bad normals.
             depth = Vector.ConditionalSelect(Vector.LessThan(normalLength, new Vector<float>(1e-10f)), new Vector<float>(float.MaxValue), depth);
         }
+
+        //[MethodImpl(MethodImplOptions.AggressiveInlining)]
+        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 Vector<float> inverseLengthSquaredAB, in Vector<float> inverseLengthSquaredBC, in Vector<float> inverseLengthSquaredCA,
+            ref Vector<float> 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, opposingAB, out var avDotAB);
+            Vector3Wide.Dot(bv, opposingBC, out var bvDotBC);
+            Vector3Wide.Dot(cv, opposingCA, out var cvDotCA);
+            var tAB = Vector.Max(Vector<float>.Zero, Vector.Min(Vector<float>.One, avDotAB * inverseLengthSquaredAB));
+            var tBC = Vector.Max(Vector<float>.Zero, Vector.Min(Vector<float>.One, bvDotBC * inverseLengthSquaredBC));
+            var tCA = Vector.Max(Vector<float>.Zero, Vector.Min(Vector<float>.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);
+
+            var useCandidate = Vector.LessThan(distanceSquaredCandidate, distanceSquared);
+            distanceSquared = Vector.Min(distanceSquaredCandidate, distanceSquared);
+            Vector3Wide.ConditionalSelect(useCandidate, offsetCandidate, offset, out offset);
+        }
+
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
         static void Select(
             ref Vector<float> depth, ref Vector3Wide normal,
@@ -103,7 +145,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<float> inverseEdgeLengthSquaredA, in Vector<float> edgeStartADotNormal, in Vector<float> edgeOffsetADotNormal,
@@ -123,19 +165,9 @@ private static void ClipEdge(
             var tA = ((intersectionPointX - edgeStartA.X) * edgeOffsetA.X + (intersectionPointY - edgeStartA.Y) * edgeOffsetA.Y) * inverseEdgeLengthSquaredA;
             intersectionExists = Vector.BitwiseAnd(Vector.GreaterThanOrEqual(tA, Vector<float>.Zero), Vector.LessThanOrEqual(tA, Vector<float>.One));
             depthContributionA = edgeStartADotNormal + edgeOffsetADotNormal * tA;
-
-
-            //var minValue = new Vector<float>(float.MinValue);
-            //var maxValue = new Vector<float>(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<float>.Zero), Vector.LessThan(Vector.Abs(velocity), new Vector<float>(1e-14f)));
-            //entry = Vector.ConditionalSelect(edgeParallelAndOutside, maxValue, entry);
-            //exit = Vector.ConditionalSelect(edgeParallelAndOutside, minValue, exit);
         }
 
-        [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,
@@ -225,8 +257,8 @@ private static void ClipBEdgeAgainstABounds( //maybe we should have.. MORE param
 
 
 
-        [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public unsafe void Test(
+        //[MethodImpl(MethodImplOptions.AggressiveInlining)]
+        public static unsafe void Test(
             ref TriangleWide a, ref TriangleWide b, ref Vector<float> speculativeMargin,
             ref Vector3Wide offsetB, ref QuaternionWide orientationA, ref QuaternionWide orientationB, int pairCount,
             out Convex4ContactManifoldWide manifold)
@@ -259,6 +291,7 @@ 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);
@@ -294,6 +327,44 @@ 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);
 
+            Vector3Wide.LengthSquared(abA, out var abALengthSquared);
+            Vector3Wide.LengthSquared(abB, out var abBLengthSquared);
+            Vector3Wide.LengthSquared(caA, out var caALengthSquared);
+            Vector3Wide.LengthSquared(caB, out var caBLengthSquared);
+            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.
+            //var tryVertexNormals = Vector.BitwiseAnd(Vector.LessThan(depth, Vector<float>.Zero), allowContacts);
+            //if (Vector.LessThanAny(tryVertexNormals, Vector<int>.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<float>.One / abALengthSquared;
+            //    var inverseBCALengthSquared = Vector<float>.One / bcALengthSquared;
+            //    var inverseCAALengthSquared = Vector<float>.One / caALengthSquared;
+            //    var inverseABBLengthSquared = Vector<float>.One / abBLengthSquared;
+            //    var inverseBCBLengthSquared = Vector<float>.One / bcBLengthSquared;
+            //    var inverseCABLengthSquared = Vector<float>.One / caBLengthSquared;
+            //    var distanceSquared = new Vector<float>(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<float>.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<float>(1e-7f)), depthCandidate, new Vector<float>(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);
             Vector3Wide.Dot(localNormal, centerAToCenterB, out var calibrationDot);
@@ -302,12 +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);
-            ManifoldCandidateHelper.CreateActiveMask(pairCount, out var activeLanes);
-            var allowContacts = Vector.BitwiseAnd(activeLanes, Vector.BitwiseAnd(
-                Vector.LessThan(localNormalDotFaceNormalA, new Vector<float>(-SphereTriangleTester.BackfaceNormalDotRejectionThreshold)),
-                Vector.GreaterThan(localNormalDotFaceNormalB, new Vector<float>(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<float>(-TriangleWide.BackfaceNormalDotRejectionThreshold)),
+                        Vector.GreaterThan(localNormalDotFaceNormalB, new Vector<float>(TriangleWide.BackfaceNormalDotRejectionThreshold)))));
             if (Vector.EqualsAll(allowContacts, Vector<int>.Zero))
             {
                 manifold.Contact0Exists = default;
@@ -357,7 +434,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<float>.One / Vector.SquareRoot(abBLengthSquared), out var tangentBX);
             Vector3Wide.CrossWithoutOverlap(tangentBX, faceNormalB, out var tangentBY);
 
@@ -367,7 +443,6 @@ public unsafe void Test(
             var candidateCount = Vector<int>.Zero;
             ref var candidates = ref *buffer;
 
-            var minimumDepth = -speculativeMargin;
             if (Vector.LessThanAny(useFaceCaseForB, Vector<int>.Zero))
             {
                 //While the edge clipping will find any edge-edge or bVertex-aFace contacts, it will not find aVertex-bFace contacts.
@@ -408,12 +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.
-            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)));
+            var epsilonScale = Vector.Min(epsilonScaleA, epsilonScaleB);
             var edgeEpsilon = new Vector<float>(1e-5f) * epsilonScale;
             ManifoldCandidateHelper.ReduceWithoutComputingDepths(ref candidates, candidateCount, 6, epsilonScale, minimumDepth, pairCount,
                 out var contact0, out var contact1, out var contact2, out var contact3,
@@ -453,12 +523,12 @@ private static void TransformContactToManifold(
             manifoldFeatureId = rawContact.FeatureId;
         }
 
-        public void Test(ref TriangleWide a, ref TriangleWide b, ref Vector<float> speculativeMargin, ref Vector3Wide offsetB, ref QuaternionWide orientationB, int pairCount, out Convex4ContactManifoldWide manifold)
+        public static void Test(ref TriangleWide a, ref TriangleWide b, ref Vector<float> 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<float> speculativeMargin, ref Vector3Wide offsetB, int pairCount, out Convex4ContactManifoldWide manifold)
+        public static void Test(ref TriangleWide a, ref TriangleWide b, ref Vector<float> speculativeMargin, ref Vector3Wide offsetB, int pairCount, out Convex4ContactManifoldWide manifold)
         {
             throw new NotImplementedException();
         }
diff --git a/BepuPhysics/CollisionDetection/CompoundMeshReduction.cs b/BepuPhysics/CollisionDetection/CompoundMeshReduction.cs
index 196cd0430..5501b4409 100644
--- a/BepuPhysics/CollisionDetection/CompoundMeshReduction.cs
+++ b/BepuPhysics/CollisionDetection/CompoundMeshReduction.cs
@@ -1,17 +1,13 @@
 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
 {
-    public struct CompoundMeshReduction : ICollisionTestContinuation
+    public unsafe struct CompoundMeshReduction : ICollisionTestContinuation
     {
         public int RegionCount;
         public Buffer<(int Start, int Count)> ChildManifoldRegions;
@@ -25,25 +21,30 @@ public struct CompoundMeshReduction : ICollisionTestContinuation
         //This uses all of the nonconvex reduction's logic, so we just nest it.
         public NonconvexReduction Inner;
 
+        //Type-erased mesh pointer plus the per-TMesh thunks. See MeshReduction for the rationale.
+        public void* Mesh;
+        public delegate*<void*, Vector3, Vector3, BufferPool, Shapes, ref MeshReduction.ChildEnumerator, void> FindLocalOverlapsThunk;
+        public delegate*<void*, int, out Triangle, void> GetLocalChildThunk;
+
         public void Create(int childManifoldCount, BufferPool pool)
         {
             Inner.Create(childManifoldCount, pool);
         }
 
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public unsafe void OnChildCompleted<TCallbacks>(ref PairContinuation report, ref ConvexContactManifold manifold, ref CollisionBatcher<TCallbacks> batcher)
+        public void OnChildCompleted<TCallbacks>(ref PairContinuation report, ref ConvexContactManifold manifold, ref CollisionBatcher<TCallbacks> batcher)
             where TCallbacks : struct, ICollisionCallbacks
         {
             Inner.OnChildCompleted(ref report, ref manifold, ref batcher);
         }
 
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public void OnChildCompletedEmpty<TCallbacks>(ref PairContinuation report, ref CollisionBatcher<TCallbacks> batcher) where TCallbacks : struct, ICollisionCallbacks
+        public void OnUntestedChildCompleted<TCallbacks>(ref PairContinuation report, ref CollisionBatcher<TCallbacks> batcher) where TCallbacks : struct, ICollisionCallbacks
         {
-            Inner.OnChildCompletedEmpty(ref report, ref batcher);
+            Inner.OnUntestedChildCompleted(ref report, ref batcher);
         }
         
-        public unsafe bool TryFlush<TCallbacks>(int pairId, ref CollisionBatcher<TCallbacks> batcher) where TCallbacks : struct, ICollisionCallbacks
+        public bool TryFlush<TCallbacks>(int pairId, ref CollisionBatcher<TCallbacks> batcher) where TCallbacks : struct, ICollisionCallbacks
         {
             Debug.Assert(Inner.ChildCount > 0);
             if (Inner.CompletedChildCount == Inner.ChildCount)
@@ -56,7 +57,8 @@ public unsafe bool TryFlush<TCallbacks>(int pairId, ref CollisionBatcher<TCallba
                     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);
+                        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/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
-    {
-        /// <summary>
-        /// 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.
-        /// </summary>
-        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/ConstraintRemover.cs b/BepuPhysics/CollisionDetection/ConstraintRemover.cs
index 51b994ccf..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
 {
@@ -27,7 +24,7 @@ public class ConstraintRemover
 
         internal struct PerBodyRemovalTarget
         {
-            public int BodyIndex;
+            public int EncodedBodyIndex;
             public ConstraintHandle ConstraintHandle;
 
             public int BatchIndex;
@@ -145,18 +142,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 ReferenceCollector(bodyIndices);
-                typeProcessor.EnumerateConnectedBodyIndices(ref typeBatch, constraint.IndexInTypeBatch, ref enumerator);
+                var encodedBodyIndices = stackalloc int[bodiesPerConstraint];
+                var enumerator = new PassthroughReferenceCollector(encodedBodyIndices);
+                solver.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];
                 }
             }
 
@@ -200,7 +197,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
@@ -210,9 +207,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<int>(3 + solver.ActiveSet.Fallback.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<int>(3, pool);
             }
         }
 
@@ -247,7 +244,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;
         /// <summary>
         /// Processes enqueued constraint removals and prepares removal jobs.
@@ -341,7 +338,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.");
+                    }
                 }
             }
         }
@@ -352,20 +354,22 @@ 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;
                 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);
                 }
             }
         }
 
         QuickList<int> allocationIdsToFree;
-        public void RemoveConstraintsFromFallbackBatch()
+        public void RemoveConstraintsFromFallbackBatchReferencedHandles()
         {
             Debug.Assert(solver.ActiveSet.Batches.Count > solver.FallbackBatchThreshold);
             for (int i = 0; i < batches.BatchCount; ++i)
@@ -376,18 +380,26 @@ 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.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,
+                            //    "The batch referenced handles must include all constraint-involved dynamics, but will not include kinematics.");
+                            solver.batchReferencedHandles[target.BatchIndex].Unset(target.BodyHandle.Value);
+                        }
                     }
                 }
             }
         }
-        public void TryRemoveAllConstraintsForBodyFromFallbackBatch(int bodyIndex)
+        public void TryRemoveBodyFromConstrainedKinematicsAndRemoveAllConstraintsForBodyFromFallbackBatch(BodyHandle bodyHandle, int bodyIndex)
         {
-            solver.ActiveSet.Fallback.TryRemove(bodyIndex, ref allocationIdsToFree);
+            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);
         }
 
         QuickList<TypeBatchIndex> removedTypeBatches;
-        SpinLock removedTypeBatchLocker = new SpinLock();
+        object batchRemovalLocker = new object();
         public void RemoveConstraintsFromTypeBatch(int index)
         {
             var batch = batches.TypeBatches[index];
@@ -395,7 +407,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];
@@ -403,15 +414,17 @@ 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.
-                    //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/ContactConstraintAccessor.cs b/BepuPhysics/CollisionDetection/ContactConstraintAccessor.cs
index 3b4a45e23..def003dd7 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
 {
@@ -52,7 +48,7 @@ public unsafe void GatherOldImpulses(ref ConstraintReference constraintReference
             }
         }
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public unsafe void ScatterNewImpulses<TContactImpulses>(ref ConstraintReference constraintReference, ref TContactImpulses contactImpulses)
+        public void ScatterNewImpulses<TContactImpulses>(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.
@@ -92,11 +88,10 @@ public abstract void FlushWithSpeculativeBatches<TCallbacks>(ref UntypedList lis
         public abstract void FlushSequentially<TCallbacks>(ref UntypedList list, int narrowPhaseConstraintTypeId, Simulation simulation, PairCache pairCache)
             where TCallbacks : struct, INarrowPhaseCallbacks;
 
-        public abstract unsafe void UpdateConstraintForManifold<TContactManifold, TCollisionCache, TCallBodyHandles, TCallbacks>(
+        public abstract void UpdateConstraintForManifold<TContactManifold, TCallBodyHandles, TCallbacks>(
             NarrowPhase<TCallbacks> 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;
 
         /// <summary>
         /// Extracts references to data from a contact constraint of the accessor's type.
@@ -144,10 +139,10 @@ public void ExtractContactPrestepAndImpulses<TExtractor>(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<TConstraintDescription, TBodyHandles, TPrestepData, TAccumulatedImpulses, TContactImpulses, TConstraintCache> : ContactConstraintAccessor
+    public abstract class ContactConstraintAccessor<TConstraintDescription, TBodyHandles, TPrestepData, TAccumulatedImpulses, TContactImpulses> : ContactConstraintAccessor
+        where TBodyHandles : unmanaged
         where TConstraintDescription : unmanaged, IConstraintDescription<TConstraintDescription>
         where TContactImpulses : unmanaged
-        where TConstraintCache : unmanaged, IPairCacheEntry
         where TPrestepData : unmanaged
     {
         protected ContactConstraintAccessor()
@@ -182,12 +177,8 @@ protected ContactConstraintAccessor()
                 Debug.Assert(ContactCount * 3 * Unsafe.SizeOf<Vector<float>>() == Unsafe.SizeOf<TAccumulatedImpulses>(),
                     "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<TConstraintCache>() == 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<TAccumulatedImpulses>();
-            ConstraintTypeId = default(TConstraintDescription).ConstraintTypeId;
+            ConstraintTypeId = TConstraintDescription.ConstraintTypeId;
         }
         public override void DeterministicallyAdd<TCallbacks>(int typeIndex, NarrowPhase<TCallbacks>.OverlapWorker[] overlapWorkers,
             ref QuickList<NarrowPhase<TCallbacks>.SortConstraintTarget> constraintsOfType,
@@ -212,29 +203,28 @@ public override void FlushWithSpeculativeBatches<TCallbacks>(ref UntypedList lis
         }
 
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        protected static unsafe void UpdateConstraint<TCallbacks, TCollisionCache, TCallBodyHandles>(
+        protected static void UpdateConstraint<TCallbacks, TCallBodyHandles>(
             NarrowPhase<TCallbacks> 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<TBodyHandles, TConstraintDescription, TContactImpulses, TCollisionCache, TConstraintCache>(
-                workerIndex, ref pair, manifoldTypeAsConstraintType, ref constraintCache, ref collisionCache, ref description, Unsafe.As<TCallBodyHandles, TBodyHandles>(ref bodyHandles));
+            narrowPhase.UpdateConstraint<TBodyHandles, TConstraintDescription, TContactImpulses>(
+                workerIndex, pair, manifoldTypeAsConstraintType, ref constraintCache, newContactCount, ref description, Unsafe.As<TCallBodyHandles, TBodyHandles>(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<TConstraintDescription, ConstraintContactData>(ref description);
-            ref var targetFeatureIds = ref Unsafe.Add(ref Unsafe.As<TConstraintCache, int>(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);
@@ -244,12 +234,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<TConstraintCache, int>(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);
@@ -260,30 +249,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<TConstraintCache, int>(ref constraintCache), 1) = manifold.Contact0.FeatureId;
+            constraintCache.FeatureId0 = manifold.Contact0.FeatureId;
             targetContacts.OffsetA = manifold.Contact0.Offset;
             targetContacts.PenetrationDepth = manifold.Contact0.Depth;
         }
     }
 
 
-    public class ConvexOneBodyAccessor<TConstraintDescription, TPrestepData, TAccumulatedImpulses, TContactImpulses, TConstraintCache> :
-        ContactConstraintAccessor<TConstraintDescription, int, TPrestepData, TAccumulatedImpulses, TContactImpulses, TConstraintCache>
+    public class ConvexOneBodyAccessor<TConstraintDescription, TPrestepData, TAccumulatedImpulses, TContactImpulses> :
+        ContactConstraintAccessor<TConstraintDescription, int, TPrestepData, TAccumulatedImpulses, TContactImpulses>
         where TConstraintDescription : unmanaged, IConvexOneBodyContactConstraintDescription<TConstraintDescription>
         where TContactImpulses : unmanaged
-        where TConstraintCache : unmanaged, IPairCacheEntry
         where TPrestepData : unmanaged, IConvexContactPrestep<TPrestepData>
         where TAccumulatedImpulses : unmanaged, IConvexContactAccumulatedImpulses<TAccumulatedImpulses>
     {
-        public override void UpdateConstraintForManifold<TContactManifold, TCollisionCache, TCallBodyHandles, TCallbacks>(
+        public override void UpdateConstraintForManifold<TContactManifold, TCallBodyHandles, TCallbacks>(
             NarrowPhase<TCallbacks> 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))
@@ -291,18 +278,18 @@ public override void UpdateConstraintForManifold<TContactManifold, TCollisionCac
                 ref var manifold = ref Unsafe.As<TContactManifold, ConvexContactManifold>(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<TContactManifold, NonconvexContactManifold>(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));
+                CopyContactData(ref manifold, ref constraintCache, ref TConstraintDescription.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);
             }
         }
 
@@ -332,17 +319,16 @@ public override void ExtractContactPrestepAndImpulses<TExtractor>(in ConstraintL
         }
     }
 
-    public class ConvexTwoBodyAccessor<TConstraintDescription, TPrestepData, TAccumulatedImpulses, TContactImpulses, TConstraintCache> :
-        ContactConstraintAccessor<TConstraintDescription, TwoBodyHandles, TPrestepData, TAccumulatedImpulses, TContactImpulses, TConstraintCache>
+    public class ConvexTwoBodyAccessor<TConstraintDescription, TPrestepData, TAccumulatedImpulses, TContactImpulses> :
+        ContactConstraintAccessor<TConstraintDescription, TwoBodyHandles, TPrestepData, TAccumulatedImpulses, TContactImpulses>
         where TConstraintDescription : unmanaged, IConvexTwoBodyContactConstraintDescription<TConstraintDescription>
         where TContactImpulses : unmanaged
-        where TConstraintCache : unmanaged, IPairCacheEntry
         where TPrestepData : unmanaged, ITwoBodyConvexContactPrestep<TPrestepData>
         where TAccumulatedImpulses : unmanaged, IConvexContactAccumulatedImpulses<TAccumulatedImpulses>
     {
-        public override void UpdateConstraintForManifold<TContactManifold, TCollisionCache, TCallBodyHandles, TCallbacks>(
+        public override void UpdateConstraintForManifold<TContactManifold, TCallBodyHandles, TCallbacks>(
             NarrowPhase<TCallbacks> 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))
@@ -350,18 +336,18 @@ public override void UpdateConstraintForManifold<TContactManifold, TCollisionCac
                 ref var manifold = ref Unsafe.As<TContactManifold, ConvexContactManifold>(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<TContactManifold, NonconvexContactManifold>(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));
+                CopyContactData(ref manifold, ref constraintCache, ref TConstraintDescription.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<TExtractor>(in ConstraintLocation constraintLocation, Solver solver, ref TExtractor extractor)
@@ -378,13 +364,13 @@ public override void ExtractContactData<TExtractor>(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);
         }
@@ -401,25 +387,24 @@ public override void ExtractContactPrestepAndImpulses<TExtractor>(in ConstraintL
         }
     }
 
-    public class NonconvexOneBodyAccessor<TConstraintDescription, TPrestepData, TAccumulatedImpulses, TContactImpulses, TConstraintCache> :
-        ContactConstraintAccessor<TConstraintDescription, int, TPrestepData, TAccumulatedImpulses, TContactImpulses, TConstraintCache>
+    public class NonconvexOneBodyAccessor<TConstraintDescription, TPrestepData, TAccumulatedImpulses, TContactImpulses> :
+        ContactConstraintAccessor<TConstraintDescription, int, TPrestepData, TAccumulatedImpulses, TContactImpulses>
         where TConstraintDescription : unmanaged, INonconvexOneBodyContactConstraintDescription<TConstraintDescription>
         where TContactImpulses : unmanaged
-        where TConstraintCache : unmanaged, IPairCacheEntry
         where TPrestepData : unmanaged, INonconvexContactPrestep<TPrestepData>
         where TAccumulatedImpulses : unmanaged, INonconvexContactAccumulatedImpulses<TAccumulatedImpulses>
     {
-        public override void UpdateConstraintForManifold<TContactManifold, TCollisionCache, TCallBodyHandles, TCallbacks>(
+        public override void UpdateConstraintForManifold<TContactManifold, TCallBodyHandles, TCallbacks>(
             NarrowPhase<TCallbacks> 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<TContactManifold, NonconvexContactManifold>(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));
+            CopyContactData(ref manifold, ref constraintCache, ref TConstraintDescription.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<TExtractor>(in ConstraintLocation constraintLocation, Solver solver, ref TExtractor extractor)
@@ -448,25 +433,24 @@ public override void ExtractContactPrestepAndImpulses<TExtractor>(in ConstraintL
         }
     }
 
-    public class NonconvexTwoBodyAccessor<TConstraintDescription, TPrestepData, TAccumulatedImpulses, TContactImpulses, TConstraintCache> :
-        ContactConstraintAccessor<TConstraintDescription, TwoBodyHandles, TPrestepData, TAccumulatedImpulses, TContactImpulses, TConstraintCache>
+    public class NonconvexTwoBodyAccessor<TConstraintDescription, TPrestepData, TAccumulatedImpulses, TContactImpulses> :
+        ContactConstraintAccessor<TConstraintDescription, TwoBodyHandles, TPrestepData, TAccumulatedImpulses, TContactImpulses>
         where TConstraintDescription : unmanaged, INonconvexTwoBodyContactConstraintDescription<TConstraintDescription>
         where TContactImpulses : unmanaged
-        where TConstraintCache : unmanaged, IPairCacheEntry
         where TPrestepData : unmanaged, ITwoBodyNonconvexContactPrestep<TPrestepData>
         where TAccumulatedImpulses : unmanaged, INonconvexContactAccumulatedImpulses<TAccumulatedImpulses>
     {
-        public override void UpdateConstraintForManifold<TContactManifold, TCollisionCache, TCallBodyHandles, TCallbacks>(
+        public override void UpdateConstraintForManifold<TContactManifold, TCallBodyHandles, TCallbacks>(
             NarrowPhase<TCallbacks> 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<TContactManifold, NonconvexContactManifold>(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));
+            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, ref collisionCache, ref description, bodyHandles);
+            UpdateConstraint(narrowPhase, manifoldTypeAsConstraintType, workerIndex, ref pair, ref constraintCache, manifold.Count, ref description, bodyHandles);
         }
 
         public override void ExtractContactData<TExtractor>(in ConstraintLocation constraintLocation, Solver solver, ref TExtractor extractor)
@@ -483,13 +467,13 @@ public override void ExtractContactData<TExtractor>(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/BepuPhysics/CollisionDetection/ContactManifold.cs b/BepuPhysics/CollisionDetection/ContactManifold.cs
index 6f99b938b..6a58b2ed2 100644
--- a/BepuPhysics/CollisionDetection/ContactManifold.cs
+++ b/BepuPhysics/CollisionDetection/ContactManifold.cs
@@ -7,11 +7,11 @@
 namespace BepuPhysics.CollisionDetection
 {
     /// <summary>
-    /// 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.
     /// </summary>
+    /// <remarks>This type contains a field for the normal; it can be used to represent contacts within nonconvex contact manifolds or convex manifolds.</remarks>
     [StructLayout(LayoutKind.Explicit, Size = 32)]
-    public struct NonconvexContact
+    public struct Contact
     {
         /// <summary>
         /// Offset from the position of collidable A to the contact position. 
@@ -72,14 +72,44 @@ public interface IContactManifold<TManifold> where TManifold : struct, IContactM
         bool Convex { get; }
 
         /// <summary>
-        /// Retrieves the feature id associated with a requested contact.
+        /// Gets or sets the contact at the given index in the manifold.
+        /// </summary>
+        /// <param name="contactIndex">Index of the contact to get or set.</param>
+        /// <returns>Contact at the specified index.</returns>
+        /// <remarks>Note that contact normals are shared across a <see cref="ConvexContactManifold"/>. Setting one contact in a convex manifold will change the entire convex manifold's normal.</remarks>
+        public Contact this[int contactIndex] { get; set; }
+
+        /// <summary>
+        /// Gets the feature id associated with a requested contact.
         /// </summary>
         /// <param name="contactIndex">Index of the contact to grab the feature id of.</param>
         /// <returns>Feature id of the requested contact.</returns>
         int GetFeatureId(int contactIndex);
 
         /// <summary>
-        /// Retrieves a copy of a contact's data.
+        /// Gets the depth associated with a requested contact.
+        /// </summary>
+        /// <param name="contactIndex">Index of the contact to grab the depth of.</param>
+        /// <returns>Depth of the requested contact.</returns>
+        float GetDepth(int contactIndex);
+
+        /// <summary>
+        /// Gets a contact's normal.
+        /// </summary>
+        /// <param name="contactIndex">Index of the contact to grab the normal of.</param>
+        /// <returns>Normal of the requested contact.</returns>
+        /// <remarks>Points from collidable B to collidable A. In convex manifolds, all contacts share a normal and will return the same value.</remarks>
+        Vector3 GetNormal(int contactIndex);
+
+        /// <summary>
+        /// Gets the offset from collidable A to the requested contact.
+        /// </summary>
+        /// <param name="contactIndex">Index of the contact to grab the offset of.</param>
+        /// <returns>Offset to a contact's offset.</returns>
+        Vector3 GetOffset(int contactIndex);
+
+        /// <summary>
+        /// Gets a copy of a contact's data.
         /// </summary>
         /// <param name="contactIndex">Index of the contact to copy data from.</param>
         /// <param name="offset">Offset from the first collidable's position to the contact position.</param>
@@ -89,39 +119,62 @@ public interface IContactManifold<TManifold> 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.</param>
         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...
         /// <summary>
-        /// Pulls a reference to a contact's depth.
+        /// Gets a copy of a contact's data.
+        /// </summary>
+        /// <param name="contactIndex">Index of the contact to copy data from.</param>
+        /// <param name="contactData">Data associated with the contact.</param>
+        void GetContact(int contactIndex, out Contact contactData);
+
+        /// <summary>
+        /// Gets a reference to a contact's depth.
         /// </summary>
         /// <param name="manifold">Manifold to pull a reference from.</param>
         /// <param name="contactIndex">Contact to pull data from.</param>
         /// <returns>Reference to a contact's depth.</returns>
-        ref float GetDepth(ref TManifold manifold, int contactIndex);
+        static abstract ref float GetDepthReference(ref TManifold manifold, int contactIndex);
 
         /// <summary>
-        /// 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.
         /// </summary>
         /// <param name="manifold">Manifold to pull a reference from.</param>
         /// <param name="contactIndex">Contact to pull data from.</param>
         /// <returns>Reference to a contact's normal (or the manifold-wide normal in a convex manifold).</returns>
-        ref Vector3 GetNormal(ref TManifold manifold, int contactIndex);
+        static abstract ref Vector3 GetNormalReference(ref TManifold manifold, int contactIndex);
 
         /// <summary>
-        /// Pulls a reference to a contact's offset.
+        /// Gets a reference to the offset from collidable A to the requested contact.
         /// </summary>
         /// <param name="manifold">Manifold to pull a reference from.</param>
         /// <param name="contactIndex">Contact to pull data from.</param>
         /// <returns>Reference to a contact's offset.</returns>
-        ref Vector3 GetOffset(ref TManifold manifold, int contactIndex);
+        static abstract ref Vector3 GetOffsetReference(ref TManifold manifold, int contactIndex);
 
         /// <summary>
-        /// Pulls a reference to a contact's feature id.
+        /// Gets a reference to a contact's feature id.
         /// </summary>
         /// <param name="manifold">Manifold to pull a reference from.</param>
         /// <param name="contactIndex">Contact to pull data from.</param>
         /// <returns>Reference to a contact's feature id.</returns>
-        ref int GetFeatureId(ref TManifold manifold, int contactIndex);
+        static abstract ref int GetFeatureIdReference(ref TManifold manifold, int contactIndex);
+
+        /// <summary>
+        /// Gets a reference to a nonconvex manifold's contact.
+        /// </summary>
+        /// <param name="manifold">Manifold to pull a reference from.</param>
+        /// <param name="contactIndex">Contact to pull data from.</param>
+        /// <returns>Reference to the requested contact.</returns>
+        /// <remarks>This is a helper that avoids manual casting. If the manifold is not a <see cref="NonconvexContactManifold"/>, the function will throw an <see cref="NotSupportedException"/>.</remarks>
+        static abstract ref Contact GetNonconvexContactReference(ref TManifold manifold, int contactIndex);
 
+        /// <summary>
+        /// Gets a reference to a convex manifold's contact.
+        /// </summary>
+        /// <param name="manifold">Manifold to pull a reference from.</param>
+        /// <param name="contactIndex">Contact to pull data from.</param>
+        /// <returns>Reference to the requested contact.</returns>
+        /// <remarks>This is a helper that avoids manual casting. If the manifold is not a <see cref="ConvexContactManifold"/>, the function will throw an <see cref="NotSupportedException"/>.</remarks>
+        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.
@@ -141,18 +194,32 @@ public unsafe struct NonconvexContactManifold : IContactManifold<NonconvexContac
         public int Count;
 
         [FieldOffset(16)]
-        public NonconvexContact Contact0;
+        public Contact Contact0;
         [FieldOffset(48)]
-        public NonconvexContact Contact1;
+        public Contact Contact1;
         [FieldOffset(80)]
-        public NonconvexContact Contact2;
+        public Contact Contact2;
         [FieldOffset(112)]
-        public NonconvexContact Contact3;
+        public Contact Contact3;
 
         readonly int IContactManifold<NonconvexContactManifold>.Count => Count;
 
         readonly bool IContactManifold<NonconvexContactManifold>.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;
+            }
+        }
+
         /// <summary>
         /// The maximum number of contacts that can exist within a nonconvex manifold.
         /// </summary>
@@ -164,16 +231,6 @@ private readonly void ValidateIndex(int contactIndex)
             Debug.Assert(contactIndex >= 0 && contactIndex < Count, "Contact index must be within the contact count.");
         }
 
-        /// <summary>
-        /// Retrieves a copy of a contact's data.
-        /// </summary>
-        /// <param name="contactIndex">Index of the contact to copy data from.</param>
-        /// <param name="offset">Offset from the first collidable's position to the contact position.</param>
-        /// <param name="normal">Normal of the contact surface at the requested contact. Points from collidable B to collidable A.</param>
-        /// <param name="depth">Penetration depth at the requested contact.</param>
-        /// <param name="featureId">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.</param>
-        [MethodImpl(MethodImplOptions.AggressiveInlining)]
         public void GetContact(int contactIndex, out Vector3 offset, out Vector3 normal, out float depth, out int featureId)
         {
             ValidateIndex(contactIndex);
@@ -183,19 +240,74 @@ public void GetContact(int contactIndex, out Vector3 offset, out Vector3 normal,
             depth = contact.Depth;
             featureId = contact.FeatureId;
         }
-        /// <summary>
-        /// Retrieves the feature id associated with a requested contact.
-        /// </summary>
-        /// <param name="contactIndex">Index of the contact to grab the feature id of.</param>
-        /// <returns>Feature id of the requested contact.</returns>
-        [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;
@@ -206,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);
@@ -216,68 +327,18 @@ 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++];
         }
-
-        /// <summary>
-        /// Pulls a reference to a contact's depth.
-        /// </summary>
-        /// <param name="manifold">Manifold to pull a reference from.</param>
-        /// <param name="contactIndex">Contact to pull data from.</param>
-        /// <returns>Reference to a contact's depth.</returns>
-        [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public ref float GetDepth(ref NonconvexContactManifold manifold, int contactIndex)
-        {
-            return ref Unsafe.Add(ref manifold.Contact0, contactIndex).Depth;
-        }
-
-        /// <summary>
-        /// Pulls a reference to a contact's normal. Points from collidable B to collidable A.
-        /// </summary>
-        /// <param name="manifold">Manifold to pull a reference from.</param>
-        /// <param name="contactIndex">Contact to pull data from.</param>
-        /// <returns>Reference to a contact's normal.</returns>
-        [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public ref Vector3 GetNormal(ref NonconvexContactManifold manifold, int contactIndex)
-        {
-            return ref Unsafe.Add(ref manifold.Contact0, contactIndex).Normal;
-        }
-
-
-        /// <summary>
-        /// Pulls a reference to a contact's offset.
-        /// </summary>
-        /// <param name="manifold">Manifold to pull a reference from.</param>
-        /// <param name="contactIndex">Contact to pull data from.</param>
-        /// <returns>Reference to a contact's offset.</returns>
-        [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public ref Vector3 GetOffset(ref NonconvexContactManifold manifold, int contactIndex)
-        {
-            return ref Unsafe.Add(ref manifold.Contact0, contactIndex).Offset;
-        }
-
-        /// <summary>
-        /// Pulls a reference to a contact's feature id.
-        /// </summary>
-        /// <param name="manifold">Manifold to pull a reference from.</param>
-        /// <param name="contactIndex">Contact to pull data from.</param>
-        /// <returns>Reference to a contact's feature id.</returns>
-        [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public ref int GetFeatureId(ref NonconvexContactManifold manifold, int contactIndex)
-        {
-            return ref Unsafe.Add(ref manifold.Contact0, contactIndex).FeatureId;
-        }
     }
 
     /// <summary>
     /// Contains the data associated with a convex contact manifold.
     /// </summary>
     [StructLayout(LayoutKind.Explicit, Size = 108)]
-    public unsafe struct ConvexContactManifold : IContactManifold<ConvexContactManifold>
+    public struct ConvexContactManifold : IContactManifold<ConvexContactManifold>
     {
         /// <summary>
         /// Offset from collidable A to collidable B.
@@ -306,22 +367,28 @@ public unsafe struct ConvexContactManifold : IContactManifold<ConvexContactManif
 
         readonly bool IContactManifold<ConvexContactManifold>.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;
+            }
         }
 
-        /// <summary>
-        /// Retrieves the feature id associated with a requested contact.
-        /// </summary>
-        /// <param name="contactIndex">Index of the contact to grab the feature id of.</param>
-        /// <returns>Feature id of the requested contact.</returns>
-        [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.");
         }
 
         /// <summary>
@@ -333,7 +400,6 @@ public int GetFeatureId(int contactIndex)
         /// <param name="depth">Penetration depth at the requested contact.</param>
         /// <param name="featureId">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.</param>
-        [MethodImpl(MethodImplOptions.AggressiveInlining)]
         public void GetContact(int contactIndex, out Vector3 offset, out Vector3 normal, out float depth, out int featureId)
         {
             ValidateIndex(contactIndex);
@@ -343,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;
@@ -354,54 +450,40 @@ public static void FastRemoveAt(ref ConvexContactManifold manifold, int index)
             }
         }
 
-        /// <summary>
-        /// Pulls a reference to a contact's depth.
-        /// </summary>
-        /// <param name="manifold">Manifold to pull a reference from.</param>
-        /// <param name="contactIndex">Contact to pull data from.</param>
-        /// <returns>Reference to a contact's depth.</returns>
-        [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;
         }
 
-        /// <summary>
-        /// 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.
-        /// </summary>
-        /// <param name="manifold">Manifold to pull a reference from.</param>
-        /// <param name="contactIndex">Contact to pull data from.</param>
-        /// <returns>Reference to the contact manifold's normal.</returns>
-        [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;
         }
 
-
-        /// <summary>
-        /// Pulls a reference to a contact's offset.
-        /// </summary>
-        /// <param name="manifold">Manifold to pull a reference from.</param>
-        /// <param name="contactIndex">Contact to pull data from.</param>
-        /// <returns>Reference to a contact's offset.</returns>
-        [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;
         }
 
-        /// <summary>
-        /// Pulls a reference to a contact's feature id.
-        /// </summary>
-        /// <param name="manifold">Manifold to pull a reference from.</param>
-        /// <param name="contactIndex">Contact to pull data from.</param>
-        /// <returns>Reference to a contact's feature id.</returns>
-        [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/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 a55da12b9..782bc6ef2 100644
--- a/BepuPhysics/CollisionDetection/DepthRefiner.cs
+++ b/BepuPhysics/CollisionDetection/DepthRefiner.cs
@@ -2,18 +2,27 @@
 //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
 {
 
+    /// <summary>
+    /// Incrementally refines a sample direction to approach a local minimum depth between two convex bodies.
+    /// </summary>
+    /// <remarks>
+    /// 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.</remarks>
+    /// <typeparam name="TShapeA">Type of the first shape.</typeparam>
+    /// <typeparam name="TShapeWideA">SIMD type of the first shape.</typeparam>
+    /// <typeparam name="TSupportFinderA">Type providing support sampling for the first shape.</typeparam>
+    /// <typeparam name="TShapeB">Type of the second shape.</typeparam>
+    /// <typeparam name="TShapeWideB">SIMD type of the second shape.</typeparam>
+    /// <typeparam name="TSupportFinderB">Type providing support sampling for the second shape.</typeparam>
     public static class DepthRefiner<TShapeA, TShapeWideA, TSupportFinderA, TShapeB, TShapeWideB, TSupportFinderB>
         where TShapeA : IConvexShape
         where TShapeWideA : IShapeWide<TShapeA>
@@ -88,10 +97,8 @@ static void GetNextNormal<T>(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<float>.Zero, bestDepth), out var searchTarget);
             var terminationEpsilon = Vector.ConditionalSelect(Vector.LessThan(bestDepth, Vector<float>.Zero), convergenceThreshold - bestDepth, convergenceThreshold);
             var terminationEpsilonSquared = terminationEpsilon * terminationEpsilon;
@@ -184,7 +191,7 @@ static void GetNextNormal<T>(ref Simplex simplex, in Vector3Wide support, ref Ve
 
             var relevantFeatures = Vector<int>.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)));
 
@@ -449,10 +456,8 @@ static void GetNextNormal<T>(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<float>.Zero, bestDepth), out var searchTarget);
             var terminationEpsilon = Vector.ConditionalSelect(Vector.LessThan(bestDepth, Vector<float>.Zero), convergenceThreshold - bestDepth, convergenceThreshold);
             var terminationEpsilonSquared = terminationEpsilon * terminationEpsilon;
@@ -549,7 +554,7 @@ static void GetNextNormal<T>(ref SimplexWithWitness simplex, in Vector3Wide supp
             simplex.C.Weight = Vector.ConditionalSelect(terminatedLanes, simplex.C.Weight, Vector<float>.Zero);
             simplex.WeightDenominator = Vector.ConditionalSelect(terminatedLanes, simplex.WeightDenominator, Vector<float>.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 a8d35e745..73ebba724 100644
--- a/BepuPhysics/CollisionDetection/DepthRefiner.tt
+++ b/BepuPhysics/CollisionDetection/DepthRefiner.tt
@@ -50,6 +50,19 @@ namespace BepuPhysics.CollisionDetection
     }
 <#}#>
 
+    /// <summary>
+    /// Incrementally refines a sample direction to approach a local minimum depth between two convex bodies.
+    /// </summary>
+    /// <remarks>
+    /// 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.</remarks>
+    /// <typeparam name="TShapeA">Type of the first shape.</typeparam>
+    /// <typeparam name="TShapeWideA">SIMD type of the first shape.</typeparam>
+    /// <typeparam name="TSupportFinderA">Type providing support sampling for the first shape.</typeparam>
+    /// <typeparam name="TShapeB">Type of the second shape.</typeparam>
+    /// <typeparam name="TShapeWideB">SIMD type of the second shape.</typeparam>
+    /// <typeparam name="TSupportFinderB">Type providing support sampling for the second shape.</typeparam>
     public static class DepthRefiner<TShapeA, TShapeWideA, TSupportFinderA, TShapeB, TShapeWideB, TSupportFinderB>
         where TShapeA : IConvexShape
         where TShapeWideA : IShapeWide<TShapeA>
@@ -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<float>.Zero, bestDepth), out var searchTarget);
             var terminationEpsilon = Vector.ConditionalSelect(Vector.LessThan(bestDepth, Vector<float>.Zero), convergenceThreshold - bestDepth, convergenceThreshold);
             var terminationEpsilonSquared = terminationEpsilon * terminationEpsilon;
@@ -267,7 +278,7 @@ namespace BepuPhysics.CollisionDetection
             simplex.WeightDenominator = Vector.ConditionalSelect(terminatedLanes, simplex.WeightDenominator, Vector<float>.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/FreshnessChecker.cs b/BepuPhysics/CollisionDetection/FreshnessChecker.cs
index 903d22175..6b3aa3e10 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<PreflushJob> jobs, BufferPool pool, int mappingCount)
+        public void CreateJobs(IThreadDispatcher dispatcher, int threadCount, ref QuickList<PreflushJob> jobs, BufferPool pool, int mappingCount)
         {
+            cachedDispatcher = dispatcher;
             if (mappingCount > 0)
             {
                 if (threadCount > 1)
@@ -140,12 +143,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);
+            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]];
@@ -156,15 +157,14 @@ 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!
-            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.WorkerPools[workerIndex]);
         }
     }
 }
diff --git a/BepuPhysics/CollisionDetection/INarrowPhaseCallbacks.cs b/BepuPhysics/CollisionDetection/INarrowPhaseCallbacks.cs
index edcf29859..9bb42c2f3 100644
--- a/BepuPhysics/CollisionDetection/INarrowPhaseCallbacks.cs
+++ b/BepuPhysics/CollisionDetection/INarrowPhaseCallbacks.cs
@@ -1,14 +1,14 @@
-using System;
-using System.Collections.Generic;
-using System.Text;
-using BepuUtilities;
-using BepuPhysics.Collidables;
+using BepuPhysics.Collidables;
 using BepuPhysics.Constraints;
 
 namespace BepuPhysics.CollisionDetection
 {
+    /// <summary>
+    /// Material properties governing the interaction between colliding bodies. Used by the narrow phase to create constraints of the appropriate configuration.
+    /// </summary>
     public struct PairMaterialProperties
     {
+
         /// <summary>
         /// Coefficient of friction to apply for the constraint. Maximum friction force will be equal to the normal force times the friction coefficient.
         /// </summary>
@@ -21,9 +21,25 @@ public struct PairMaterialProperties
         /// Defines the constraint's penetration recovery spring properties.
         /// </summary>
         public SpringSettings SpringSettings;
+
+        /// <summary>
+        /// Constructs a pair's material properties.
+        /// </summary>
+        /// <param name="frictionCoefficient">Coefficient of friction to apply for the constraint. Maximum friction force will be equal to the normal force times the friction coefficient.</param>
+        /// <param name="maximumRecoveryVelocity">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. </param>
+        /// <param name="springSettings">Defines the constraint's penetration recovery spring properties.</param>
+        public PairMaterialProperties(float frictionCoefficient, float maximumRecoveryVelocity, SpringSettings springSettings)
+        {
+            FrictionCoefficient = frictionCoefficient;
+            MaximumRecoveryVelocity = maximumRecoveryVelocity;
+            SpringSettings = springSettings;
+        }
     }
 
-    public unsafe interface INarrowPhaseCallbacks
+    /// <summary>
+    /// Defines handlers for narrow phase events.
+    /// </summary>
+    public interface INarrowPhaseCallbacks
     {
         /// <summary>
         /// Performs any required initialization logic after the Simulation instance has been constructed.
@@ -37,8 +53,10 @@ public unsafe interface INarrowPhaseCallbacks
         /// <param name="workerIndex">Index of the worker that identified the overlap.</param>
         /// <param name="a">Reference to the first collidable in the pair.</param>
         /// <param name="b">Reference to the second collidable in the pair.</param>
+        /// <param name="speculativeMargin">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.</param>
         /// <returns>True if collision detection should proceed, false otherwise.</returns>
-        bool AllowContactGeneration(int workerIndex, CollidableReference a, CollidableReference b);
+        bool AllowContactGeneration(int workerIndex, CollidableReference a, CollidableReference b, ref float speculativeMargin);
 
 
         /// <summary>
@@ -58,6 +76,7 @@ public unsafe interface INarrowPhaseCallbacks
         /// <summary>
         /// Chooses whether to allow contact generation to proceed for the children of two overlapping collidables in a compound-including pair.
         /// </summary>
+        /// <param name="workerIndex">Index of the worker thread processing this pair.</param>
         /// <param name="pair">Parent pair of the two child collidables.</param>
         /// <param name="childIndexA">Index of the child of collidable A in the pair. If collidable A is not compound, then this is always 0.</param>
         /// <param name="childIndexB">Index of the child of collidable B in the pair. If collidable B is not compound, then this is always 0.</param>
@@ -76,7 +95,7 @@ public unsafe interface INarrowPhaseCallbacks
         /// <param name="childIndexA">Index of the child of collidable A in the pair. If collidable A is not compound, then this is always 0.</param>
         /// <param name="childIndexB">Index of the child of collidable B in the pair. If collidable B is not compound, then this is always 0.</param>
         /// <param name="manifold">Set of contacts detected between the collidables.</param>
-        /// <returns>True if this manifold should be considered for constraint generation, false otherwise.</returns>
+        /// <returns>True if this manifold should be considered for the parent pair's contact manifold generation, false otherwise.</returns>
         bool ConfigureContactManifold(int workerIndex, CollidablePair pair, int childIndexA, int childIndexB, ref ConvexContactManifold manifold);
         
         /// <summary>
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<TShape, TShapeWide> where TShape : IConvexShape where TShapeWide : IShapeWide<TShape>
diff --git a/BepuPhysics/CollisionDetection/InactiveSetBuilder.cs b/BepuPhysics/CollisionDetection/InactiveSetBuilder.cs
index 319311382..a7baf0e3d 100644
--- a/BepuPhysics/CollisionDetection/InactiveSetBuilder.cs
+++ b/BepuPhysics/CollisionDetection/InactiveSetBuilder.cs
@@ -1,159 +1,44 @@
-using BepuPhysics.Collidables;
-using BepuUtilities.Collections;
+using BepuUtilities.Collections;
 using BepuUtilities.Memory;
-using System.Diagnostics;
-using System.Runtime.CompilerServices;
 
 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<SleepingCache> ConstraintCaches;
-        public Buffer<SleepingCache> CollisionCaches;
         public QuickList<SleepingPair> 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<UntypedList> ConstraintCaches;
-        public Buffer<UntypedList> CollisionCaches;
         public QuickList<SleepingPair> 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<SleepingPair>(initialPairCapacity, pool);
-            InitialCapacityPerCache = initialCapacityPerCache;
         }
 
-        [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        unsafe TypedIndex CopyToBuilderCache(ref Buffer<UntypedList> sourceCaches, ref Buffer<UntypedList> 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<WorkerPairCache> 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<UntypedList> sourceCaches, BufferPool pool, out Buffer<SleepingCache> inactiveCaches, out Buffer<int> 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<int>();
-                inactiveCaches = new Buffer<SleepingCache>();
-            }
-        }
         public void FinalizeSet(BufferPool pool, out SleepingSet set)
         {
             //Repackage the gathered caches into a smaller format for longer term storage.
@@ -164,24 +49,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<SleepingPair>(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 +62,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/MeshReduction.cs b/BepuPhysics/CollisionDetection/MeshReduction.cs
index 9fdf53e26..0d08b0e61 100644
--- a/BepuPhysics/CollisionDetection/MeshReduction.cs
+++ b/BepuPhysics/CollisionDetection/MeshReduction.cs
@@ -3,15 +3,13 @@
 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
 {
-    public struct MeshReduction : ICollisionTestContinuation
+    public unsafe struct MeshReduction : ICollisionTestContinuation
     {
         /// <summary>
         /// Flag used to mark a contact as being generated by the face of a triangle in its feature id.
@@ -31,51 +29,72 @@ public struct MeshReduction : ICollisionTestContinuation
         //This uses all of the nonconvex reduction's logic, so we just nest it.
         public NonconvexReduction Inner;
 
+        //Type-erased pointer to the mesh shape data, plus two function pointers that close over the concrete mesh type.
+        //ConvexMeshContinuations<TMesh> populates these from MeshReductionThunks<TMesh> 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 'way too many subpairs' path's per-contact calls cheap without requiring
+        //CollisionBatcher to know about TMesh.
+        public void* Mesh;
+        public delegate*<void*, Vector3, Vector3, BufferPool, Shapes, ref ChildEnumerator, void> FindLocalOverlapsThunk;
+        public delegate*<void*, int, out Triangle, void> GetLocalChildThunk;
+
         public void Create(int childManifoldCount, BufferPool pool)
         {
             Inner.Create(childManifoldCount, pool);
         }
 
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public unsafe void OnChildCompleted<TCallbacks>(ref PairContinuation report, ref ConvexContactManifold manifold, ref CollisionBatcher<TCallbacks> batcher)
+        public void OnChildCompleted<TCallbacks>(ref PairContinuation report, ref ConvexContactManifold manifold, ref CollisionBatcher<TCallbacks> batcher)
             where TCallbacks : struct, ICollisionCallbacks
         {
             Inner.OnChildCompleted(ref report, ref manifold, ref batcher);
         }
 
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public void OnChildCompletedEmpty<TCallbacks>(ref PairContinuation report, ref CollisionBatcher<TCallbacks> batcher) where TCallbacks : struct, ICollisionCallbacks
+        public void OnUntestedChildCompleted<TCallbacks>(ref PairContinuation report, ref CollisionBatcher<TCallbacks> batcher) where TCallbacks : struct, ICollisionCallbacks
         {
-            Inner.OnChildCompletedEmpty(ref report, ref batcher);
+            Inner.OnUntestedChildCompleted(ref report, ref batcher);
         }
 
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        private static unsafe void ComputeMeshSpaceContacts(ref ConvexContactManifold manifold, in Matrix3x3 inverseMeshOrientation, bool requiresFlip, Vector3* meshSpaceContacts, out Vector3 meshSpaceNormal)
+        private static void ComputeMeshSpaceContact(ref ConvexContactManifold manifold, in Matrix3x3 inverseMeshOrientation, bool requiresFlip, out Vector3 meshSpaceContact, out Vector3 meshSpaceNormal)
         {
-            //First, if the manifold considers the mesh and its triangles to be shape B, then we need to flip it.
-            if (requiresFlip)
+            //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)
             {
-                //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)
+                var depth = Unsafe.Add(ref manifold.Contact0, j).Depth;
+                if (deepestDepth < depth)
                 {
-                    Matrix3x3.Transform(Unsafe.Add(ref manifold.Contact0, i).Offset - manifold.OffsetB, inverseMeshOrientation, out meshSpaceContacts[i]);
+                    deepestDepth = depth;
+                    deepestIndex = j;
                 }
+            }
+            if (requiresFlip)
+            {
+                //If the manifold considers the mesh and its triangles to be shape B, it needs to be flipped before being transformed.
+                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);
             }
         }
 
         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;
@@ -131,7 +150,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 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. 
@@ -154,7 +173,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)
@@ -170,10 +189,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
@@ -182,7 +204,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 
@@ -190,10 +211,10 @@ 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-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 +226,84 @@ private static unsafe bool ShouldBlockNormal(in TestTriangle triangle, in Vector
             return false;
         }
 
-        public unsafe static void ReduceManifolds(ref Buffer<Triangle> continuationTriangles, ref Buffer<NonconvexReductionChild> continuationChildren, int start, int count,
-           bool requiresFlip, in BoundingBox queryBounds, in Matrix3x3 meshOrientation, in Matrix3x3 meshInverseOrientation)
+        //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<NonconvexReductionChild> 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;
+                    }
+                }
+            }
+        }
+
+        public struct ChildEnumerator : IBreakableForEach<int>
+        {
+            public QuickList<int> List;
+            public BufferPool Pool;
+            public bool LoopBody(int i)
+            {
+                List.Allocate(Pool) = i;
+                return true;
+            }
+        }
+
+        public static void ReduceManifolds(ref Buffer<Triangle> continuationTriangles, ref Buffer<NonconvexReductionChild> continuationChildren, int start, int count,
+           bool requiresFlip, in BoundingBox queryBounds, in Matrix3x3 meshOrientation, in Matrix3x3 meshInverseOrientation,
+           void* mesh,
+           delegate*<void*, Vector3, Vector3, BufferPool, Shapes, ref ChildEnumerator, void> findLocalOverlapsThunk,
+           delegate*<void*, int, out Triangle, void> 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,
@@ -228,155 +325,210 @@ public unsafe static void ReduceManifolds(ref Buffer<Triangle> 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.
-
-            //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 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;
+
             //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.
-            //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<TestTriangle>(memory, count);
-            for (int i = 0; i < count; ++i)
+            const int bruteForceThreshold = 128;
+            //Console.WriteLine($"count: {count}");
+            if (count < bruteForceThreshold)
             {
-                if (children[i].Manifold.Count > 0)
+                //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<TestTriangle>(memory, count);
+                for (int i = 0; i < count; ++i)
                 {
-                    activeTriangles[activeChildCount] = new TestTriangle(triangles[i], i);
-                    ++activeChildCount;
+                    activeTriangles[i] = new TestTriangle(triangles[i], i);
                 }
-            }
-            var meshSpaceContacts = stackalloc Vector3[4];
-            for (int i = 0; i < activeChildCount; ++i)
-            {
-                ref var sourceTriangle = ref activeTriangles[i];
-                ref var sourceChild = ref children[sourceTriangle.ChildIndex];
-                //Can't correct contacts that were created by face collisions.
-                if ((sourceChild.Manifold.Contact0.FeatureId & FaceCollisionFlag) == 0)
+
+                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 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)
                     {
-                        var depth = Unsafe.Add(ref sourceChild.Manifold.Contact0, j).Depth;
-                        if (deepestDepth < depth)
-                        {
-                            deepestDepth = depth;
-                            deepestIndex = j;
-                        }
-                    }
-                    ref var meshSpaceContact = ref meshSpaceContacts[deepestIndex];
-                    for (int j = 0; j < activeChildCount; ++j)
-                    {
-                        //No point in trying to check a normal against its own triangle.
-                        if (i != j)
+                        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)
                         {
                             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);
+                                //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;
                             }
                         }
+
+                        //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");
+                        //}
                     }
-                    //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;
                         }
                     }
                 }
-                else
+                //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)
                 {
-                    //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 < activeChildCount; ++i)
+            else
             {
-                ref var triangle = ref activeTriangles[i];
-                if (triangle.Blocked)
+                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.
+                var allocationSize = count * 2;
+                enumerator.Pool = pool;
+                enumerator.List = new QuickList<int>(allocationSize, pool);
+                QuickDictionary<int, TestTriangle, PrimitiveComparer<int>> 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 guaranteed to encounter all the triangles with contacts that we collected, so go ahead and create their entries.
+                if (requiresFlip)
                 {
-                    ref var manifold = ref children[triangle.ChildIndex].Manifold;
-                    if (triangle.ForceDeletionOnBlock)
+                    for (int i = 0; i < count; ++i)
                     {
-                        //The manifold was infringing, and no other manifold infringed upon it. Can safely just ignore the manifold completely.
-                        manifold.Count = 0;
+                        ref var child = ref children[i];
+                        if (child.Manifold.Count > 0)
+                            testTriangles.AddUnsafely(child.ChildIndexB, new TestTriangle(triangles[i], i));
                     }
-                    else
+                }
+                else
+                {
+                    for (int i = 0; i < count; ++i)
                     {
-                        var manifoldHasPositiveDepth = false;
-                        for (int j = 0; j < manifold.Count; ++j)
+                        ref var child = ref children[i];
+                        if (child.Manifold.Count > 0)
+                            testTriangles.AddUnsafely(child.ChildIndexA, new TestTriangle(triangles[i], 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.
+                    if ((sourceChild.Manifold.Contact0.FeatureId & FaceCollisionFlag) == 0)
+                    {
+                        ComputeMeshSpaceContact(ref sourceChild.Manifold, meshInverseOrientation, requiresFlip, out var meshSpaceContact, out var meshSpaceNormal);
+                        var contactQueryMin = meshSpaceContact - contactExpansion;
+                        var contactQueryMax = meshSpaceContact + contactExpansion;
+                        enumerator.List.Count = 0;
+                        //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);
+                        for (int j = 0; j < enumerator.List.Count; ++j)
                         {
-                            if (Unsafe.Add(ref manifold.Contact0, j).Depth > 0)
+                            var triangleIndexInMesh = enumerator.List[j];
+                            if (!testTriangles.FindOrAllocateSlotUnsafely(triangleIndexInMesh, out var triangleIndex))
+                            {
+                                //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.
+                                getLocalChildThunk(mesh, triangleIndexInMesh, out var triangle);
+                                testTriangles.Values[triangleIndex] = new TestTriangle(triangle, triangleIndex);
+                            }
+                            ref var targetTriangle = ref testTriangles.Values[triangleIndex];
+
+                            if (ShouldBlockNormal(targetTriangle, meshSpaceContact, meshSpaceNormal))
                             {
-                                manifoldHasPositiveDepth = true;
+                                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;
                             }
                         }
-                        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 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.
-                        }
-                        else
+                        //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
+                    {
+                        //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 < activeChildCount; ++i)
+                {
+                    TryApplyBlockToTriangle(ref testTriangles.Values[i], children, meshOrientation, requiresFlip);
+                }
+
+                testTriangles.Dispose(pool);
+                enumerator.List.Dispose(pool);
             }
+
+
         }
 
         //[MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public unsafe bool TryFlush<TCallbacks>(int pairId, ref CollisionBatcher<TCallbacks> batcher) where TCallbacks : struct, ICollisionCallbacks
+        public bool TryFlush<TCallbacks>(int pairId, ref CollisionBatcher<TCallbacks> batcher) where TCallbacks : struct, ICollisionCallbacks
         {
             Debug.Assert(Inner.ChildCount > 0);
             if (Inner.CompletedChildCount == Inner.ChildCount)
             {
                 Matrix3x3.CreateFromQuaternion(MeshOrientation, out var meshOrientation);
                 Matrix3x3.Transpose(meshOrientation, out var meshInverseOrientation);
-
-                ReduceManifolds(ref Triangles, ref Inner.Children, 0, Inner.ChildCount, RequiresFlip, QueryBounds, meshOrientation, meshInverseOrientation);
+                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);
@@ -387,4 +539,28 @@ public unsafe bool TryFlush<TCallbacks>(int pairId, ref CollisionBatcher<TCallba
         }
 
     }
+
+    /// <summary>
+    /// 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 <typeparamref name="TMesh"/> are devirtualized.
+    /// </summary>
+    /// <typeparam name="TMesh">Concrete homogeneous triangle compound shape type.</typeparam>
+    public static unsafe class MeshReductionThunks<TMesh> where TMesh : struct, IHomogeneousCompoundShape<Triangle, TriangleWide>
+    {
+        public static readonly delegate*<void*, Vector3, Vector3, BufferPool, Shapes, ref MeshReduction.ChildEnumerator, void> FindLocalOverlaps = &FindLocalOverlapsImpl;
+        public static readonly delegate*<void*, int, out Triangle, void> GetLocalChild = &GetLocalChildImpl;
+
+        [MethodImpl(MethodImplOptions.AggressiveInlining)]
+        static void FindLocalOverlapsImpl(void* mesh, Vector3 min, Vector3 max, BufferPool pool, Shapes shapes, ref MeshReduction.ChildEnumerator enumerator)
+        {
+            Unsafe.AsRef<TMesh>(mesh).FindLocalOverlaps(min, max, pool, shapes, ref enumerator);
+        }
+
+        [MethodImpl(MethodImplOptions.AggressiveInlining)]
+        static void GetLocalChildImpl(void* mesh, int childIndex, out Triangle triangle)
+        {
+            Unsafe.AsRef<TMesh>(mesh).GetLocalChild(childIndex, out triangle);
+        }
+    }
 }
diff --git a/BepuPhysics/CollisionDetection/NarrowPhase.cs b/BepuPhysics/CollisionDetection/NarrowPhase.cs
index e89c4fe7b..e1ab13fb0 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;
@@ -81,7 +78,7 @@ public struct NarrowPhaseFlushJob
         public int Index;
     }
 
-    public abstract class NarrowPhase
+    public unsafe abstract class NarrowPhase
     {
         public Simulation Simulation;
         public BufferPool Pool;
@@ -202,13 +199,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)
             {
@@ -222,7 +218,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);
@@ -237,8 +233,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<NarrowPhaseFlushJob>(128, Pool);
             PairCache.PrepareFlushJobs(ref flushJobs);
             var removalBatchJobCount = ConstraintRemover.CreateFlushJobs(deterministic);
@@ -263,18 +261,17 @@ public void Flush(IThreadDispatcher threadDispatcher = null)
             {
                 for (int i = 0; i < flushJobs.Count; ++i)
                 {
-                    ExecuteFlushJob(ref flushJobs[i], Pool);
+                    ExecuteFlushJob(ref flushJobs[i]);
                 }
             }
             else
             {
                 flushJobIndex = -1;
                 this.threadDispatcher = threadDispatcher;
-                threadDispatcher.DispatchWorkers(flushWorkerLoop);
+                threadDispatcher.DispatchWorkers(flushWorkerLoop, flushJobs.Count);
+                //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();
@@ -297,16 +294,40 @@ 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.
-
+        /// <summary>
+        /// Sorts references to guarantee that two collidables in the same pair will always be in the same order.
+        /// </summary>
+        /// <param name="a">First collidable reference to sort.</param>
+        /// <param name="b">First collidable reference to sort.</param>
+        /// <param name="aMobility">Mobility extracted from collidable A.</param>
+        /// <param name="bMobility">Mobility extracted from collidable B.</param>
+        [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;
+            }
+        }
     }
 
     /// <summary>
     /// Turns broad phase overlaps into contact manifolds and uses them to manage constraints in the solver.
     /// </summary>
     /// <typeparam name="TCallbacks">Type of the callbacks to use.</typeparam>
-    public partial class NarrowPhase<TCallbacks> : NarrowPhase where TCallbacks : struct, INarrowPhaseCallbacks
+    public unsafe partial class NarrowPhase<TCallbacks> : NarrowPhase where TCallbacks : struct, INarrowPhaseCallbacks
     {
         public TCallbacks Callbacks;
         public struct OverlapWorker
@@ -327,7 +348,7 @@ public OverlapWorker(int workerIndex, BufferPool pool, NarrowPhase<TCallbacks> 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;
@@ -340,7 +361,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;
         }
@@ -354,7 +375,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);
             }
         }
 
@@ -375,39 +396,60 @@ 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!");
-            //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)
+            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.");
+
+            //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.DynamicsState[bodyLocationA.Index];
+            ref var collidableA = ref setA.Collidables[bodyLocationA.Index];
+            float speculativeMarginB;
+            if (twoBodies)
+            {
+                ref var bodyLocationB = ref Bodies.HandleToLocation[b.BodyHandle.Value];
+                ref var collidableB = ref Bodies.Sets[bodyLocationB.SetIndex].Collidables[bodyLocationB.Index];
+                speculativeMarginB = collidableB.SpeculativeMargin;
+            }
+            else
             {
-                var temp = b;
-                b = a;
-                a = temp;
+                //Slot B is a static.
+                speculativeMarginB = 0;
             }
-            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))
+
+            //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.
+            //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 = 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 stateB = ref setB.DynamicsState[bodyLocationB.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 setA.Poses[bodyLocationA.Index], ref setB.Poses[bodyLocationB.Index],
-                    ref setA.Velocities[bodyLocationA.Index], ref setB.Velocities[bodyLocationB.Index]);
+                    ref collidableA.Continuity, ref collidableB.Continuity,
+                    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);
             }
             else
             {
@@ -417,20 +459,22 @@ 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 staticB = ref Statics.GetDirectReference(b.StaticHandle);
                 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 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);
             }
 
         }
 
-        unsafe struct CCDSweepFilter : ISweepFilter
+        struct CCDSweepFilter : ISweepFilter
         {
             public NarrowPhase<TCallbacks> NarrowPhase;
             public CollidablePair Pair;
@@ -444,18 +488,17 @@ 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)
+        private void AddBatchEntries(int workerIndex, ref OverlapWorker overlapWorker,
+            ref CollidablePair pair,
+            ref ContinuousDetection continuityA, ref ContinuousDetection continuityB,
+            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);
-            //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);
-            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
@@ -466,9 +509,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
@@ -483,19 +526,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);
+                    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)
                     {
-                        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
                         {
@@ -503,10 +546,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
                         {
@@ -515,8 +558,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),
@@ -532,7 +575,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));
                         }
@@ -544,7 +587,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/CollisionDetection/NarrowPhaseCCDContinuations.cs b/BepuPhysics/CollisionDetection/NarrowPhaseCCDContinuations.cs
index 54663ebdc..d0d98ee64 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
 {
@@ -54,7 +48,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,16 +114,15 @@ 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);
             }
 
             [MethodImpl(MethodImplOptions.AggressiveInlining)]
-            public unsafe void OnPairCompleted<TManifold>(int pairId, ref TManifold manifoldReference) where TManifold : unmanaged, IContactManifold<TManifold>
+            public void OnPairCompleted<TManifold>(int pairId, ref TManifold manifoldReference) where TManifold : unmanaged, IContactManifold<TManifold>
             {
-                var todoTestCollisionCache = default(EmptyCollisionCache);
                 CCDContinuationIndex continuationId = new CCDContinuationIndex(pairId);
                 Debug.Assert(continuationId.Exists);
                 var continuationIndex = continuationId.Index;
@@ -137,11 +130,11 @@ public unsafe void OnPairCompleted<TManifold>(int pairId, ref TManifold manifold
                 //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)
@@ -150,7 +143,7 @@ public unsafe void OnPairCompleted<TManifold>(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 +177,7 @@ public unsafe void OnPairCompleted<TManifold>(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;
@@ -217,9 +210,16 @@ 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)
             {
-                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()
diff --git a/BepuPhysics/CollisionDetection/NarrowPhaseConstraintUpdate.cs b/BepuPhysics/CollisionDetection/NarrowPhaseConstraintUpdate.cs
index 079fd6cf4..965822f1d 100644
--- a/BepuPhysics/CollisionDetection/NarrowPhaseConstraintUpdate.cs
+++ b/BepuPhysics/CollisionDetection/NarrowPhaseConstraintUpdate.cs
@@ -1,12 +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;
 
 namespace BepuPhysics.CollisionDetection
 {
@@ -19,14 +15,6 @@ public struct TwoBodyHandles
         public int B;
     }
 
-    /// <summary>
-    /// Special type for collision pairs that do not need to store any supplementary information.
-    /// </summary>
-    struct EmptyCollisionCache : IPairCacheEntry
-    {
-        public int CacheTypeId => -1;
-    }
-
     public struct ContactImpulses1
     {
         public float Impulse0;
@@ -144,37 +132,31 @@ private unsafe void RedistributeImpulses<TContactImpulses>(
         }
 
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        unsafe void RequestAddConstraint<TDescription, TBodyHandles, TContactImpulses>(int workerIndex, int manifoldConstraintType,
-            ref CollidablePair pair, PairCacheIndex constraintCacheIndex, ref TContactImpulses newImpulses,
-            ref TDescription description, TBodyHandles bodyHandles) where TDescription : unmanaged, IConstraintDescription<TDescription>
+        void RequestAddConstraint<TDescription, TBodyHandles, TContactImpulses>(int workerIndex, int manifoldConstraintType,
+            CollidablePair pair, PairCacheChangeIndex pairCacheChange, ref TContactImpulses newImpulses,
+            ref TDescription description, TBodyHandles bodyHandles) where TBodyHandles : unmanaged where TDescription : unmanaged, IConstraintDescription<TDescription>
         {
             //Note that this branch is (was?) JIT constant.
             if (typeof(TBodyHandles) != typeof(TwoBodyHandles) && typeof(TBodyHandles) != typeof(int))
             {
                 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<TBodyHandles, TDescription, TContactImpulses, TCollisionCache, TConstraintCache>(int workerIndex, ref CollidablePair pair,
-            int manifoldTypeAsConstraintType, ref TConstraintCache newConstraintCache, ref TCollisionCache collisionCache,
-            ref TDescription description, TBodyHandles bodyHandles)
-            where TConstraintCache : unmanaged, IPairCacheEntry
-            where TCollisionCache : unmanaged, IPairCacheEntry
+        public unsafe void UpdateConstraint<TBodyHandles, TDescription, TContactImpulses>(int workerIndex, CollidablePair pair,
+            int manifoldTypeAsConstraintType, ref ConstraintCache newConstraintCache, int newContactCount, ref TDescription description, TBodyHandles bodyHandles)
+            where TBodyHandles : unmanaged
             where TDescription : unmanaged, IConstraintDescription<TDescription>
             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;
-                Solver.GetConstraintReference(constraintHandle, out var constraintReference);
+                ref var cache = ref PairCache.GetCache(index);
+                var oldConstraintHandle = cache.ConstraintHandle;
+                var constraintReference = Solver.GetConstraintReference(oldConstraintHandle);
                 Debug.Assert(
                     constraintReference.typeBatchPointer != null &&
                     constraintReference.IndexInTypeBatch >= 0 &&
@@ -192,19 +174,17 @@ public unsafe void UpdateConstraint<TBodyHandles, TDescription, TContactImpulses
 #endif
                 //The first slot in the constraint cache is the constraint handle; the following slots are feature ids.
                 RedistributeImpulses(
-                    accessor.ContactCount, (int*)oldConstraintCachePointer + 1, oldImpulses,
-                    newConstraintCache.CacheTypeId + 1, ref Unsafe.Add(ref Unsafe.As<TConstraintCache, int>(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<TConstraintCache, ConstraintHandle>(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(ref constraintReference, ref description);
+                    Solver.ApplyDescriptionWithoutWaking(constraintReference, description);
                     accessor.ScatterNewImpulses(ref constraintReference, ref newImpulses);
                 }
                 else
@@ -212,19 +192,19 @@ public unsafe void UpdateConstraint<TBodyHandles, TDescription, TContactImpulses
                     //There exists a constraint, but it's a different type. This is more complex:
                     //1) The new manifold's constraint must be added, but upon the adder's return the solver does not yet contain the constraint. They are deferred.
                     //2) The old constraint must be removed.
-                    PairCache.Update(workerIndex, index, ref pointers, ref collisionCache, ref newConstraintCache);
-                    RequestAddConstraint(workerIndex, manifoldTypeAsConstraintType, ref pair, pointers.ConstraintCache, ref newImpulses, ref description, bodyHandles);
-                    ConstraintRemover.EnqueueRemoval(workerIndex, constraintHandle);
+                    var pairCacheChange = PairCache.Update(index, newConstraintCache);
+                    RequestAddConstraint(workerIndex, manifoldTypeAsConstraintType, pair, pairCacheChange, ref newImpulses, ref description, bodyHandles);
+                    ConstraintRemover.EnqueueRemoval(workerIndex, oldConstraintHandle);
                 }
             }
             else
             {
                 //No preexisting constraint; add a fresh constraint and pair cache entry.
                 //The pair cache entry has to be created first so that the adder has a place to put the result of the constraint add.
-                var constraintCacheIndex = PairCache.Add(workerIndex, ref pair, ref collisionCache, ref newConstraintCache);
+                var pendingPairCacheChange = PairCache.Add(workerIndex, pair, newConstraintCache);
                 var newImpulses = default(TContactImpulses);
                 //TODO: It would be nice to avoid the impulse scatter for fully new constraints; it's going to be all zeroes regardless. Worth investigating later.
-                RequestAddConstraint(workerIndex, manifoldTypeAsConstraintType, ref pair, constraintCacheIndex, ref newImpulses, ref description, bodyHandles);
+                RequestAddConstraint(workerIndex, manifoldTypeAsConstraintType, pair, pendingPairCacheChange, ref newImpulses, ref description, bodyHandles);
                 //This is a new connection in the constraint graph, so we must check to see if any involved body is inactive.
                 //Note that this is only possible when both colliders are bodies. If only one collider is a body, then it must be active otherwise this pair would never have been tested.
                 if (typeof(TBodyHandles) == typeof(TwoBodyHandles))
@@ -269,9 +249,8 @@ static int GetConvexConstraintTypeId<TBodyHandles>(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<TContactManifold, TCollisionCache, TBodyHandles>(
-            int workerIndex, ref CollidablePair pair, ref TContactManifold manifold, ref TCollisionCache collisionCache, ref PairMaterialProperties material, TBodyHandles bodyHandles)
-            where TCollisionCache : unmanaged, IPairCacheEntry
+        void UpdateConstraintForManifold<TContactManifold, TBodyHandles>(
+            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.
@@ -306,12 +285,10 @@ unsafe void UpdateConstraintForManifold<TContactManifold, TCollisionCache, TBody
                         manifoldTypeAsConstraintType += 7;
                 }
             }
-            contactConstraintAccessors[manifoldTypeAsConstraintType].UpdateConstraintForManifold(this, manifoldTypeAsConstraintType, workerIndex, ref pair, ref manifold, ref collisionCache, ref material, bodyHandles);
+            contactConstraintAccessors[manifoldTypeAsConstraintType].UpdateConstraintForManifold(this, manifoldTypeAsConstraintType, workerIndex, ref pair, ref manifold, ref material, bodyHandles);
         }
 
-        public unsafe void UpdateConstraintsForPair<TContactManifold, TCollisionCache>(int workerIndex, ref CollidablePair pair, ref TContactManifold manifold, ref TCollisionCache collisionCache) 
-            where TCollisionCache : unmanaged, IPairCacheEntry
-            where TContactManifold : unmanaged, IContactManifold<TContactManifold>
+        public void UpdateConstraintsForPair<TContactManifold>(int workerIndex, CollidablePair pair, ref TContactManifold manifold) where TContactManifold : unmanaged, IContactManifold<TContactManifold>
         {
             //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.
@@ -331,13 +308,13 @@ public unsafe void UpdateConstraintsForPair<TContactManifold, TCollisionCache>(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 2bee68413..bd59de91d 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
 {
@@ -22,11 +15,12 @@ public partial class NarrowPhase<TCallbacks>
         public struct PendingConstraintAddCache
         {
             BufferPool pool;
-            struct PendingConstraint<TBodyHandles, TDescription, TContactImpulses> where TDescription : unmanaged, IConstraintDescription<TDescription>
+            [StructLayout(LayoutKind.Sequential)]
+            struct PendingConstraint<TBodyHandles, TDescription, TContactImpulses> where TBodyHandles : unmanaged where TDescription : unmanaged, IConstraintDescription<TDescription>
             {
                 //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 +41,14 @@ public PendingConstraintAddCache(BufferPool pool, int minimumConstraintCountPerC
             }
 
             public unsafe void AddConstraint<TBodyHandles, TDescription, TContactImpulses>(int manifoldConstraintType,
-                ref CollidablePair pair, PairCacheIndex constraintCacheIndex, TBodyHandles bodyHandles, ref TDescription constraintDescription, ref TContactImpulses impulses)
-                where TDescription : unmanaged, IConstraintDescription<TDescription>
+                CollidablePair pair, PairCacheChangeIndex pairCacheChange, TBodyHandles bodyHandles, ref TDescription constraintDescription, ref TContactImpulses impulses)
+                where TBodyHandles : unmanaged where TDescription : unmanaged, IConstraintDescription<TDescription>
             {
                 ref var cache = ref pendingConstraintsByType[manifoldConstraintType];
                 var byteIndex = cache.Allocate<PendingConstraint<TBodyHandles, TDescription, TContactImpulses>>(minimumConstraintCountPerCache, pool);
                 ref var pendingAdd = ref Unsafe.AsRef<PendingConstraint<TBodyHandles, TDescription, TContactImpulses>>(cache.Buffer.Memory + byteIndex);
                 pendingAdd.Pair = pair;
-                pendingAdd.ConstraintCacheIndex = constraintCacheIndex;
+                pendingAdd.PairCacheChange = pairCacheChange;
                 pendingAdd.BodyHandles = bodyHandles;
                 pendingAdd.ConstraintDescription = constraintDescription;
                 pendingAdd.Impulses = impulses;
@@ -62,7 +56,7 @@ public unsafe void AddConstraint<TBodyHandles, TDescription, TContactImpulses>(i
 
             [MethodImpl(MethodImplOptions.AggressiveInlining)]
             public static unsafe void SequentialAddToSimulation<TBodyHandles, TDescription, TContactImpulses>(ref UntypedList list, int narrowPhaseConstraintTypeId, Simulation simulation, PairCache pairCache)
-                where TDescription : unmanaged, IConstraintDescription<TDescription>
+                 where TBodyHandles : unmanaged where TDescription : unmanaged, IConstraintDescription<TDescription>
             {
                 if (list.Buffer.Allocated)
                 {
@@ -73,8 +67,8 @@ public static unsafe void SequentialAddToSimulation<TBodyHandles, TDescription,
                     {
                         ref var add = ref Unsafe.Add(ref start, i);
                         //Unsafe.AsPointer is not a GC hole here; it's coming from unmanaged memory.
-                        var handle = simulation.Solver.Add(new Span<BodyHandle>(Unsafe.AsPointer(ref add.BodyHandles), typeof(TBodyHandles) == typeof(TwoBodyHandles) ? 2 : 1), ref add.ConstraintDescription);
-                        pairCache.CompleteConstraintAdd(simulation.NarrowPhase, simulation.Solver, ref add.Impulses, add.ConstraintCacheIndex, handle, ref add.Pair);
+                        var handle = simulation.Solver.Add(new Span<BodyHandle>(Unsafe.AsPointer(ref add.BodyHandles), typeof(TBodyHandles) == typeof(TwoBodyHandles) ? 2 : 1), add.ConstraintDescription);
+                        pairCache.CompleteConstraintAdd(simulation.NarrowPhase, simulation.Solver, ref add.Impulses, add.PairCacheChange, handle, ref add.Pair);
                     }
                 }
             }
@@ -96,7 +90,7 @@ internal void FlushSequentially(Simulation simulation, PairCache pairCache)
             [MethodImpl(MethodImplOptions.AggressiveInlining)]
             static unsafe void AddToSimulationSpeculative<TBodyHandles, TDescription, TContactImpulses>(
                 ref PendingConstraint<TBodyHandles, TDescription, TContactImpulses> constraint, int batchIndex, Simulation simulation, PairCache pairCache)
-                where TDescription : unmanaged, IConstraintDescription<TDescription>
+                where TBodyHandles : unmanaged where TDescription : unmanaged, IConstraintDescription<TDescription>
             {
                 //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.
@@ -120,14 +114,17 @@ static unsafe void AddToSimulationSpeculative<TBodyHandles, TDescription, TConta
                 ConstraintHandle constraintHandle;
                 ConstraintReference reference;
                 var handles = new Span<BodyHandle>(Unsafe.AsPointer(ref constraint.BodyHandles), typeof(TBodyHandles) == typeof(TwoBodyHandles) ? 2 : 1);
+                Span<BodyHandle> blockingBodyHandles = stackalloc BodyHandle[handles.Length];
+                Span<int> encodedBodyIndices = stackalloc int[handles.Length];
+                simulation.Solver.GetBlockingBodyHandles(handles, ref blockingBodyHandles, encodedBodyIndices);
                 while (!simulation.Solver.TryAllocateInBatch(
-                    default(TDescription).ConstraintTypeId, batchIndex,
-                    handles, out constraintHandle, out reference))
+                    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.
                     ++batchIndex;
                 }
-                simulation.Solver.ApplyDescriptionWithoutWaking(ref reference, ref constraint.ConstraintDescription);
+                simulation.Solver.ApplyDescriptionWithoutWaking(reference, constraint.ConstraintDescription);
                 ref var aLocation = ref simulation.Bodies.HandleToLocation[handles[0].Value];
                 Debug.Assert(aLocation.SetIndex == 0, "By the time we flush new constraints into the solver, all associated islands should be awake.");
                 simulation.Bodies.AddConstraint(aLocation.Index, constraintHandle, 0);
@@ -137,7 +134,7 @@ static unsafe void AddToSimulationSpeculative<TBodyHandles, TDescription, TConta
                     Debug.Assert(bLocation.SetIndex == 0, "By the time we flush new constraints into the solver, all associated islands should have be awake.");
                     simulation.Bodies.AddConstraint(bLocation.Index, constraintHandle, 1);
                 }
-                pairCache.CompleteConstraintAdd(simulation.NarrowPhase, simulation.Solver, ref constraint.Impulses, constraint.ConstraintCacheIndex, constraintHandle, ref constraint.Pair);
+                pairCache.CompleteConstraintAdd(simulation.NarrowPhase, simulation.Solver, ref constraint.Impulses, constraint.PairCacheChange, constraintHandle, ref constraint.Pair);
             }
 
 
@@ -145,7 +142,7 @@ static unsafe void AddToSimulationSpeculative<TBodyHandles, TDescription, TConta
             [MethodImpl(MethodImplOptions.AggressiveInlining)]
             public static unsafe void SequentialAddToSimulationSpeculative<TBodyHandles, TDescription, TContactImpulses>(
                 ref UntypedList list, int narrowPhaseConstraintTypeId, ref Buffer<Buffer<ushort>> speculativeBatchIndices, Simulation simulation, PairCache pairCache)
-                where TDescription : unmanaged, IConstraintDescription<TDescription>
+                where TBodyHandles : unmanaged where TDescription : unmanaged, IConstraintDescription<TDescription>
             {
                 if (list.Buffer.Allocated)
                 {
@@ -177,7 +174,7 @@ internal void FlushWithSpeculativeBatches(Simulation simulation, ref PairCache p
             [MethodImpl(MethodImplOptions.AggressiveInlining)]
             public static unsafe void DeterministicAdd<TBodyHandles, TDescription, TContactImpulses>(
                 int typeIndex, ref SortConstraintTarget target, OverlapWorker[] overlapWorkers, Simulation simulation, ref PairCache pairCache)
-                where TDescription : unmanaged, IConstraintDescription<TDescription>
+                where TBodyHandles : unmanaged where TDescription : unmanaged, IConstraintDescription<TDescription>
             {
                 ref var cache = ref overlapWorkers[target.WorkerIndex].PendingConstraints;
                 ref var constraint = ref Unsafe.As<byte, PendingConstraint<TBodyHandles, TDescription, TContactImpulses>>(
@@ -197,11 +194,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<BodyHandle>(list.Buffer.Memory + byteIndex, bodyCount));
+                    speculativeBatchIndicesForType[i] = (ushort)solver.FindCandidateBatch(*(CollidablePair*)(list.Buffer.Memory + byteIndex));
                     byteIndex += list.ElementSizeInBytes;
                 }
             }
@@ -259,11 +255,11 @@ internal int CountConstraints()
         }
 
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        void AddConstraint<TBodyHandles, TDescription, TContactImpulses>(int workerIndex, int manifoldConstraintType, ref CollidablePair pair,
-            PairCacheIndex constraintCacheIndex, ref TContactImpulses impulses, TBodyHandles bodyHandles, ref TDescription constraintDescription)
-            where TDescription : unmanaged, IConstraintDescription<TDescription>
+        void AddConstraint<TBodyHandles, TDescription, TContactImpulses>(int workerIndex, int manifoldConstraintType, CollidablePair pair,
+            PairCacheChangeIndex pairCacheChange, ref TContactImpulses impulses, TBodyHandles bodyHandles, ref TDescription constraintDescription)
+            where TBodyHandles : unmanaged where TDescription : unmanaged, IConstraintDescription<TDescription>
         {
-            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 7c468a930..63d129683 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
@@ -91,7 +89,7 @@ internal struct PreflushJob
         public int JobIndex;
     }
 
-    public partial class NarrowPhase<TCallbacks>
+    public unsafe partial class NarrowPhase<TCallbacks>
     {
         public struct SortConstraintTarget
         {
@@ -122,13 +120,13 @@ void PreflushWorkerLoop(int workerIndex)
         struct PendingConstraintComparer : IComparerRef<SortConstraintTarget>
         {
             [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<SortConstraintTarget> list, int typeIndex, int workerCount)
+        void BuildSortingTargets(ref QuickList<SortConstraintTarget> list, int typeIndex, int workerCount)
         {
             for (int i = 0; i < workerCount; ++i)
             {
@@ -153,7 +151,7 @@ unsafe void BuildSortingTargets(ref QuickList<SortConstraintTarget> list, int ty
             }
         }
 
-        unsafe void ExecutePreflushJob(int workerIndex, ref PreflushJob job)
+        void ExecutePreflushJob(int workerIndex, ref PreflushJob job)
         {
             switch (job.Type)
             {
@@ -205,7 +203,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;
@@ -322,11 +320,8 @@ protected override void OnPreflush(IThreadDispatcher threadDispatcher, bool dete
                 var originalPairCacheMappingCount = PairCache.Mapping.Count;
                 //var start = Stopwatch.GetTimestamp();
                 preflushJobIndex = -1;
-                threadDispatcher.DispatchWorkers(preflushWorkerLoop);
-                //for (int i = 0; i < preflushJobs.Count; ++i)
-                //{
-                //    ExecutePreflushJob(0, ref preflushJobs[i]);
-                //}
+                threadDispatcher.DispatchWorkers(preflushWorkerLoop, preflushJobs.Count);
+                //preflushWorkerLoop(0);
                 //var end = Stopwatch.GetTimestamp();
                 //Console.WriteLine($"Preflush phase 1 time (us): {1e6 * (end - start) / Stopwatch.Frequency}");
 
@@ -338,14 +333,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]);
-                //}
+                threadDispatcher.DispatchWorkers(preflushWorkerLoop, preflushJobs.Count);
+                //preflushWorkerLoop(0);
                 //end = Stopwatch.GetTimestamp();
                 //Console.WriteLine($"Preflush phase 2 time (us): {1e6 * (end - start) / Stopwatch.Frequency}");
 
@@ -365,15 +356,12 @@ 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);
-                //for (int i = 0; i < preflushJobs.Count; ++i)
-                //{
-                //    ExecutePreflushJob(0, ref preflushJobs[i]);
-                //}
+                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}");
 
@@ -417,7 +405,6 @@ protected override void OnPreflush(IThreadDispatcher threadDispatcher, bool dete
                 Simulation.Awakener.DisposeForCompletedAwakenings(ref setsToAwaken);
             }
             setsToAwaken.Dispose(Pool);
-
         }
     }
 }
diff --git a/BepuPhysics/CollisionDetection/NonconvexReduction.cs b/BepuPhysics/CollisionDetection/NonconvexReduction.cs
index 4a298e257..782f50258 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
 {
@@ -66,7 +62,7 @@ unsafe static void UseContact(ref QuickList<RemainingCandidate> 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 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
@@ -324,7 +320,7 @@ public unsafe void Flush<TCallbacks>(int pairId, ref CollisionBatcher<TCallbacks
         }
 
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public unsafe void OnChildCompleted<TCallbacks>(ref PairContinuation report, ref ConvexContactManifold manifold, ref CollisionBatcher<TCallbacks> batcher)
+        public void OnChildCompleted<TCallbacks>(ref PairContinuation report, ref ConvexContactManifold manifold, ref CollisionBatcher<TCallbacks> batcher)
             where TCallbacks : struct, ICollisionCallbacks
         {
             Children[report.ChildIndex].Manifold = manifold;
@@ -332,7 +328,7 @@ public unsafe void OnChildCompleted<TCallbacks>(ref PairContinuation report, ref
         }
 
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public void OnChildCompletedEmpty<TCallbacks>(ref PairContinuation report, ref CollisionBatcher<TCallbacks> batcher) where TCallbacks : struct, ICollisionCallbacks
+        public void OnUntestedChildCompleted<TCallbacks>(ref PairContinuation report, ref CollisionBatcher<TCallbacks> batcher) where TCallbacks : struct, ICollisionCallbacks
         {
             Children[report.ChildIndex].Manifold.Count = 0;
             ++CompletedChildCount;
diff --git a/BepuPhysics/CollisionDetection/PairCache.cs b/BepuPhysics/CollisionDetection/PairCache.cs
index 340ebbc01..32c2abbd0 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<CollidablePair, CollidablePairPointers, CollidablePairComparer>;
+    using OverlapMapping = QuickDictionary<CollidablePair, ConstraintCache, CollidablePairComparer>;
 
     [StructLayout(LayoutKind.Explicit, Size = 8)]
     public struct CollidablePair
@@ -56,38 +51,52 @@ public int Hash(ref CollidablePair item)
         }
     }
 
-    public struct CollidablePairPointers
+    /// <summary>
+    /// Refers to a change in a <see cref="PairCache"/>.
+    /// </summary>
+    public struct PairCacheChangeIndex
     {
         /// <summary>
-        /// 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 <see cref="WorkerPendingPairChanges"/> storing the pending change, if any. If -1, then this pair cache change refers to a change directly to the mapping.
         /// </summary>
-        public PairCacheIndex ConstraintCache;
+        public int WorkerIndex;
         /// <summary>
-        /// 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.
         /// </summary>
-        public PairCacheIndex CollisionDetectionCache;
+        public int Index;
+
+        /// <summary>
+        /// Gets whether this change is in the <see cref="PairCache"/>
+        /// </summary>
+        public bool IsPending => WorkerIndex >= 0;
     }
 
-    internal struct ArrayList<T>
+    /// <summary>
+    /// Stores information about a contact constraint from the previous timestep.
+    /// </summary>
+    [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++];
-        }
+        /// <summary>
+        /// Handle of the contact constraint associated with this cache.
+        /// </summary>
+        public ConstraintHandle ConstraintHandle;
+        /// <summary>
+        /// Feature id of the first contact in the constraint associated with this cache.
+        /// </summary>
+        public int FeatureId0;
+        /// <summary>
+        /// Feature id of the second contact in the constraint associated with this cache.
+        /// </summary>
+        public int FeatureId1;
+        /// <summary>
+        /// Feature id of the third contact in the constraint associated with this cache.
+        /// </summary>
+        public int FeatureId2;
+        /// <summary>
+        /// Feature id of the fourth contact in the constraint associated with this cache.
+        /// </summary>
+        public int FeatureId3;
     }
 
     public partial class PairCache
@@ -102,25 +111,20 @@ 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.
         /// </remarks>
-        internal RawBuffer PairFreshness;
-        BufferPool pool;
+        internal Buffer<byte> PairFreshness;
+        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<WorkerPairCache> workerCaches;
-        internal ArrayList<WorkerPairCache> NextWorkerCaches;
+        //While the current worker caches are read from, changes to the cache are accumulated.
+        internal Buffer<WorkerPendingPairChanges> WorkerPendingChanges;
+        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);
@@ -128,56 +132,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<PreallocationSizes>(maximumConstraintTypeCount, pool);
-            var minimumSizesPerCollisionType = new QuickList<PreallocationSizes>(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.WorkerPools[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 +183,15 @@ internal void ResizeConstraintToPairMappingCapacity(Solver solver, int targetCap
         /// </summary>
         public void PrepareFlushJobs(ref QuickList<NarrowPhaseFlushJob> 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)
@@ -238,26 +202,26 @@ public void PrepareFlushJobs(ref QuickList<NarrowPhaseFlushJob> 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.
             //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)
                 {
-                    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(pending.Pair, pending.Cache);
                     Debug.Assert(added);
                 }
             }
@@ -267,37 +231,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.WorkerPools[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 +271,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 +296,42 @@ public void Dispose()
 
 
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public int IndexOf(ref CollidablePair pair)
+        public int IndexOf(CollidablePair pair)
         {
-            return Mapping.IndexOfRef(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<TConstraintCache, TCollisionCache>(int workerIndex, ref CollidablePair pair,
-            ref TCollisionCache collisionCache, ref TConstraintCache constraintCache)
-            where TConstraintCache : IPairCacheEntry
-            where TCollisionCache : IPairCacheEntry
+        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 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.WorkerPools[workerIndex], pair, constraintCache) };
         }
 
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        internal unsafe void Update<TConstraintCache, TCollisionCache>(int workerIndex, int pairIndex, ref CollidablePairPointers pointers,
-            ref TCollisionCache collisionCache, ref TConstraintCache constraintCache)
-            where TConstraintCache : IPairCacheEntry
-            where TCollisionCache : IPairCacheEntry
+        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;
-            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)
+        internal ConstraintHandle GetOldConstraintHandle(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;
         }
 
         /// <summary>
@@ -406,17 +341,22 @@ internal unsafe ConstraintHandle GetOldConstraintHandle(int pairIndex)
         /// <param name="narrowPhase">Narrow phase that triggered the constraint add.</param>
         /// <param name="solver">Solver containing the constraint to set the impulses of.</param>
         /// <param name="impulses">Warm starting impulses to apply to the contact constraint.</param>
-        /// <param name="constraintCacheIndex">Index of the constraint cache to update.</param>
+        /// <param name="pairCacheChangeIndex">Index of the change associated with this constraint in the <see cref="PairCache"/>.</param>
         /// <param name="constraintHandle">Constraint handle associated with the constraint cache being updated.</param>
         /// <param name="pair">Collidable pair associated with the new constraint.</param>
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        internal unsafe void CompleteConstraintAdd<TContactImpulses>(NarrowPhase narrowPhase, Solver solver, ref TContactImpulses impulses, PairCacheIndex constraintCacheIndex,
+        internal void CompleteConstraintAdd<TContactImpulses>(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;
-            solver.GetConstraintReference(constraintHandle, out var reference);
+            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);
             //This mapping entry had to be deferred until now because no constraint handle was known until now. Now that we have it,
@@ -424,18 +364,5 @@ internal unsafe void CompleteConstraintAdd<TContactImpulses>(NarrowPhase narrowP
             ConstraintHandleToPair[constraintHandle.Value].Pair = pair;
         }
 
-        [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public unsafe ref TConstraintCache GetConstraintCache<TConstraintCache>(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<TConstraintCache>(workerCaches[constraintCacheIndex.Cache].GetConstraintCachePointer(constraintCacheIndex));
-        }
-
-        [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public unsafe ref TCollisionData GetCollisionData<TCollisionData>(PairCacheIndex index) where TCollisionData : struct, IPairCacheEntry
-        {
-            return ref Unsafe.AsRef<TCollisionData>(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
-{
-    /// <summary>
-    /// Packed indirection to data associated with a pair cache entry.
-    /// </summary>
-    public struct PairCacheIndex
-    {
-        internal ulong packed;
-
-        /// <summary>
-        /// Gets whether this index actually refers to anything. The Type and Index should only be used if this is true.
-        /// </summary>
-        public bool Exists
-        {
-            [MethodImpl(MethodImplOptions.AggressiveInlining)]
-            get { return (packed & (1UL << 63)) > 0; }
-        }
-
-        /// <summary>
-        /// Gets whether this index refers to an active cache entry. If false, the entry exists in an inactive set.
-        /// </summary>
-        public bool Active
-        {
-            [MethodImpl(MethodImplOptions.AggressiveInlining)]
-            get { return (packed & (1UL << 62)) > 0; }
-        }
-
-
-        /// <summary>
-        /// Gets the index of the cache that owns the entry.
-        /// </summary>
-        public int Cache
-        {
-            [MethodImpl(MethodImplOptions.AggressiveInlining)]
-            get { return (int)(packed >> 38) & 0x3FFFFF; } //24 bits
-        }
-
-        /// <summary>
-        /// Gets the type index of the object.
-        /// </summary>
-        public int Type
-        {
-            [MethodImpl(MethodImplOptions.AggressiveInlining)]
-            get { return (int)(packed >> 30) & 0xFF; } //8 bits
-        }
-
-        /// <summary>
-        /// Gets the index of the object within the type specific list.
-        /// </summary>
-        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 6fa8aea11..8943f2252 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
 {
@@ -40,43 +36,15 @@ 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<WorkerPairCache> 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)
+        internal void ValidateHandleCountInMapping(ConstraintHandle constraintHandle, int expectedCount)
         {
             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.");
@@ -85,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)
@@ -102,11 +70,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 +83,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<WorkerPairCache>(1);
-            var constraints = new QuickList<WorkerPairCache.PreallocationSizes>(1, pool);
-            var collisions = new QuickList<WorkerPairCache.PreallocationSizes>(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<SleepingCache> inactiveCaches, ref Buffer<UntypedList> 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)
+        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.
             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.AddUnsafelyRef(ref pair.Pair, pointers);
+                    Mapping.AddUnsafely(pair.Pair, pair.Cache);
                 }
             }
         }
@@ -183,7 +101,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/CollisionDetection/RayBatchers.cs b/BepuPhysics/CollisionDetection/RayBatchers.cs
index 85d66216b..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);
     }
 
     /// <summary>
@@ -31,15 +31,15 @@ struct LeafTester : IBatchedRayLeafTester
             public Buffer<CollidableReference> 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);
             }
         }
 
@@ -50,13 +50,14 @@ public unsafe void TestLeaf(int leafIndex, RayData* rayData, float* maximumT)
         /// Constructs a ray batcher for the broad phase and initializes its backing resources.
         /// </summary>
         /// <param name="pool">Pool to pull resources from.</param>
+        /// <param name="broadPhase">Broad phase to be tested.</param>
         /// <param name="rayTester">Ray tester used to test leaves found by the broad phase tree traversals.</param>
         /// <param name="batcherRayCapacity">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.</param>
         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))));
@@ -129,7 +130,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 void OnRayHit(in RayData ray, ref float maximumT, float t, Vector3 normal, int childIndex)
                 {
                     HitHandler.OnRayHit(ray, ref maximumT, t, normal, Reference, childIndex);
                 }
@@ -137,24 +138,24 @@ public unsafe void OnRayHit(in RayData ray, ref float maximumT, float t, in Vect
 
 
             [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/CollisionDetection/SweepTaskRegistry.cs b/BepuPhysics/CollisionDetection/SweepTaskRegistry.cs
index 43dba7e22..57b972980 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.
         /// </summary>
         /// <param name="childA">Index of the child belonging to collidable A.</param>
-        /// <param name="childB">Index of the child belonging to collidable B.
+        /// <param name="childB">Index of the child belonging to collidable B.</param>
         /// <returns>True if testing should proceed, false otherwise.</returns>
         bool AllowTest(int childA, int childB);
     }
@@ -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<TSweepFilter>(
-            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<TSweepFilter>(
-            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
@@ -182,7 +182,7 @@ public SweepTask GetTask<TShapeA, TShapeB>()
             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/CapsuleBoxDistanceTester.cs b/BepuPhysics/CollisionDetection/SweepTasks/CapsuleBoxDistanceTester.cs
index 85eaf85bd..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;
 
@@ -187,7 +185,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/CollisionDetection/SweepTasks/CompoundHomogeneousCompoundSweepTask.cs b/BepuPhysics/CollisionDetection/SweepTasks/CompoundHomogeneousCompoundSweepTask.cs
index d9cfbdc81..4a7993478 100644
--- a/BepuPhysics/CollisionDetection/SweepTasks/CompoundHomogeneousCompoundSweepTask.cs
+++ b/BepuPhysics/CollisionDetection/SweepTasks/CompoundHomogeneousCompoundSweepTask.cs
@@ -15,24 +15,23 @@ public class CompoundHomogeneousCompoundSweepTask<TCompoundA, TCompoundB, TChild
     {
         public CompoundHomogeneousCompoundSweepTask()
         {
-            ShapeTypeIndexA = default(TCompoundA).TypeId;
-            ShapeTypeIndexB = default(TCompoundB).TypeId;
+            ShapeTypeIndexA = TCompoundA.TypeId;
+            ShapeTypeIndexB = TCompoundB.TypeId;
         }
 
         protected unsafe override bool PreorderedTypeSweep<TSweepFilter>(
-            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)
         {
             ref var compoundB = ref Unsafe.AsRef<TCompoundB>(shapeDataB);
-            TOverlapFinder overlapFinder = default;
             t0 = float.MaxValue;
             t1 = float.MaxValue;
             hitLocation = new Vector3();
             hitNormal = new Vector3();
             ref var compoundA = ref Unsafe.AsRef<TCompoundA>(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);
@@ -44,11 +43,11 @@ protected unsafe override bool PreorderedTypeSweep<TSweepFilter>(
                         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.LocalPose, orientationA, velocityA,
-                            Unsafe.AsPointer(ref childB), Triangle.Id, childPoseB, offsetB, orientationB, velocityB,
+                            compoundChildShapeData, compoundChildType, compoundChild.AsPose(), orientationA, velocityA,
+                            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))
                         {
@@ -69,7 +68,7 @@ protected unsafe override bool PreorderedTypeSweep<TSweepFilter>(
             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 dba5614d1..699073b74 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<TCompoundA, TCompoundB> 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,
+        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,9 +18,9 @@ public struct CompoundPairSweepOverlapFinder<TCompoundA, TCompoundB> : ICompound
         where TCompoundA : struct, ICompoundShape
         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,
+        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)
         {
             overlaps = new CompoundPairSweepOverlaps(pool, compoundA.ChildCount);
@@ -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, 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 c4967890e..549b54e4c 100644
--- a/BepuPhysics/CollisionDetection/SweepTasks/CompoundPairSweepTask.cs
+++ b/BepuPhysics/CollisionDetection/SweepTasks/CompoundPairSweepTask.cs
@@ -13,13 +13,13 @@ public class CompoundPairSweepTask<TCompoundA, TCompoundB, TOverlapFinder> : Swe
     {
         public CompoundPairSweepTask()
         {
-            ShapeTypeIndexA = default(TCompoundA).TypeId;
-            ShapeTypeIndexB = default(TCompoundB).TypeId;
+            ShapeTypeIndexA = TCompoundA.TypeId;
+            ShapeTypeIndexB = TCompoundB.TypeId;
         }
 
         protected override unsafe bool PreorderedTypeSweep<TSweepFilter>(
-            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)
         {
@@ -29,7 +29,7 @@ protected override unsafe bool PreorderedTypeSweep<TSweepFilter>(
             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)
@@ -50,8 +50,8 @@ protected override unsafe bool PreorderedTypeSweep<TSweepFilter>(
                     {
                         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, 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))
                         {
@@ -72,7 +72,7 @@ protected override unsafe bool PreorderedTypeSweep<TSweepFilter>(
             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..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;
 
@@ -12,16 +9,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<TShapeA, TCompoundB> 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,
+        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);
     }
 
     public struct ConvexCompoundSweepOverlapFinder<TShapeA, TCompoundB> : IConvexCompoundSweepOverlapFinder<TShapeA, TCompoundB>
         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 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)
         {
             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 656a2b92d..a577e8a69 100644
--- a/BepuPhysics/CollisionDetection/SweepTasks/ConvexCompoundSweepTask.cs
+++ b/BepuPhysics/CollisionDetection/SweepTasks/ConvexCompoundSweepTask.cs
@@ -14,13 +14,13 @@ public class ConvexCompoundSweepTask<TShapeA, TShapeWideA, TCompound, TOverlapFi
     {
         public ConvexCompoundSweepTask()
         {
-            ShapeTypeIndexA = default(TShapeA).TypeId;
-            ShapeTypeIndexB = default(TCompound).TypeId;
+            ShapeTypeIndexA = TShapeA.TypeId;
+            ShapeTypeIndexB = TCompound.TypeId;
         }
 
         protected override unsafe bool PreorderedTypeSweep<TSweepFilter>(
-            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)
         {
@@ -30,7 +30,7 @@ protected override unsafe bool PreorderedTypeSweep<TSweepFilter>(
             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,10 +39,10 @@ protected override unsafe bool PreorderedTypeSweep<TSweepFilter>(
                     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,
-                        childShapeData, childType, child.LocalPose, offsetB, orientationB, velocityB,
+                        shapeDataA, TShapeA.TypeId, new RigidPose() { Orientation = Quaternion.Identity }, orientationA, velocityA,
+                        childShapeData, childType, child.AsPose(), offsetB, orientationB, velocityB,
                         maximumT, minimumProgression, convergenceThreshold, maximumIterationCount,
                         out var t0Candidate, out var t1Candidate, out var hitLocationCandidate, out var hitNormalCandidate))
                     {
@@ -62,7 +62,7 @@ protected override unsafe bool PreorderedTypeSweep<TSweepFilter>(
             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 9b7198ee0..87cdc2f97 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
@@ -18,14 +17,14 @@ public class ConvexHomogeneousCompoundSweepTask<TConvex, TConvexWide, TCompound,
     {
         public ConvexHomogeneousCompoundSweepTask()
         {
-            ShapeTypeIndexA = default(TConvex).TypeId;
-            ShapeTypeIndexB = default(TCompound).TypeId;
+            ShapeTypeIndexA = TConvex.TypeId;
+            ShapeTypeIndexB = TCompound.TypeId;
         }
 
 
         protected override unsafe bool PreorderedTypeSweep<TSweepFilter>(
-            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)
         {
@@ -34,10 +33,10 @@ protected override unsafe bool PreorderedTypeSweep<TSweepFilter>(
             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<TConvex>(shapeDataA), orientationA, velocityA, ref compound, offsetB, orientationB, velocityB, maximumT, shapes, pool, out var overlaps);
+                TOverlapFinder.FindOverlaps(ref Unsafe.AsRef<TConvex>(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];
@@ -45,8 +44,8 @@ protected override unsafe bool PreorderedTypeSweep<TSweepFilter>(
                     {
                         compound.GetPosedLocalChild(childIndex, out var childShape, out var childPose);
                         if (task.Sweep(
-                            shapeDataA, ShapeTypeIndexA, new RigidPose(Vector3.Zero, Quaternion.Identity), orientationA, velocityA,
-                            Unsafe.AsPointer(ref childShape), Triangle.Id, childPose, offsetB, orientationB, velocityB,
+                            shapeDataA, ShapeTypeIndexA, RigidPose.Identity, orientationA, velocityA,
+                            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))
                         {
@@ -67,7 +66,7 @@ protected override unsafe bool PreorderedTypeSweep<TSweepFilter>(
             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 f1cfc95f3..d14f1d209 100644
--- a/BepuPhysics/CollisionDetection/SweepTasks/ConvexSweepTaskCommon.cs
+++ b/BepuPhysics/CollisionDetection/SweepTasks/ConvexSweepTaskCommon.cs
@@ -23,12 +23,12 @@ public class ConvexPairSweepTask<TShapeA, TShapeWideA, TShapeB, TShapeWideB, TPa
     {
         public ConvexPairSweepTask()
         {
-            ShapeTypeIndexA = default(TShapeA).TypeId;
-            ShapeTypeIndexB = default(TShapeB).TypeId;
+            ShapeTypeIndexA = TShapeA.TypeId;
+            ShapeTypeIndexB = 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<TSweepFilter>(
-            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<TSweepFilter>(
                 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<float> 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<float> samples, ref Vector3Wide sampleOffsetB, ref QuaternionWide sampleOrientationA, ref QuaternionWide sampleOrientationB);
             void GetNonlinearVelocityContribution(ref Vector3Wide normal,
                 out Vector<float> velocityContributionA, out Vector<float> maximumDisplacementA,
                 out Vector<float> velocityContributionB, out Vector<float> 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<TShapeA, TShapeWideA, TShapeB, TShapeWideB, TPairDistanceTester>.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);
@@ -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);
 
@@ -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<TSweepModifier>(
-            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)
@@ -285,12 +285,12 @@ static unsafe bool Sweep<TSweepModifier>(
             if (wideA.InternalAllocationSize > 0)
             {
                 var memory = stackalloc byte[wideA.InternalAllocationSize];
-                wideA.Initialize(new RawBuffer(memory, wideA.InternalAllocationSize));
+                wideA.Initialize(new Buffer<byte>(memory, wideA.InternalAllocationSize));
             }
             if (wideB.InternalAllocationSize > 0)
             {
                 var memory = stackalloc byte[wideB.InternalAllocationSize];
-                wideB.Initialize(new RawBuffer(memory, wideB.InternalAllocationSize));
+                wideB.Initialize(new Buffer<byte>(memory, wideB.InternalAllocationSize));
             }
             wideA.Broadcast(shapeA);
             wideB.Broadcast(shapeB);
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/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<int> intersected, out Vector<float> 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.
diff --git a/BepuPhysics/CollisionDetection/UntypedList.cs b/BepuPhysics/CollisionDetection/UntypedList.cs
index e3dec4878..fb3e90c13 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<byte> Buffer;
         public int Count;
         public int ByteCount;
         public int ElementSizeInBytes;
@@ -69,7 +69,7 @@ public unsafe ref T Get<T>(int index)
         }
 
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public unsafe ref T AllocateUnsafely<T>()
+        public ref T AllocateUnsafely<T>()
         {
             Validate();
             Debug.Assert(Unsafe.SizeOf<T>() == 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<byte>(newSize, out var newBuffer);
                     Unsafe.CopyBlockUnaligned(newBuffer.Memory, Buffer.Memory, (uint)Buffer.Length);
                     pool.ReturnUnsafely(Buffer.Id);
                     Buffer = newBuffer;
@@ -123,14 +123,14 @@ public unsafe int Allocate(int elementSizeInBytes, int minimumElementCount, Buff
         }
 
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public unsafe int Allocate<T>(int minimumElementCount, BufferPool pool)
+        public int Allocate<T>(int minimumElementCount, BufferPool pool)
         {
             var elementSizeInBytes = Unsafe.SizeOf<T>();
             return Allocate(elementSizeInBytes, minimumElementCount, pool);
         }
 
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public unsafe int Add<T>(ref T data, int minimumCount, BufferPool pool)
+        public int Add<T>(ref T data, int minimumCount, BufferPool pool)
         {
             var byteIndex = Allocate<T>(minimumCount, pool);
             GetFromBytes<T>(byteIndex) = data;
diff --git a/BepuPhysics/CollisionDetection/WideRayTester.cs b/BepuPhysics/CollisionDetection/WideRayTester.cs
index 7d5718994..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;
 
@@ -24,16 +23,16 @@ public unsafe static void Test<TRaySource, TShape, TShapeWide, TRayHitHandler>(r
             if (wide.InternalAllocationSize > 0)
             {
                 var memory = stackalloc byte[wide.InternalAllocationSize];
-                wide.Initialize(new RawBuffer(memory, wide.InternalAllocationSize));
+                wide.Initialize(new Buffer<byte>(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<float>.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/CollisionDetection/WorkerPairCache.cs b/BepuPhysics/CollisionDetection/WorkerPairCache.cs
index b19afa95d..8a6b40bde 100644
--- a/BepuPhysics/CollisionDetection/WorkerPairCache.cs
+++ b/BepuPhysics/CollisionDetection/WorkerPairCache.cs
@@ -1,35 +1,18 @@
 using BepuUtilities.Collections;
 using BepuUtilities.Memory;
-using System;
-using System.Diagnostics;
 using System.Runtime.CompilerServices;
 
 namespace BepuPhysics.CollisionDetection
 {
-
-
     /// <summary>
-    /// 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.
     /// </summary>
-    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<UntypedList> constraintCaches;
-        internal Buffer<UntypedList> collisionCaches;
-
         public struct PendingAdd
         {
             public CollidablePair Pair;
-            public CollidablePairPointers Pointers;
+            public ConstraintCache Cache;
         }
 
         /// <summary>
@@ -41,162 +24,28 @@ public struct PendingAdd
         /// </summary>
         public QuickList<CollidablePair> PendingRemoves;
 
-        public WorkerPairCache(int workerIndex, BufferPool pool,
-            ref QuickList<PreallocationSizes> minimumSizesPerConstraintType,
-            ref QuickList<PreallocationSizes> 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<PendingAdd>(pendingCapacity, pool);
             PendingRemoves = new QuickList<CollidablePair>(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<PreallocationSizes> minimumSizesPerConstraintType,
-            ref QuickList<PreallocationSizes> 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<TCollision, TConstraint>(ref TCollision collisionCache, ref TConstraint constraintCache, out CollidablePairPointers pointers)
-            where TCollision : IPairCacheEntry where TConstraint : IPairCacheEntry
+        public 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<TCollision, TConstraint>(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<TCollision, TConstraint>(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/ConstraintBatch.cs b/BepuPhysics/ConstraintBatch.cs
index 033a193f5..551ce4358 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<BodyHandle> 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<int>
         {
             public Bodies Bodies;
@@ -182,9 +156,12 @@ public ActiveBodyHandleRemover(Bodies bodies, IndexSet* handles)
             }
 
             [MethodImpl(MethodImplOptions.AggressiveInlining)]
-            public void LoopBody(int bodyIndex)
+            public void LoopBody(int encodedBodyIndex)
             {
-                Handles->Remove(Bodies.ActiveSet.IndexToHandle[bodyIndex].Value);
+                if (Bodies.IsEncodedDynamicReference(encodedBodyIndex))
+                {
+                    Handles->Remove(Bodies.ActiveSet.IndexToHandle[encodedBodyIndex & Bodies.BodyReferenceMask].Value);
+                }
             }
         }
 
@@ -206,33 +183,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.EnumerateConnectedRawBodyReferences(ref TypeBatches[typeBatchIndex], indexInTypeBatch, ref handleRemover);
         }
 
-        public unsafe void Remove(int constraintTypeId, int indexInTypeBatch, Solver solver)
+        public void Remove(int constraintTypeId, int indexInTypeBatch, bool isFallback, 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<ConstraintLocation> 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, isFallback);
+            RemoveTypeBatchIfEmpty(ref typeBatch, typeBatchIndex, solver.pool);
         }
 
         public void Clear(BufferPool pool)
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
-{
-    /// <summary>
-    /// Enumerates the bodies attached to an active constraint and removes the constraint's handle from all of the connected body constraint reference lists.
-    /// </summary>
-    struct ConstraintGraphRemovalEnumerator : IForEach<int>
-    {
-        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/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
-        {
-            /// <summary>
-            /// Index of the target constraint bundle to optimize.
-            /// </summary>
-            public int BundleIndex;
-            /// <summary>
-            /// Index of the last optimized type batch.
-            /// </summary>
-            public int TypeBatchIndex;
-            /// <summary>
-            /// Index of the last optimized batch.
-            /// </summary>
-            public int BatchIndex;
-
-        }
-
-        Optimization nextTarget;
-
-        /// <summary>
-        /// 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.
-        /// </summary>
-        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<int> generateSortKeysDelegate;
-        Action<int> regatherDelegate;
-        Action<int> 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<int>.Count;
-
-            var typeBatch = activeSet.Batches[target.BatchIndex].TypeBatches.GetPointer(target.TypeBatchIndex);
-            SortByBodyLocation(typeBatch, target.BundleIndex, Math.Min(typeBatch->ConstraintCount - target.BundleIndex * Vector<int>.Count, maximumRegionSizeInConstraints),
-                solver.HandleToConstraint, bodies.ActiveSet.Count, bufferPool, threadDispatcher);
-
-        }
-
-        unsafe TypeBatch* typeBatchPointer;
-        IThreadDispatcher threadDispatcher;
-        Buffer<ConstraintLocation> handlesToConstraints;
-        struct MultithreadingContext
-        {
-            public Buffer<int> SortKeys;
-            public Buffer<int> SourceIndices;
-            public RawBuffer BodyReferencesCache;
-            public int SourceStartBundleIndex;
-            public int BundlesPerWorker;
-            public int BundlesPerWorkerRemainder;
-            public int TypeBatchConstraintCount;
-
-            public Buffer<int> SortedKeys; //This is only really stored for debug use.
-            public Buffer<int> SortedSourceIndices;
-            public Buffer<int> ScratchKeys;
-            public Buffer<int> ScratchValues;
-            public Buffer<ConstraintHandle> 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<ConstraintLocation> 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<ConstraintLocation>();
-
-            //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/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
+{
+    /// <summary>
+    /// Location in memory where a constraint is stored.
+    /// </summary>
+    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.)
+        /// <summary>
+        /// Index of the constraint set that owns the constraint. If zero, the constraint is attached to bodies that are awake.
+        /// </summary>
+        public int SetIndex;
+        /// <summary>
+        /// Index of the constraint batch the constraint belongs to.
+        /// </summary>
+        public int BatchIndex;
+        /// <summary>
+        /// 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.
+        /// </summary>
+        public int TypeId;
+        /// <summary>
+        /// Index of the constraint in a type batch.
+        /// </summary>
+        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
+{
+    /// <summary>
+    /// Reference to a constraint's memory location in the solver.
+    /// </summary>
+    public unsafe struct ConstraintReference
+    {
+        internal TypeBatch* typeBatchPointer;
+        /// <summary>
+        /// Gets a reference to the type batch holding the constraint.
+        /// </summary>
+        public ref TypeBatch TypeBatch
+        {
+            [MethodImpl(MethodImplOptions.AggressiveInlining)]
+            get
+            {
+                return ref *typeBatchPointer;
+            }
+        }
+        /// <summary>
+        /// Index in the type batch where the constraint is allocated.
+        /// </summary>
+        public readonly int IndexInTypeBatch;
+
+        /// <summary>
+        /// Creates a new constraint reference from a constraint memory location.
+        /// </summary>
+        /// <param name="typeBatchPointer">Pointer to the type batch where the constraint lives.</param>
+        /// <param name="indexInTypeBatch">Index in the type batch where the constraint is allocated.</param>
+        [MethodImpl(MethodImplOptions.AggressiveInlining)]
+        public ConstraintReference(TypeBatch* typeBatchPointer, int indexInTypeBatch)
+        {
+            this.typeBatchPointer = typeBatchPointer;
+            IndexInTypeBatch = indexInTypeBatch;
+        }
+    }
+}
diff --git a/BepuPhysics/ConstraintSet.cs b/BepuPhysics/ConstraintSet.cs
index f7cc631f6..8aa1a6dae 100644
--- a/BepuPhysics/ConstraintSet.cs
+++ b/BepuPhysics/ConstraintSet.cs
@@ -7,12 +7,12 @@ namespace BepuPhysics
     public struct ConstraintSet
     {
         public QuickList<ConstraintBatch> Batches;
-        public FallbackBatch Fallback;
+        public SequentialFallbackBatch SequentialFallback;
 
         public ConstraintSet(BufferPool pool, int initialBatchCapacity)
         {
             Batches = new QuickList<ConstraintBatch>(initialBatchCapacity, pool);
-            Fallback = default;
+            SequentialFallback = default;
         }
 
         /// <summary>
@@ -70,7 +70,7 @@ public void Clear(BufferPool pool)
             {
                 Batches[i].Dispose(pool);
             }
-            Fallback.Dispose(pool);
+            SequentialFallback.Dispose(pool);
             Batches.Count = 0;
         }
 
@@ -80,7 +80,7 @@ public void Dispose(BufferPool pool)
             {
                 Batches[i].Dispose(pool);
             }
-            Fallback.Dispose(pool);
+            SequentialFallback.Dispose(pool);
             Batches.Dispose(pool);
             this = new ConstraintSet();
         }
diff --git a/BepuPhysics/Constraints/AngularAxisGearMotor.cs b/BepuPhysics/Constraints/AngularAxisGearMotor.cs
index 5584e3d17..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;
@@ -28,7 +27,7 @@ public struct AngularAxisGearMotor : ITwoBodyConstraintDescription<AngularAxisGe
         /// </summary>
         public MotorSettings Settings;
 
-        public readonly int ConstraintTypeId
+        public static int ConstraintTypeId
         {
             [MethodImpl(MethodImplOptions.AggressiveInlining)]
             get
@@ -37,7 +36,8 @@ public readonly int ConstraintTypeId
             }
         }
 
-        public readonly Type TypeProcessorType => typeof(AngularAxisGearMotorTypeProcessor);
+        public static Type TypeProcessorType => typeof(AngularAxisGearMotorTypeProcessor);
+        public static TypeProcessor CreateTypeProcessor() => new AngularAxisGearMotorTypeProcessor();
 
         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 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<AngularAxisGearMotorPrestepData>.Get(ref batch.PrestepData, bundleIndex), innerIndex);
@@ -67,72 +67,53 @@ public struct AngularAxisGearMotorPrestepData
         public MotorSettingsWide Settings;
     }
 
-    public struct AngularAxisGearMotorProjection
-    {
-        public Vector3Wide NegatedVelocityToImpulseB;
-        public Vector<float> VelocityScale;
-        public Vector<float> SoftnessImpulseScale;
-        public Vector<float> MaximumImpulse;
-        public Vector3Wide ImpulseToVelocityA;
-        public Vector3Wide NegatedImpulseToVelocityB;
-    }
-
-
-    public struct AngularAxisGearMotorFunctions : IConstraintFunctions<AngularAxisGearMotorPrestepData, AngularAxisGearMotorProjection, Vector<float>>
+    public struct AngularAxisGearMotorFunctions : ITwoBodyConstraintFunctions<AngularAxisGearMotorPrestepData, Vector<float>>
     {
         [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 static void ApplyImpulse(in Vector3Wide impulseToVelocityA, in Vector3Wide negatedImpulseToVelocityB, in Vector<float> csi, ref Vector3Wide angularVelocityA, ref Vector3Wide angularVelocityB)
         {
-            //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);
-            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;
-
+            angularVelocityA += impulseToVelocityA * csi;
+            angularVelocityB -= negatedImpulseToVelocityB * csi;
         }
 
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public static void ApplyImpulse(ref Vector3Wide angularVelocityA, ref Vector3Wide angularVelocityB, in AngularAxisGearMotorProjection projection, in Vector<float> csi)
+        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<float> accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB)
         {
-            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);
+            QuaternionWide.TransformWithoutOverlap(prestep.LocalAxisA, orientationA, out var axis);
+            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 WarmStart(ref BodyVelocities velocityA, ref BodyVelocities velocityB, ref AngularAxisGearMotorProjection projection, ref Vector<float> accumulatedImpulse)
+        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<float> accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB)
         {
-            ApplyImpulse(ref velocityA.Angular, ref velocityB.Angular, projection, accumulatedImpulse);
-        }
-
+            //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 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);
 
-        [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public void Solve(ref BodyVelocities velocityA, ref BodyVelocities velocityB, ref AngularAxisGearMotorProjection projection, ref Vector<float> 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);
-
+            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(impulseToVelocityA, negatedImpulseToVelocityB, accumulatedImpulses, ref wsvA.Angular, ref wsvB.Angular);
         }
 
+        public static bool RequiresIncrementalSubstepUpdates => false;
+        [MethodImpl(MethodImplOptions.AggressiveInlining)]
+        public static void IncrementallyUpdateForSubstep(in Vector<float> dt, in BodyVelocityWide wsvA, in BodyVelocityWide wsvB, ref AngularAxisGearMotorPrestepData prestepData) { }
     }
 
-    public class AngularAxisGearMotorTypeProcessor : TwoBodyTypeProcessor<AngularAxisGearMotorPrestepData, AngularAxisGearMotorProjection, Vector<float>, AngularAxisGearMotorFunctions>
+    public class AngularAxisGearMotorTypeProcessor : TwoBodyTypeProcessor<AngularAxisGearMotorPrestepData, Vector<float>, AngularAxisGearMotorFunctions, AccessOnlyAngular, AccessOnlyAngularWithoutPose, AccessOnlyAngular, AccessOnlyAngularWithoutPose>
     {
         public const int BatchTypeId = 54;
     }
diff --git a/BepuPhysics/Constraints/AngularAxisMotor.cs b/BepuPhysics/Constraints/AngularAxisMotor.cs
index 633dae752..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;
@@ -27,7 +26,7 @@ public struct AngularAxisMotor : ITwoBodyConstraintDescription<AngularAxisMotor>
         /// </summary>
         public MotorSettings Settings;
 
-        public readonly int ConstraintTypeId
+        public static int ConstraintTypeId
         {
             [MethodImpl(MethodImplOptions.AggressiveInlining)]
             get
@@ -36,8 +35,9 @@ public readonly int ConstraintTypeId
             }
         }
 
-        public readonly Type TypeProcessorType => typeof(AngularAxisMotorTypeProcessor);
-        
+        public static Type TypeProcessorType => typeof(AngularAxisMotorTypeProcessor);
+        public static TypeProcessor CreateTypeProcessor() => new AngularAxisMotorTypeProcessor();
+
         public readonly void ApplyDescription(ref TypeBatch batch, int bundleIndex, int innerIndex)
         {
             ConstraintChecker.AssertUnitLength(LocalAxisA, nameof(AngularAxisMotor), nameof(LocalAxisA));
@@ -49,7 +49,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<AngularAxisMotorPrestepData>.Get(ref batch.PrestepData, bundleIndex), innerIndex);
@@ -66,69 +66,46 @@ public struct AngularAxisMotorPrestepData
         public MotorSettingsWide Settings;
     }
 
-    public struct AngularAxisMotorProjection
-    {
-        public Vector3Wide VelocityToImpulseA;
-        public Vector<float> BiasImpulse;
-        public Vector<float> SoftnessImpulseScale;
-        public Vector<float> MaximumImpulse;
-        public Vector3Wide ImpulseToVelocityA;
-        public Vector3Wide NegatedImpulseToVelocityB;
-    }
-
-
-    public struct AngularAxisMotorFunctions : IConstraintFunctions<AngularAxisMotorPrestepData, AngularAxisMotorProjection, Vector<float>>
+    public struct AngularAxisMotorFunctions : ITwoBodyConstraintFunctions<AngularAxisMotorPrestepData, Vector<float>>
     {
         [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 static void ApplyImpulse(in Vector3Wide impulseToVelocityA, in Vector3Wide negatedImpulseToVelocityB, in Vector<float> csi, ref Vector3Wide angularVelocityA, ref Vector3Wide angularVelocityB)
         {
-            //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);
-            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;
+            angularVelocityA += impulseToVelocityA * csi;
+            angularVelocityB -= negatedImpulseToVelocityB * csi;
         }
 
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public static void ApplyImpulse(ref Vector3Wide angularVelocityA, ref Vector3Wide angularVelocityB, in AngularAxisMotorProjection projection, in Vector<float> csi)
+        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<float> accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB)
         {
-            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);
+            QuaternionWide.TransformWithoutOverlap(prestep.LocalAxisA, orientationA, out var axis);
+            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)]
-        public void WarmStart(ref BodyVelocities velocityA, ref BodyVelocities velocityB, ref AngularAxisMotorProjection projection, ref Vector<float> accumulatedImpulse)
+        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<float> accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB)
         {
-            ApplyImpulse(ref velocityA.Angular, ref velocityB.Angular, projection, accumulatedImpulse);
+            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 = (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(jIA, jIB, csi, ref wsvA.Angular, ref wsvB.Angular);
         }
 
-
+        public static bool RequiresIncrementalSubstepUpdates => false;
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public void Solve(ref BodyVelocities velocityA, ref BodyVelocities velocityB, ref AngularAxisMotorProjection projection, ref Vector<float> 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);
-
-        }
-
+        public static void IncrementallyUpdateForSubstep(in Vector<float> dt, in BodyVelocityWide wsvA, in BodyVelocityWide wsvB, ref AngularAxisMotorPrestepData prestepData) { }
     }
 
-    public class AngularAxisMotorTypeProcessor : TwoBodyTypeProcessor<AngularAxisMotorPrestepData, AngularAxisMotorProjection, Vector<float>, AngularAxisMotorFunctions>
+    public class AngularAxisMotorTypeProcessor : TwoBodyTypeProcessor<AngularAxisMotorPrestepData, Vector<float>, AngularAxisMotorFunctions, AccessOnlyAngular, AccessOnlyAngularWithoutPose, AccessOnlyAngular, AccessOnlyAngular>
     {
         public const int BatchTypeId = 41;
     }
diff --git a/BepuPhysics/Constraints/AngularHinge.cs b/BepuPhysics/Constraints/AngularHinge.cs
index f73dd2ff9..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;
@@ -26,7 +25,7 @@ public struct AngularHinge : ITwoBodyConstraintDescription<AngularHinge>
         /// </summary>
         public SpringSettings SpringSettings;
 
-        public readonly int ConstraintTypeId
+        public static int ConstraintTypeId
         {
             [MethodImpl(MethodImplOptions.AggressiveInlining)]
             get
@@ -35,7 +34,8 @@ public readonly int ConstraintTypeId
             }
         }
 
-        public readonly Type TypeProcessorType => typeof(AngularHingeTypeProcessor);
+        public static Type TypeProcessorType => typeof(AngularHingeTypeProcessor);
+        public static TypeProcessor CreateTypeProcessor() => new AngularHingeTypeProcessor();
 
         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
             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<AngularHingePrestepData>.Get(ref batch.PrestepData, bundleIndex), innerIndex);
@@ -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<float> SoftnessImpulseScale;
-        public Matrix2x3Wide ImpulseToVelocityA;
-        public Matrix2x3Wide NegatedImpulseToVelocityB;
-    }
-
-
-    public struct AngularHingeFunctions : IConstraintFunctions<AngularHingePrestepData, AngularHingeProjection, Vector2Wide>
+    public struct AngularHingeFunctions : ITwoBodyConstraintFunctions<AngularHingePrestepData, Vector2Wide>
     {
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
         public static void GetErrorAngles(in Vector3Wide hingeAxisA, in Vector3Wide hingeAxisB, in Matrix2x3Wide jacobianA, out Vector2Wide errorAngles)
@@ -113,28 +102,46 @@ 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<float>.Zero), errorAngles.X, -errorAngles.X);
             errorAngles.Y = Vector.ConditionalSelect(Vector.LessThan(hbyax, Vector<float>.Zero), -errorAngles.Y, errorAngles.Y);
         }
 
-
         [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)
+        private static void ApplyImpulse(in Matrix2x3Wide impulseToVelocityA, in Matrix2x3Wide negatedImpulseToVelocityB, in Vector2Wide csi, ref Vector3Wide angularVelocityA, ref Vector3Wide angularVelocityB)
         {
-            bodies.GatherOrientation(ref bodyReferences, count, out var orientationA, out var orientationB);
+            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(prestep.LocalHingeAxisA, out var localAX, out var localAY);
+            Helpers.BuildOrthonormalBasis(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(localHingeAxisA, orientationMatrixA, out hingeAxisA);
             Matrix3x3Wide.TransformWithoutOverlap(localAX, orientationMatrixA, out jacobianA.X);
             Matrix3x3Wide.TransformWithoutOverlap(localAY, orientationMatrixA, out jacobianA.Y);
+        }
+
+        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);
+            Symmetric3x3Wide.MultiplyWithoutOverlap(jacobianA, inertiaB.InverseInertiaTensor, out var negatedImpulseToVelocityB);
+            ApplyImpulse(impulseToVelocityA, negatedImpulseToVelocityB, accumulatedImpulses, ref wsvA.Angular, ref wsvB.Angular);
+        }
+
+        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);
             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. 
@@ -180,57 +187,42 @@ public void Prestep(Bodies bodies, ref TwoBodyReferences bodyReferences, int cou
 
             //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);
+            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 projection.NegatedImpulseToVelocityB);
-            Symmetric2x2Wide.CompleteMatrixSandwich(projection.ImpulseToVelocityA, jacobianA, out var angularA);
-            Symmetric2x2Wide.CompleteMatrixSandwich(projection.NegatedImpulseToVelocityB, jacobianA, out var angularB);
+            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 projection.SoftnessImpulseScale);
-            Symmetric2x2Wide.Scale(effectiveMass, effectiveMassCFMScale, out effectiveMass);
-            Symmetric2x2Wide.MultiplyTransposed(jacobianA, effectiveMass, out projection.VelocityToImpulseA);
+            SpringSettingsWide.ComputeSpringiness(prestep.SpringSettings, dt, out var positionErrorToVelocity, out var effectiveMassCFMScale, out var softnessImpulseScale);
+            //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 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);
-        }
+            Symmetric2x2Wide.TransformWithoutOverlap(biasVelocity, effectiveMass, out var biasImpulse);
 
-        [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public void WarmStart(ref BodyVelocities velocityA, ref BodyVelocities 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)
-        {
-            //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);
+            //JB = -JA. This is (angularVelocityA * JA + angularVelocityB * JB) * effectiveMass
+            Vector3Wide.Subtract(wsvA.Angular, wsvB.Angular, out var difference);
+            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(accumulatedImpulse, projection.SoftnessImpulseScale, out var softnessContribution);
+            Vector2Wide.Scale(accumulatedImpulses, softnessImpulseScale, out var softnessContribution);
             Vector2Wide.Add(softnessContribution, csi, out csi);
-            Vector2Wide.Subtract(projection.BiasImpulse, csi, out csi);
+            Vector2Wide.Subtract(biasImpulse, csi, out csi);
 
-            Vector2Wide.Add(accumulatedImpulse, csi, out accumulatedImpulse);
+            Vector2Wide.Add(accumulatedImpulses, csi, out accumulatedImpulses);
 
-            ApplyImpulse(ref velocityA.Angular, ref velocityB.Angular, ref projection, ref csi);
+            ApplyImpulse(impulseToVelocityA, negatedImpulseToVelocityB, csi, ref wsvA.Angular, ref wsvB.Angular);
         }
 
+        public static bool RequiresIncrementalSubstepUpdates => false;
+        [MethodImpl(MethodImplOptions.AggressiveInlining)]
+        public static void IncrementallyUpdateForSubstep(in Vector<float> dt, in BodyVelocityWide wsvA, in BodyVelocityWide wsvB, ref AngularHingePrestepData prestepData) { }
     }
 
-    public class AngularHingeTypeProcessor : TwoBodyTypeProcessor<AngularHingePrestepData, AngularHingeProjection, Vector2Wide, AngularHingeFunctions>
+    public class AngularHingeTypeProcessor : TwoBodyTypeProcessor<AngularHingePrestepData, Vector2Wide, AngularHingeFunctions, AccessOnlyAngular, AccessOnlyAngularWithoutPose, AccessOnlyAngular, AccessOnlyAngular>
     {
         public const int BatchTypeId = 23;
     }
diff --git a/BepuPhysics/Constraints/AngularMotor.cs b/BepuPhysics/Constraints/AngularMotor.cs
index f665a2702..ca8ddd832 100644
--- a/BepuPhysics/Constraints/AngularMotor.cs
+++ b/BepuPhysics/Constraints/AngularMotor.cs
@@ -22,7 +22,7 @@ public struct AngularMotor : ITwoBodyConstraintDescription<AngularMotor>
         /// </summary>
         public MotorSettings Settings;
 
-        public readonly int ConstraintTypeId
+        public static int ConstraintTypeId
         {
             [MethodImpl(MethodImplOptions.AggressiveInlining)]
             get
@@ -31,7 +31,8 @@ public readonly int ConstraintTypeId
             }
         }
 
-        public readonly Type TypeProcessorType => typeof(AngularMotorTypeProcessor);
+        public static Type TypeProcessorType => typeof(AngularMotorTypeProcessor);
+        public static TypeProcessor CreateTypeProcessor() => new AngularMotorTypeProcessor();
 
         public readonly void ApplyDescription(ref TypeBatch batch, int bundleIndex, int innerIndex)
         {
@@ -42,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<AngularMotorPrestepData>.Get(ref batch.PrestepData, bundleIndex), innerIndex);
@@ -57,54 +58,42 @@ public struct AngularMotorPrestepData
         public MotorSettingsWide Settings;
     }
 
-    public struct AngularMotorProjection
+    public struct AngularMotorFunctions : ITwoBodyConstraintFunctions<AngularMotorPrestepData, Vector3Wide>
     {
-        public Symmetric3x3Wide EffectiveMass;
-        public Vector3Wide BiasImpulse;
-        public Vector<float> SoftnessImpulseScale;
-        public Vector<float> MaximumImpulse;
-        public Symmetric3x3Wide ImpulseToVelocityA;
-        public Symmetric3x3Wide NegatedImpulseToVelocityB;
-    }
-
-
-    public struct AngularMotorFunctions : IConstraintFunctions<AngularMotorPrestepData, AngularMotorProjection, 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 AngularMotorPrestepData prestep, out AngularMotorProjection projection)
+        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)
         {
-            bodies.GatherOrientation(ref bodyReferences, count, out var orientationA, out var orientationB);
-            projection.ImpulseToVelocityA = inertiaA.InverseInertiaTensor;
-            projection.NegatedImpulseToVelocityB = inertiaB.InverseInertiaTensor;
+            AngularServoFunctions.ApplyImpulse(ref wsvA.Angular, ref wsvB.Angular, inertiaA.InverseInertiaTensor, inertiaB.InverseInertiaTensor, accumulatedImpulses);
+        }
 
+        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 projection.SoftnessImpulseScale, out projection.MaximumImpulse);
+            MotorSettingsWide.ComputeSoftness(prestep.Settings, dt, out var effectiveMassCFMScale, out var softnessImpulseScale, out var maximumImpulse);
 
-            Symmetric3x3Wide.Add(projection.ImpulseToVelocityA, projection.NegatedImpulseToVelocityB, out var unsoftenedInverseEffectiveMass);
+            Symmetric3x3Wide.Add(inertiaA.InverseInertiaTensor, inertiaB.InverseInertiaTensor, out var unsoftenedInverseEffectiveMass);
             Symmetric3x3Wide.Invert(unsoftenedInverseEffectiveMass, out var unsoftenedEffectiveMass);
-            Symmetric3x3Wide.Scale(unsoftenedEffectiveMass, effectiveMassCFMScale, out projection.EffectiveMass);
+            //Note that we don't scale the effective mass directly; instead scale CSI.
 
             QuaternionWide.TransformWithoutOverlap(prestep.TargetVelocityLocalA, orientationA, out var biasVelocity);
-            Symmetric3x3Wide.TransformWithoutOverlap(biasVelocity, projection.EffectiveMass, out projection.BiasImpulse);
-        }
 
-        [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public void WarmStart(ref BodyVelocities velocityA, ref BodyVelocities velocityB, ref AngularMotorProjection projection, ref Vector3Wide accumulatedImpulse)
-        {
-            AngularServoFunctions.ApplyImpulse(ref velocityA.Angular, ref velocityB.Angular, projection.ImpulseToVelocityA, projection.NegatedImpulseToVelocityB, accumulatedImpulse);
-        }
+            //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);
 
-        [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public void Solve(ref BodyVelocities velocityA, ref BodyVelocities 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);
+            ServoSettingsWide.ClampImpulse(maximumImpulse, ref accumulatedImpulses, ref csi);
+            AngularServoFunctions.ApplyImpulse(ref wsvA.Angular, ref wsvB.Angular, inertiaA.InverseInertiaTensor, inertiaB.InverseInertiaTensor, csi);
         }
 
+        public static bool RequiresIncrementalSubstepUpdates => false;
+        [MethodImpl(MethodImplOptions.AggressiveInlining)]
+        public static void IncrementallyUpdateForSubstep(in Vector<float> dt, in BodyVelocityWide wsvA, in BodyVelocityWide wsvB, ref AngularMotorPrestepData prestepData) { }
     }
 
-    public class AngularMotorTypeProcessor : TwoBodyTypeProcessor<AngularMotorPrestepData, AngularMotorProjection, Vector3Wide, AngularMotorFunctions>
+    public class AngularMotorTypeProcessor : TwoBodyTypeProcessor<AngularMotorPrestepData, Vector3Wide, AngularMotorFunctions, AccessOnlyAngularWithoutPose, AccessOnlyAngularWithoutPose, AccessOnlyAngular, AccessOnlyAngularWithoutPose>
     {
         public const int BatchTypeId = 30;
     }
diff --git a/BepuPhysics/Constraints/AngularServo.cs b/BepuPhysics/Constraints/AngularServo.cs
index 4afee7639..c7fdf3945 100644
--- a/BepuPhysics/Constraints/AngularServo.cs
+++ b/BepuPhysics/Constraints/AngularServo.cs
@@ -26,7 +26,7 @@ public struct AngularServo : ITwoBodyConstraintDescription<AngularServo>
         /// </summary>
         public ServoSettings ServoSettings;
 
-        public readonly int ConstraintTypeId
+        public static int ConstraintTypeId
         {
             [MethodImpl(MethodImplOptions.AggressiveInlining)]
             get
@@ -35,7 +35,8 @@ public readonly int ConstraintTypeId
             }
         }
 
-        public readonly Type TypeProcessorType => typeof(AngularServoTypeProcessor);
+        public static Type TypeProcessorType => typeof(AngularServoTypeProcessor);
+        public static TypeProcessor CreateTypeProcessor() => new AngularServoTypeProcessor();
 
         public readonly void ApplyDescription(ref TypeBatch batch, int bundleIndex, int innerIndex)
         {
@@ -48,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<AngularServoPrestepData>.Get(ref batch.PrestepData, bundleIndex), innerIndex);
@@ -65,43 +66,8 @@ public struct AngularServoPrestepData
         public ServoSettingsWide ServoSettings;
     }
 
-    public struct AngularServoProjection
+    public struct AngularServoFunctions : ITwoBodyConstraintFunctions<AngularServoPrestepData, Vector3Wide>
     {
-        public Symmetric3x3Wide EffectiveMass;
-        public Vector3Wide BiasImpulse;
-        public Vector<float> SoftnessImpulseScale;
-        public Vector<float> MaximumImpulse;
-        public Symmetric3x3Wide ImpulseToVelocityA;
-        public Symmetric3x3Wide NegatedImpulseToVelocityB;
-    }
-
-
-    public struct AngularServoFunctions : IConstraintFunctions<AngularServoPrestepData, AngularServoProjection, 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 AngularServoPrestepData prestep, out AngularServoProjection projection)
-        {
-            bodies.GatherOrientation(ref bodyReferences, count, out var orientationA, out var orientationB);
-            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)
@@ -112,40 +78,66 @@ public static void ApplyImpulse(ref Vector3Wide angularVelocityA, ref Vector3Wid
             Vector3Wide.Subtract(angularVelocityB, negatedVelocityChangeB, out angularVelocityB);
         }
 
-        [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public void WarmStart(ref BodyVelocities velocityA, ref BodyVelocities velocityB, ref AngularServoProjection projection, ref Vector3Wide accumulatedImpulse)
+
+        //[MethodImpl(MethodImplOptions.AggressiveInlining)]
+        //public static void Solve(ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB,
+        //    in Symmetric3x3Wide effectiveMass, in Vector<float> softnessImpulseScale, in Vector3Wide biasImpulse, in Vector<float> 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);
+
+        //    ApplyImpulse(ref velocityA.Angular, ref velocityB.Angular, impulseToVelocityA, negatedImpulseToVelocityB, csi);
+        //}
+
+
+        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 velocityA.Angular, ref velocityB.Angular, projection.ImpulseToVelocityA, projection.NegatedImpulseToVelocityB, accumulatedImpulse);
+            ApplyImpulse(ref wsvA.Angular, ref wsvB.Angular, inertiaA.InverseInertiaTensor, inertiaB.InverseInertiaTensor, accumulatedImpulses);
         }
 
-        [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public static void Solve(ref BodyVelocities velocityA, ref BodyVelocities velocityB,
-            in Symmetric3x3Wide effectiveMass, in Vector<float> softnessImpulseScale, in Vector3Wide biasImpulse, in Vector<float> maximumImpulse,
-            in Symmetric3x3Wide impulseToVelocityA, in Symmetric3x3Wide negatedImpulseToVelocityB, ref Vector3Wide accumulatedImpulse)
+        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.
-            Vector3Wide.Subtract(velocityA.Angular, velocityB.Angular, out var csv);
-            Symmetric3x3Wide.TransformWithoutOverlap(csv, effectiveMass, out var csiVelocityComponent);
+            QuaternionWide.ConcatenateWithoutOverlap(prestep.TargetRelativeRotationLocalA, orientationA, out var targetOrientationB);
+            QuaternionWide.Conjugate(targetOrientationB, out var inverseTarget);
+            QuaternionWide.ConcatenateWithoutOverlap(inverseTarget, orientationB, out var errorRotation);
+
+            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);
+            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.Scale(accumulatedImpulse, softnessImpulseScale, out var softnessComponent);
-            Vector3Wide.Subtract(biasImpulse, softnessComponent, out var csi);
-            Vector3Wide.Subtract(csi, csiVelocityComponent, out csi);
+            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 accumulatedImpulse, ref csi);
+            ServoSettingsWide.ClampImpulse(maximumImpulse, ref accumulatedImpulses, ref csi);
 
-            ApplyImpulse(ref velocityA.Angular, ref velocityB.Angular, impulseToVelocityA, negatedImpulseToVelocityB, csi);
+            ApplyImpulse(ref wsvA.Angular, ref wsvB.Angular, inertiaA.InverseInertiaTensor, inertiaB.InverseInertiaTensor, csi);
         }
 
+        public static bool RequiresIncrementalSubstepUpdates => false;
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public void Solve(ref BodyVelocities velocityA, ref BodyVelocities 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 static void IncrementallyUpdateForSubstep(in Vector<float> dt, in BodyVelocityWide wsvA, in BodyVelocityWide wsvB, ref AngularServoPrestepData prestepData) { }
     }
 
-    public class AngularServoTypeProcessor : TwoBodyTypeProcessor<AngularServoPrestepData, AngularServoProjection, Vector3Wide, AngularServoFunctions>
+    public class AngularServoTypeProcessor : TwoBodyTypeProcessor<AngularServoPrestepData, Vector3Wide, AngularServoFunctions, AccessOnlyAngularWithoutPose, AccessOnlyAngularWithoutPose, AccessOnlyAngular, AccessOnlyAngular>
     {
         public const int BatchTypeId = 29;
     }
diff --git a/BepuPhysics/Constraints/AngularSwivelHinge.cs b/BepuPhysics/Constraints/AngularSwivelHinge.cs
index da2dea129..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;
@@ -26,7 +25,7 @@ public struct AngularSwivelHinge : ITwoBodyConstraintDescription<AngularSwivelHi
         /// </summary>
         public SpringSettings SpringSettings;
 
-        public readonly int ConstraintTypeId
+        public static int ConstraintTypeId
         {
             [MethodImpl(MethodImplOptions.AggressiveInlining)]
             get
@@ -35,7 +34,8 @@ public readonly int ConstraintTypeId
             }
         }
 
-        public readonly Type TypeProcessorType => typeof(AngularSwivelHingeTypeProcessor);
+        public static Type TypeProcessorType => typeof(AngularSwivelHingeTypeProcessor);
+        public static TypeProcessor CreateTypeProcessor() => new AngularSwivelHingeTypeProcessor();
 
         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
             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<AngularSwivelHingePrestepData>.Get(ref batch.PrestepData, bundleIndex), innerIndex);
@@ -68,24 +68,41 @@ public struct AngularSwivelHingePrestepData
         public SpringSettingsWide SpringSettings;
     }
 
-    public struct AngularSwivelHingeProjection
+    public struct AngularSwivelHingeFunctions : ITwoBodyConstraintFunctions<AngularSwivelHingePrestepData, Vector<float>>
     {
-        //JacobianB = -JacobianA, so no need to store it explicitly.
-        public Vector3Wide VelocityToImpulseA;
-        public Vector<float> BiasImpulse;
-        public Vector<float> SoftnessImpulseScale;
-        public Vector3Wide ImpulseToVelocityA;
-        public Vector3Wide NegatedImpulseToVelocityB;
-    }
+        [MethodImpl(MethodImplOptions.AggressiveInlining)]
+        private static void ApplyImpulse(in Vector3Wide impulseToVelocityA, in Vector3Wide negatedImpulseToVelocityB, in Vector<float> 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);
+        }
 
-    public struct AngularSwivelHingeFunctions : IConstraintFunctions<AngularSwivelHingePrestepData, AngularSwivelHingeProjection, Vector<float>>
-    {
         [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)
+        static void ComputeJacobian(in Vector3Wide localSwivelAxisA, in Vector3Wide localHingeAxisB, in QuaternionWide orientationA, in QuaternionWide orientationB, out Vector3Wide swivelAxis, out Vector3Wide hingeAxis, out Vector3Wide jacobianA)
         {
-            bodies.GatherOrientation(ref bodyReferences, count, out var orientationA, out var orientationB);
+            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<float>(1e-3f));
+            Vector3Wide.ConditionalSelect(useFallback, fallbackJacobian, jacobianA, out jacobianA);
+        }
 
+        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<float> 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);
+            Symmetric3x3Wide.TransformWithoutOverlap(jacobianA, inertiaB.InverseInertiaTensor, out var negatedImpulseToVelocityB);
+            ApplyImpulse(impulseToVelocityA, negatedImpulseToVelocityB, accumulatedImpulses, ref wsvA.Angular, ref wsvB.Angular);
+        }
+
+        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<float> 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
             //C' = dot(d/dt(swivelA), hingeB) + dot(swivelA, d/dt(hingeB)) = 0
@@ -96,69 +113,41 @@ public void Prestep(Bodies bodies, ref TwoBodyReferences bodyReferences, int cou
             //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<float>(1e-7f));
-            Vector3Wide.ConditionalSelect(useFallback, fallbackJacobian, jacobianA, out jacobianA);
+            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 projection.ImpulseToVelocityA);
+            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 projection.NegatedImpulseToVelocityB);
-            Vector3Wide.Dot(projection.ImpulseToVelocityA, jacobianA, out var angularA);
-            Vector3Wide.Dot(projection.NegatedImpulseToVelocityB, jacobianA, out var angularB);
+            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 projection.SoftnessImpulseScale);
+            SpringSettingsWide.ComputeSpringiness(prestep.SpringSettings, dt, out var positionErrorToVelocity, out var effectiveMassCFMScale, out var 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;
-
-        }
+            //Note the negation: we want to oppose the separation.
+            var biasVelocity = -(positionErrorToVelocity * error);
 
-        [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        private static void ApplyImpulse(ref Vector3Wide angularVelocityA, ref Vector3Wide angularVelocityB, ref AngularSwivelHingeProjection projection, ref Vector<float> 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 BodyVelocities velocityA, ref BodyVelocities velocityB, ref AngularSwivelHingeProjection projection, ref Vector<float> 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<float> 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);
+            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);
-            csi = projection.BiasImpulse - accumulatedImpulse * projection.SoftnessImpulseScale - csi;
+            var csi = effectiveMass * (biasVelocity - csv) - accumulatedImpulses * softnessImpulseScale;
+
+            accumulatedImpulses += csi;
+            ApplyImpulse(impulseToVelocityA, negatedImpulseToVelocityB, csi, ref wsvA.Angular, ref wsvB.Angular);
 
-            accumulatedImpulse += csi;
-            ApplyImpulse(ref velocityA.Angular, ref velocityB.Angular, ref projection, ref csi);
         }
 
+        public static bool RequiresIncrementalSubstepUpdates => false;
+        [MethodImpl(MethodImplOptions.AggressiveInlining)]
+        public static void IncrementallyUpdateForSubstep(in Vector<float> dt, in BodyVelocityWide wsvA, in BodyVelocityWide wsvB, ref AngularSwivelHingePrestepData prestepData) { }
     }
 
-    public class AngularSwivelHingeTypeProcessor : TwoBodyTypeProcessor<AngularSwivelHingePrestepData, AngularSwivelHingeProjection, Vector<float>, AngularSwivelHingeFunctions>
+    public class AngularSwivelHingeTypeProcessor : TwoBodyTypeProcessor<AngularSwivelHingePrestepData, Vector<float>, AngularSwivelHingeFunctions, AccessOnlyAngular, AccessOnlyAngular, AccessOnlyAngular, AccessOnlyAngular>
     {
         public const int BatchTypeId = 24;
     }
diff --git a/BepuPhysics/Constraints/AreaConstraint.cs b/BepuPhysics/Constraints/AreaConstraint.cs
index 140072583..12e4aab08 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;
@@ -31,13 +30,13 @@ public struct AreaConstraint : IThreeBodyConstraintDescription<AreaConstraint>
         /// <param name="c">Initial position of the third body.</param>
         /// <param name="springSettings">Spring settings to apply to the volume constraint.</param>
         [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;
         }
 
-        public readonly int ConstraintTypeId
+        public static int ConstraintTypeId
         {
             [MethodImpl(MethodImplOptions.AggressiveInlining)]
             get
@@ -46,7 +45,8 @@ public readonly int ConstraintTypeId
             }
         }
 
-        public readonly Type TypeProcessorType => typeof(AreaConstraintTypeProcessor);
+        public static Type TypeProcessorType => typeof(AreaConstraintTypeProcessor);
+        public static TypeProcessor CreateTypeProcessor() => new AreaConstraintTypeProcessor();
 
         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
             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<AreaConstraintPrestepData>.Get(ref batch.PrestepData, bundleIndex), innerIndex);
@@ -73,27 +73,28 @@ public struct AreaConstraintPrestepData
         public SpringSettingsWide SpringSettings;
     }
 
-    public struct AreaConstraintProjection
-    {
-        public Vector3Wide JacobianB;
-        public Vector3Wide JacobianC;
-        public Vector<float> EffectiveMass;
-        public Vector<float> BiasImpulse;
-        public Vector<float> SoftnessImpulseScale;
-        public Vector<float> InverseMassA;
-        public Vector<float> InverseMassB;
-        public Vector<float> InverseMassC;
-    }
-
-    public struct AreaConstraintFunctions : IThreeBodyConstraintFunctions<AreaConstraintPrestepData, AreaConstraintProjection, Vector<float>>
+    public struct AreaConstraintFunctions : IThreeBodyConstraintFunctions<AreaConstraintPrestepData, Vector<float>>
     {
         [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)
+        private static void ApplyImpulse(in Vector<float> inverseMassA, in Vector<float> inverseMassB, in Vector<float> inverseMassC,
+            in Vector3Wide negatedJacobianA, in Vector3Wide jacobianB, in Vector3Wide jacobianC, in Vector<float> impulse,
+            ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB, ref BodyVelocityWide velocityC)
         {
-            bodies.GatherOffsets(ref bodyReferences, count, out var ab, out var ac);
+            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);
+        }
 
+        [MethodImpl(MethodImplOptions.AggressiveInlining)]
+        static void ComputeJacobian(in Vector3Wide positionA, in Vector3Wide positionB, in Vector3Wide positionC,
+            out Vector<float> normalLength,
+            out Vector3Wide negatedJacobianA, out Vector3Wide jacobianB, out Vector3Wide jacobianC,
+            out Vector<float> contributionA, out Vector<float> contributionB, out Vector<float> contributionC,
+            out Vector<float> inverseJacobianLength)
+        {
             //Area of a triangle with vertices a, b, and c is:
             //||ab x ac|| * 0.5
             //So the constraint is:
@@ -110,87 +111,92 @@ public void Prestep(Bodies bodies, ref ThreeBodyReferences bodyReferences, int c
             //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 var normalLength);
+            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<float>(1e-10f)), Vector<float>.One / normalLength, Vector<float>.Zero), out var normal);
 
-            Vector3Wide.CrossWithoutOverlap(ac, normal, out projection.JacobianB);
-            Vector3Wide.CrossWithoutOverlap(normal, ab, out projection.JacobianC);
+            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(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;
+            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<float>(1e-14f), jacobianLengthSquared);
+            inverseJacobianLength = MathHelper.FastReciprocalSquareRoot(jacobianLengthSquared);
         }
 
-        [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        private static void ApplyImpulse(ref BodyVelocities velocityA, ref BodyVelocities velocityB, ref BodyVelocities velocityC,
-            ref AreaConstraintProjection projection, ref Vector3Wide negatedJacobianA, ref Vector<float> impulse)
+        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,
+            ref AreaConstraintPrestepData prestep, ref Vector<float> accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB, ref BodyVelocityWide wsvC)
         {
-            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 BodyVelocities velocityA, ref BodyVelocities velocityB, ref BodyVelocities velocityC, ref AreaConstraintProjection projection, ref Vector<float> accumulatedImpulse)
-        {
-            Vector3Wide.Add(projection.JacobianB, projection.JacobianC, out var negatedJacobianA);
-            ApplyImpulse(ref velocityA, ref velocityB, ref velocityC, ref projection, ref negatedJacobianA, ref accumulatedImpulse);
+            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);
         }
 
-        [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public void Solve(ref BodyVelocities velocityA, ref BodyVelocities velocityB, ref BodyVelocities velocityC, ref AreaConstraintProjection projection, ref Vector<float> accumulatedImpulse)
+        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<float> accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB, ref BodyVelocityWide wsvC)
         {
+            //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<float>(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) * inverseJacobianLength * positionErrorToVelocity;
+
             //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);
+            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 = inverseJacobianLength * (velocityContributionB + velocityContributionC - negatedVelocityContributionA);
+            var csi = (biasVelocity - csv) * effectiveMass - accumulatedImpulses * softnessImpulseScale;
+            accumulatedImpulses += csi;
+
+            ApplyImpulse(inertiaA.InverseMass, inertiaB.InverseMass, inertiaC.InverseMass, negatedJacobianA, jacobianB, jacobianC, inverseJacobianLength * csi, ref wsvA, ref wsvB, ref wsvC);
         }
+
+        public static bool RequiresIncrementalSubstepUpdates => false;
+        [MethodImpl(MethodImplOptions.AggressiveInlining)]
+        public static void IncrementallyUpdateForSubstep(in Vector<float> dt, in BodyVelocityWide wsvA, in BodyVelocityWide wsvB, in BodyVelocityWide wsvC, ref AreaConstraintPrestepData prestepData) { }
     }
 
 
     /// <summary>
-    /// Handles the solve iterations of a bunch of ball socket constraints.
+    /// Handles the solve iterations of a bunch of area constraints.
     /// </summary>
-    public class AreaConstraintTypeProcessor : ThreeBodyTypeProcessor<AreaConstraintPrestepData, AreaConstraintProjection, Vector<float>, AreaConstraintFunctions>
+    public class AreaConstraintTypeProcessor : ThreeBodyTypeProcessor<AreaConstraintPrestepData, Vector<float>, 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 d8dad9704..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;
@@ -14,11 +13,11 @@ namespace BepuPhysics.Constraints
     public struct BallSocket : ITwoBodyConstraintDescription<BallSocket>
     {
         /// <summary>
-        /// 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.
         /// </summary>
         public Vector3 LocalOffsetA;
         /// <summary>
-        /// 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.
         /// </summary>
         public Vector3 LocalOffsetB;
         /// <summary>
@@ -26,7 +25,7 @@ public struct BallSocket : ITwoBodyConstraintDescription<BallSocket>
         /// </summary>
         public SpringSettings SpringSettings;
 
-        public readonly int ConstraintTypeId
+        public static int ConstraintTypeId
         {
             [MethodImpl(MethodImplOptions.AggressiveInlining)]
             get
@@ -35,7 +34,8 @@ public readonly int ConstraintTypeId
             }
         }
 
-        public readonly Type TypeProcessorType => typeof(BallSocketTypeProcessor);
+        public static Type TypeProcessorType => typeof(BallSocketTypeProcessor);
+        public static TypeProcessor CreateTypeProcessor() => new BallSocketTypeProcessor();
 
         public readonly void ApplyDescription(ref TypeBatch batch, int bundleIndex, int innerIndex)
         {
@@ -47,7 +47,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<BallSocketPrestepData>.Get(ref batch.PrestepData, bundleIndex), innerIndex);
@@ -63,59 +63,43 @@ public struct BallSocketPrestepData
         public Vector3Wide LocalOffsetB;
         public SpringSettingsWide SpringSettings;
     }
-
-    public struct BallSocketProjection
-    {
-        public Vector3Wide OffsetA;
-        public Vector3Wide OffsetB;
-        public Vector3Wide BiasVelocity;
-        public Symmetric3x3Wide EffectiveMass;
-        public Vector<float> SoftnessImpulseScale;
-        public BodyInertias InertiaA;
-        public BodyInertias InertiaB;
-    }
-
-    public struct BallSocketFunctions : IConstraintFunctions<BallSocketPrestepData, BallSocketProjection, Vector3Wide>
+    public struct BallSocketFunctions : ITwoBodyConstraintFunctions<BallSocketPrestepData, 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 BallSocketPrestepData prestep, out BallSocketProjection projection)
+        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)
         {
-            bodies.GatherPose(ref bodyReferences, count, out var offsetB, out var orientationA, out var orientationB);
-            projection.InertiaA = inertiaA;
-            projection.InertiaB = inertiaB;
+            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);
+        }
 
-            //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(ref inertiaA, ref inertiaB, ref projection.OffsetA, ref projection.OffsetB, ref effectiveMassCFMScale, out projection.EffectiveMass);
+        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);
+            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.
-            Vector3Wide.Add(offsetB, projection.OffsetB, out var anchorB);
-            Vector3Wide.Subtract(anchorB, projection.OffsetA, out var error);
-            Vector3Wide.Scale(error, positionErrorToVelocity, out projection.BiasVelocity);
-        }
-
+            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);
 
-        [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.Solve(ref wsvA, ref wsvB, offsetA, offsetB, biasVelocity, effectiveMass, softnessImpulseScale, ref accumulatedImpulses, inertiaA, inertiaB);
         }
 
+        public static bool RequiresIncrementalSubstepUpdates => false;
         [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);
-        }
+        public static void IncrementallyUpdateForSubstep(in Vector<float> dt, in BodyVelocityWide wsvA, in BodyVelocityWide wsvB, ref BallSocketPrestepData prestepData) { }
     }
 
 
     /// <summary>
     /// Handles the solve iterations of a bunch of ball socket constraints.
     /// </summary>
-    public class BallSocketTypeProcessor : TwoBodyTypeProcessor<BallSocketPrestepData, BallSocketProjection, Vector3Wide, BallSocketFunctions>
+    public class BallSocketTypeProcessor : TwoBodyTypeProcessor<BallSocketPrestepData, Vector3Wide, BallSocketFunctions, AccessNoPosition, AccessNoPosition, AccessAll, AccessAll>
     {
         public const int BatchTypeId = 22;
     }
diff --git a/BepuPhysics/Constraints/BallSocketMotor.cs b/BepuPhysics/Constraints/BallSocketMotor.cs
index 7704fe1df..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;
@@ -12,13 +9,12 @@
 namespace BepuPhysics.Constraints
 {
     /// <summary>
-    /// 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.
     /// </summary>
     public struct BallSocketMotor : ITwoBodyConstraintDescription<BallSocketMotor>
     {
         /// <summary>
-        /// Offset from body B to its anchor.
+        /// Offset from body B to its attachment in B's local space.
         /// </summary>
         public Vector3 LocalOffsetB;
         /// <summary>
@@ -30,7 +26,7 @@ public struct BallSocketMotor : ITwoBodyConstraintDescription<BallSocketMotor>
         /// </summary>
         public MotorSettings Settings;
 
-        public readonly int ConstraintTypeId
+        public static int ConstraintTypeId
         {
             [MethodImpl(MethodImplOptions.AggressiveInlining)]
             get
@@ -39,7 +35,8 @@ public readonly int ConstraintTypeId
             }
         }
 
-        public readonly Type TypeProcessorType => typeof(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 +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 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<BallSocketMotorPrestepData>.Get(ref batch.PrestepData, bundleIndex), innerIndex);
@@ -68,57 +65,38 @@ public struct BallSocketMotorPrestepData
         public MotorSettingsWide Settings;
     }
 
-    public struct BallSocketMotorProjection
+    public struct BallSocketMotorFunctions : ITwoBodyConstraintFunctions<BallSocketMotorPrestepData, Vector3Wide>
     {
-        public Vector3Wide OffsetA;
-        public Vector3Wide OffsetB;
-        public Vector3Wide BiasVelocity;
-        public Symmetric3x3Wide EffectiveMass;
-        public Vector<float> SoftnessImpulseScale;
-        public Vector<float> MaximumImpulse;
-        public BodyInertias InertiaA;
-        public BodyInertias InertiaB;
-    }
+        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 struct BallSocketMotorFunctions : IConstraintFunctions<BallSocketMotorPrestepData, BallSocketMotorProjection, 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 BallSocketMotorPrestepData prestep, out BallSocketMotorProjection projection)
+        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)
         {
-            bodies.GatherPose(ref bodyReferences, count, out var offsetFromACenterToBCenter, out var orientationA, out var orientationB);
-            projection.InertiaA = inertiaA;
-            projection.InertiaB = inertiaB;
+            QuaternionWide.TransformWithoutOverlap(prestep.LocalOffsetB, orientationB, out var targetOffsetB);
+            var offsetA = (positionB - positionA) + targetOffsetB;
 
-            //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);
-            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);
+            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 projection.BiasVelocity);
-            Vector3Wide.Negate(projection.BiasVelocity, out projection.BiasVelocity);
-        }
+            QuaternionWide.Transform(prestep.TargetVelocityLocalA, orientationA, out var biasVelocity);
+            Vector3Wide.Negate(biasVelocity, out biasVelocity);
 
-        [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.Solve(ref wsvA, ref wsvB, offsetA, targetOffsetB, biasVelocity, effectiveMass, softnessImpulseScale, maximumImpulse, ref accumulatedImpulses, inertiaA, inertiaB);
         }
 
+        public static bool RequiresIncrementalSubstepUpdates => false;
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public void Solve(ref BodyVelocities velocityA, ref BodyVelocities 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);
-        }
-
+        public static void IncrementallyUpdateForSubstep(in Vector<float> dt, in BodyVelocityWide wsvA, in BodyVelocityWide wsvB, ref BallSocketMotorPrestepData prestepData) { }
     }
 
 
     /// <summary>
     /// Handles the solve iterations of a bunch of ball socket motor constraints.
     /// </summary>
-    public class BallSocketMotorTypeProcessor : TwoBodyTypeProcessor<BallSocketMotorPrestepData, BallSocketMotorProjection, Vector3Wide, BallSocketMotorFunctions>
+    public class BallSocketMotorTypeProcessor : TwoBodyTypeProcessor<BallSocketMotorPrestepData, Vector3Wide, BallSocketMotorFunctions, AccessNoOrientation, AccessAll, AccessAll, AccessAll>
     {
         public const int BatchTypeId = 52;
     }
diff --git a/BepuPhysics/Constraints/BallSocketServo.cs b/BepuPhysics/Constraints/BallSocketServo.cs
index c49a244f9..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;
@@ -15,11 +14,11 @@ namespace BepuPhysics.Constraints
     public struct BallSocketServo : ITwoBodyConstraintDescription<BallSocketServo>
     {
         /// <summary>
-        /// 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.
         /// </summary>
         public Vector3 LocalOffsetA;
         /// <summary>
-        /// 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.
         /// </summary>
         public Vector3 LocalOffsetB;
         /// <summary>
@@ -31,7 +30,7 @@ public struct BallSocketServo : ITwoBodyConstraintDescription<BallSocketServo>
         /// </summary>
         public ServoSettings ServoSettings;
 
-        public readonly int ConstraintTypeId
+        public static int ConstraintTypeId
         {
             [MethodImpl(MethodImplOptions.AggressiveInlining)]
             get
@@ -40,7 +39,8 @@ public readonly int ConstraintTypeId
             }
         }
 
-        public readonly Type TypeProcessorType => typeof(BallSocketServoTypeProcessor);
+        public static Type TypeProcessorType => typeof(BallSocketServoTypeProcessor);
+        public static TypeProcessor CreateTypeProcessor() => new BallSocketServoTypeProcessor();
 
         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 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<BallSocketServoPrestepData>.Get(ref batch.PrestepData, bundleIndex), innerIndex);
@@ -72,59 +72,41 @@ public struct BallSocketServoPrestepData
         public ServoSettingsWide ServoSettings;
     }
 
-    public struct BallSocketServoProjection
+    public struct BallSocketServoFunctions : ITwoBodyConstraintFunctions<BallSocketServoPrestepData, Vector3Wide>
     {
-        public Vector3Wide OffsetA;
-        public Vector3Wide OffsetB;
-        public Vector3Wide BiasVelocity;
-        public Symmetric3x3Wide EffectiveMass;
-        public Vector<float> SoftnessImpulseScale;
-        public Vector<float> MaximumImpulse;
-        public BodyInertias InertiaA;
-        public BodyInertias InertiaB;
-    }
-
-    public struct BallSocketServoFunctions : IConstraintFunctions<BallSocketServoPrestepData, BallSocketServoProjection, 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 BallSocketServoPrestepData prestep, out BallSocketServoProjection projection)
+        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)
         {
-            bodies.GatherPose(ref bodyReferences, count, out var offsetFromACenterToBCenter, out var orientationA, out var orientationB);
-            projection.InertiaA = inertiaA;
-            projection.InertiaB = inertiaB;
+            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);
+        }
 
-            //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(ref inertiaA, ref inertiaB, ref projection.OffsetA, ref projection.OffsetB, ref effectiveMassCFMScale, out projection.EffectiveMass);
+        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);
+            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.
-            Vector3Wide.Add(offsetFromACenterToBCenter, 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);
-        }
-
+            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);
 
-        [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.Solve(ref wsvA, ref wsvB, offsetA, offsetB, biasVelocity, effectiveMass, softnessImpulseScale, maximumImpulse, ref accumulatedImpulses, inertiaA, inertiaB);
         }
 
+        public static bool RequiresIncrementalSubstepUpdates => false;
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public void Solve(ref BodyVelocities velocityA, ref BodyVelocities 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);
-        }
+        public static void IncrementallyUpdateForSubstep(in Vector<float> dt, in BodyVelocityWide wsvA, in BodyVelocityWide wsvB, ref BallSocketServoPrestepData prestepData) { }
     }
 
 
     /// <summary>
     /// Handles the solve iterations of a bunch of ball socket servo constraints.
     /// </summary>
-    public class BallSocketServoTypeProcessor : TwoBodyTypeProcessor<BallSocketServoPrestepData, BallSocketServoProjection, Vector3Wide, BallSocketServoFunctions>
+    public class BallSocketServoTypeProcessor : TwoBodyTypeProcessor<BallSocketServoPrestepData, Vector3Wide, BallSocketServoFunctions, AccessNoPosition, AccessNoPosition, AccessAll, AccessAll>
     {
         public const int BatchTypeId = 53;
     }
diff --git a/BepuPhysics/Constraints/BallSocketShared.cs b/BepuPhysics/Constraints/BallSocketShared.cs
index a0a9a5a39..b985731c4 100644
--- a/BepuPhysics/Constraints/BallSocketShared.cs
+++ b/BepuPhysics/Constraints/BallSocketShared.cs
@@ -16,8 +16,8 @@ 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,
-            ref Vector3Wide offsetA, ref Vector3Wide offsetB, ref Vector<float> effectiveMassCFMScale, out Symmetric3x3Wide effectiveMass)
+        public static void ComputeEffectiveMass(in BodyInertiaWide inertiaA, in BodyInertiaWide inertiaB,
+            in Vector3Wide offsetA, in Vector3Wide offsetB, in Vector<float> 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
@@ -74,8 +74,8 @@ public static void ComputeEffectiveMass(ref BodyInertias inertiaA, ref BodyInert
         }
 
         [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)
+        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,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<float> softnessImpulseScale, ref Vector3Wide accumulatedImpulse, out Vector3Wide correctiveImpulse)
+        public static void ComputeCorrectiveImpulse(ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB, in Vector3Wide offsetA, in Vector3Wide offsetB,
+            in Vector3Wide biasVelocity, in Symmetric3x3Wide effectiveMass, in Vector<float> 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<float> softnessImpulseScale, ref Vector3Wide accumulatedImpulse, ref BodyInertias inertiaA, ref 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<float> softnessImpulseScale, ref Vector3Wide accumulatedImpulse, in BodyInertiaWide inertiaA, in BodyInertiaWide 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<float> softnessImpulseScale, ref Vector<float> maximumImpulse, ref Vector3Wide accumulatedImpulse, ref BodyInertias inertiaA, ref 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<float> softnessImpulseScale, in Vector<float> maximumImpulse, ref Vector3Wide accumulatedImpulse, in BodyInertiaWide inertiaA, in BodyInertiaWide 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/CenterDistanceConstraint.cs b/BepuPhysics/Constraints/CenterDistanceConstraint.cs
index aa7a3c557..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;
@@ -30,7 +29,7 @@ public CenterDistanceConstraint(float targetDistance, in SpringSettings springSe
             SpringSettings = springSettings;
         }
 
-        public readonly int ConstraintTypeId
+        public static int ConstraintTypeId
         {
             [MethodImpl(MethodImplOptions.AggressiveInlining)]
             get
@@ -39,7 +38,8 @@ public readonly int ConstraintTypeId
             }
         }
 
-        public readonly Type TypeProcessorType => typeof(CenterDistanceTypeProcessor);
+        public static Type TypeProcessorType => typeof(CenterDistanceTypeProcessor);
+        public static TypeProcessor CreateTypeProcessor() => new CenterDistanceTypeProcessor();
 
         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
             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<CenterDistancePrestepData>.Get(ref batch.PrestepData, bundleIndex), innerIndex);
@@ -66,76 +66,73 @@ public struct CenterDistancePrestepData
         public SpringSettingsWide SpringSettings;
     }
 
-    public struct CenterDistanceProjection
+    public struct CenterDistanceConstraintFunctions : ITwoBodyConstraintFunctions<CenterDistancePrestepData, Vector<float>>
     {
-        public Vector3Wide JacobianA;
-        public Vector<float> BiasVelocity;
-        public Vector<float> SoftnessImpulseScale;
-        public Vector<float> EffectiveMass;
-        public Vector<float> InverseMassA;
-        public Vector<float> InverseMassB;
-    }
-
-    public struct CenterDistanceConstraintFunctions : IConstraintFunctions<CenterDistancePrestepData, CenterDistanceProjection, Vector<float>>
-    {
-        [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)
-        {
-            bodies.GatherOffsets(ref bodyReferences, count, out var ab);
-
-            Vector3Wide.Length(ab, out var distance);
-            Vector3Wide.Scale(ab, Vector<float>.One / distance, out projection.JacobianA);
-
-            var useFallback = Vector.LessThan(distance, new Vector<float>(1e-10f));
-            projection.JacobianA.X = Vector.ConditionalSelect(useFallback, Vector<float>.One, projection.JacobianA.X);
-            projection.JacobianA.Y = Vector.ConditionalSelect(useFallback, Vector<float>.Zero, projection.JacobianA.Y);
-            projection.JacobianA.Z = Vector.ConditionalSelect(useFallback, Vector<float>.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 BodyVelocities a, ref BodyVelocities b, ref CenterDistanceProjection projection, ref Vector<float> impulse)
+        public static void ApplyImpulse(in Vector3Wide jacobianA, in Vector<float> inverseMassA, in Vector<float> inverseMassB, in Vector<float> impulse, ref BodyVelocityWide a, ref BodyVelocityWide b)
         {
-            Vector3Wide.Scale(projection.JacobianA, impulse * projection.InverseMassA, out var changeA);
-            Vector3Wide.Scale(projection.JacobianA, impulse * projection.InverseMassB, out var negatedChangeB);
+            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.AggressiveInlining)]
-        public void WarmStart(ref BodyVelocities velocityA, ref BodyVelocities velocityB, ref CenterDistanceProjection projection, ref Vector<float> accumulatedImpulse)
+
+        //[MethodImpl(MethodImplOptions.AggressiveInlining)]
+        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<float> accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB)
         {
-            ApplyImpulse(ref velocityA, ref velocityB, ref projection, ref accumulatedImpulse);
+            var ab = positionB - positionA;
+            var lengthSquared = ab.LengthSquared();
+            var inverseDistance = MathHelper.FastReciprocalSquareRoot(lengthSquared);
+            var useFallback = Vector.LessThan(lengthSquared, new Vector<float>(1e-10f));
+            Vector3Wide.Scale(ab, inverseDistance, out var jacobianA);
+            jacobianA.X = Vector.ConditionalSelect(useFallback, Vector<float>.One, jacobianA.X);
+            jacobianA.Y = Vector.ConditionalSelect(useFallback, Vector<float>.Zero, jacobianA.Y);
+            jacobianA.Z = Vector.ConditionalSelect(useFallback, Vector<float>.Zero, jacobianA.Z);
+
+            ApplyImpulse(jacobianA, inertiaA.InverseMass, inertiaB.InverseMass, accumulatedImpulses, ref wsvA, ref wsvB);
         }
-
-        [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public void Solve(ref BodyVelocities velocityA, ref BodyVelocities velocityB, ref CenterDistanceProjection projection, ref Vector<float> accumulatedImpulse)
+        //[MethodImpl(MethodImplOptions.AggressiveInlining)]
+        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<float> 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<float>(1e-5f));
+            Vector3Wide.Scale(ab, inverseDistance, out var jacobianA);
+            jacobianA.X = Vector.ConditionalSelect(useFallback, Vector<float>.One, jacobianA.X);
+            jacobianA.Y = Vector.ConditionalSelect(useFallback, Vector<float>.Zero, jacobianA.Y);
+            jacobianA.Z = Vector.ConditionalSelect(useFallback, Vector<float>.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(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;
+            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(ref velocityA, ref velocityB, ref projection, ref csi);
+            ApplyImpulse(jacobianA, inertiaA.InverseMass, inertiaB.InverseMass, csi, ref wsvA, ref wsvB);
         }
 
+        public static bool RequiresIncrementalSubstepUpdates => false;
+        [MethodImpl(MethodImplOptions.AggressiveInlining)]
+        public static void IncrementallyUpdateForSubstep(in Vector<float> dt, in BodyVelocityWide wsvA, in BodyVelocityWide wsvB, ref CenterDistancePrestepData prestepData) { }
+
     }
 
 
     /// <summary>
     /// Handles the solve iterations of a bunch of distance servos.
     /// </summary>
-    public class CenterDistanceTypeProcessor : TwoBodyTypeProcessor<CenterDistancePrestepData, CenterDistanceProjection, Vector<float>, CenterDistanceConstraintFunctions>
+    public class CenterDistanceTypeProcessor : TwoBodyTypeProcessor<CenterDistancePrestepData, Vector<float>, CenterDistanceConstraintFunctions, AccessOnlyLinear, AccessOnlyLinear, AccessOnlyLinear, AccessOnlyLinear>
     {
         public const int BatchTypeId = 35;
     }
diff --git a/BepuPhysics/Constraints/CenterDistanceLimit.cs b/BepuPhysics/Constraints/CenterDistanceLimit.cs
new file mode 100644
index 000000000..3ecc5d874
--- /dev/null
+++ b/BepuPhysics/Constraints/CenterDistanceLimit.cs
@@ -0,0 +1,138 @@
+using BepuUtilities;
+using BepuUtilities.Memory;
+using System;
+using System.Diagnostics;
+using System.Numerics;
+using System.Runtime.CompilerServices;
+using static BepuUtilities.GatherScatter;
+namespace BepuPhysics.Constraints
+{
+
+    /// <summary>
+    /// Constrains the center of two bodies to be separated by a distance within a range.
+    /// </summary>
+    public struct CenterDistanceLimit : ITwoBodyConstraintDescription<CenterDistanceLimit>
+    {
+        /// <summary>
+        /// Minimum distance between the body centers.
+        /// </summary>
+        public float MinimumDistance;
+        /// <summary>
+        /// Maximum distance between the body centers.
+        /// </summary>
+        public float MaximumDistance;
+        /// <summary>
+        /// Spring frequency and damping parameters.
+        /// </summary>
+        public SpringSettings SpringSettings;
+
+        [MethodImpl(MethodImplOptions.AggressiveInlining)]
+        public CenterDistanceLimit(float minimumDistance, float maximumDistance, in SpringSettings springSettings)
+        {
+            MinimumDistance = minimumDistance;
+            MaximumDistance = maximumDistance;
+            SpringSettings = springSettings;
+        }
+
+        public static int ConstraintTypeId
+        {
+            [MethodImpl(MethodImplOptions.AggressiveInlining)]
+            get
+            {
+                return CenterDistanceLimitTypeProcessor.BatchTypeId;
+            }
+        }
+
+        public static Type TypeProcessorType => typeof(CenterDistanceLimitTypeProcessor);
+        public static TypeProcessor CreateTypeProcessor() => new 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<CenterDistanceLimitPrestepData>.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 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<CenterDistanceLimitPrestepData>.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<float> MinimumDistance;
+        public Vector<float> MaximumDistance;
+        public SpringSettingsWide SpringSettings;
+    }
+
+    public struct CenterDistanceLimitFunctions : ITwoBodyConstraintFunctions<CenterDistanceLimitPrestepData, Vector<float>>
+    {
+        [MethodImpl(MethodImplOptions.AggressiveInlining)]
+        static void ComputeJacobian(Vector<float> minimumDistance, Vector<float> maximumDistance, in Vector3Wide positionA, in Vector3Wide positionB, out Vector3Wide jacobianA, out Vector<float> distance, out Vector<int> 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<float>(1e-5f));
+            Vector3Wide.Scale(ab, inverseDistance, out jacobianA);
+            jacobianA.X = Vector.ConditionalSelect(useFallback, Vector<float>.One, jacobianA.X);
+            jacobianA.Y = Vector.ConditionalSelect(useFallback, Vector<float>.Zero, jacobianA.Y);
+            jacobianA.Z = Vector.ConditionalSelect(useFallback, Vector<float>.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 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<float> 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 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<float> 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 static bool RequiresIncrementalSubstepUpdates => false;
+        [MethodImpl(MethodImplOptions.AggressiveInlining)]
+        public static void IncrementallyUpdateForSubstep(in Vector<float> dt, in BodyVelocityWide wsvA, in BodyVelocityWide wsvB, ref CenterDistanceLimitPrestepData prestepData) { }
+
+    }
+
+
+    /// <summary>
+    /// Handles the solve iterations of a bunch of distance servos.
+    /// </summary>
+    public class CenterDistanceLimitTypeProcessor : TwoBodyTypeProcessor<CenterDistanceLimitPrestepData, Vector<float>, CenterDistanceLimitFunctions, AccessOnlyLinear, AccessOnlyLinear, AccessOnlyLinear, AccessOnlyLinear>
+    {
+        public const int BatchTypeId = 55;
+    }
+}
diff --git a/BepuPhysics/Constraints/ConstraintChecker.cs b/BepuPhysics/Constraints/ConstraintChecker.cs
index 64c102a05..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
 {
@@ -60,7 +57,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 +66,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/Contact/ContactConvexCommon.cs b/BepuPhysics/Constraints/Contact/ContactConvexCommon.cs
index bb6776a28..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
 {
@@ -22,34 +18,34 @@ public struct MaterialPropertiesWide
 
     public interface IContactPrestep<TPrestep> where TPrestep : struct, IContactPrestep<TPrestep>
     {
-        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<TPrestep> : IContactPrestep<TPrestep> where TPrestep : struct, IConvexContactPrestep<TPrestep>
     {
-        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<TPrestep> : IConvexContactPrestep<TPrestep> where TPrestep : struct, ITwoBodyConvexContactPrestep<TPrestep>
     {
-        ref Vector3Wide GetOffsetB(ref TPrestep prestep);
+        static abstract ref Vector3Wide GetOffsetB(ref TPrestep prestep);
     }
 
     public interface IContactAccumulatedImpulses<TAccumulatedImpulses> where TAccumulatedImpulses : struct, IContactAccumulatedImpulses<TAccumulatedImpulses>
     {
-        int ContactCount { get; }
+        static abstract int ContactCount { get; }
     }
 
     public interface IConvexContactAccumulatedImpulses<TAccumulatedImpulses> : IContactAccumulatedImpulses<TAccumulatedImpulses> where TAccumulatedImpulses : struct, IConvexContactAccumulatedImpulses<TAccumulatedImpulses>
     {
-        ref Vector2Wide GetTangentFriction(ref TAccumulatedImpulses impulses);
-        ref Vector<float> GetTwistFriction(ref TAccumulatedImpulses impulses);
-        ref Vector<float> GetPenetrationImpulseForContact(ref TAccumulatedImpulses impulses, int index);
+        static abstract ref Vector2Wide GetTangentFriction(ref TAccumulatedImpulses impulses);
+        static abstract ref Vector<float> GetTwistFriction(ref TAccumulatedImpulses impulses);
+        static abstract ref Vector<float> GetPenetrationImpulseForContact(ref TAccumulatedImpulses impulses, int index);
     }
 
 }
diff --git a/BepuPhysics/Constraints/Contact/ContactConvexTypes.cs b/BepuPhysics/Constraints/Contact/ContactConvexTypes.cs
index f77bd5172..a858f6300 100644
--- a/BepuPhysics/Constraints/Contact/ContactConvexTypes.cs
+++ b/BepuPhysics/Constraints/Contact/ContactConvexTypes.cs
@@ -15,23 +15,23 @@ public struct Contact1AccumulatedImpulses : IConvexContactAccumulatedImpulses<Co
         public Vector<float> 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<float> GetTwistFriction(ref Contact1AccumulatedImpulses impulses)
+        public static ref Vector<float> GetTwistFriction(ref Contact1AccumulatedImpulses impulses)
         {
             return ref impulses.Twist;
         }
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public ref Vector<float> GetPenetrationImpulseForContact(ref Contact1AccumulatedImpulses impulses, int index)
+        public static ref Vector<float> 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<Contact2AccumulatedImpulses>
@@ -42,23 +42,23 @@ public struct Contact2AccumulatedImpulses : IConvexContactAccumulatedImpulses<Co
         public Vector<float> 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<float> GetTwistFriction(ref Contact2AccumulatedImpulses impulses)
+        public static ref Vector<float> GetTwistFriction(ref Contact2AccumulatedImpulses impulses)
         {
             return ref impulses.Twist;
         }
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public ref Vector<float> GetPenetrationImpulseForContact(ref Contact2AccumulatedImpulses impulses, int index)
+        public static ref Vector<float> 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<Contact3AccumulatedImpulses>
@@ -70,23 +70,23 @@ public struct Contact3AccumulatedImpulses : IConvexContactAccumulatedImpulses<Co
         public Vector<float> 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<float> GetTwistFriction(ref Contact3AccumulatedImpulses impulses)
+        public static ref Vector<float> GetTwistFriction(ref Contact3AccumulatedImpulses impulses)
         {
             return ref impulses.Twist;
         }
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public ref Vector<float> GetPenetrationImpulseForContact(ref Contact3AccumulatedImpulses impulses, int index)
+        public static ref Vector<float> 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<Contact4AccumulatedImpulses>
@@ -99,23 +99,23 @@ public struct Contact4AccumulatedImpulses : IConvexContactAccumulatedImpulses<Co
         public Vector<float> 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<float> GetTwistFriction(ref Contact4AccumulatedImpulses impulses)
+        public static ref Vector<float> GetTwistFriction(ref Contact4AccumulatedImpulses impulses)
         {
             return ref impulses.Twist;
         }
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public ref Vector<float> GetPenetrationImpulseForContact(ref Contact4AccumulatedImpulses impulses, int index)
+        public static ref Vector<float> 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<Contact1OneBodyPrestepData>.Get(ref batch.PrestepData, bundleIndex), innerIndex);
@@ -239,18 +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 static Type TypeProcessorType => typeof(Contact1OneBodyTypeProcessor);
+        public static TypeProcessor CreateTypeProcessor() => new Contact1OneBodyTypeProcessor();
 
     }
 
@@ -264,99 +265,74 @@ public struct Contact1OneBodyPrestepData : IConvexContactPrestep<Contact1OneBody
         public Vector3Wide Normal;
         public MaterialPropertiesWide MaterialProperties;
 		
-        public readonly int BodyCount => 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;
         }
 
     }
 
-    public unsafe struct Contact1OneBodyProjection
-    {
-        public BodyInertias InertiaA;
-        public Vector<float> PremultipliedFrictionCoefficient;
-        public Vector3Wide Normal;
-        public TangentFrictionOneBody.Projection Tangent;
-        public Vector<float> 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<float> LeverArm0;
-        public TwistFrictionProjection Twist;
-    }
 
-    public struct Contact1OneBodyFunctions : IOneBodyContactConstraintFunctions<Contact1OneBodyPrestepData, Contact1OneBodyProjection, Contact1AccumulatedImpulses>
-    {
-        [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public void Prestep(Bodies bodies, ref Vector<int> bodyReferences, int count,
-            float dt, float inverseDt, ref BodyInertias inertiaA, 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<float>.Zero, prestep.Contact0.Depth);
-            TwistFrictionOneBody.Prestep(ref projection.InertiaA, ref prestep.Normal, out projection.Twist);
-        }
+    public struct Contact1OneBodyFunctions : IOneBodyConstraintFunctions<Contact1OneBodyPrestepData, Contact1AccumulatedImpulses>
+    {       
+        public static bool RequiresIncrementalSubstepUpdates => true;
 
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public void WarmStart(ref BodyVelocities wsvA, ref Contact1OneBodyProjection projection, ref Contact1AccumulatedImpulses accumulatedImpulses)
+        public static void IncrementallyUpdateForSubstep(in Vector<float> dt, in BodyVelocityWide velocityA, ref Contact1OneBodyPrestepData prestep)
         {
-            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);
+            PenetrationLimitOneBody.UpdatePenetrationDepth(dt, prestep.Contact0.OffsetA, prestep.Normal, velocityA, ref prestep.Contact0.Depth);
         }
-
-        [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public void Solve(ref BodyVelocities wsvA, ref Contact1OneBodyProjection projection, ref Contact1AccumulatedImpulses accumulatedImpulses)
+               
+        [MethodImpl(MethodImplOptions.AggressiveInlining)] 
+        public static void WarmStart(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, ref Contact1OneBodyPrestepData prestep, ref Contact1AccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide 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);
+            Helpers.BuildOrthonormalBasis(prestep.Normal, out var x, out var z);
+            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 IncrementallyUpdateContactData(in Vector<float> dt, in BodyVelocities velocityA, ref Contact1OneBodyPrestepData prestep)
-        {
-            PenetrationLimitOneBody.UpdatePenetrationDepth(dt, prestep.Contact0.OffsetA, prestep.Normal, velocityA, ref prestep.Contact0.Depth);
-        }
+        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.
+            //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<float>(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<float>.Zero, prestep.Contact0.Depth);
+            TwistFrictionOneBody.Solve(prestep.Normal, inertiaA, maximumTwistImpulse, ref accumulatedImpulses.Twist, ref wsvA);
+        }               
     }
     
     /// <summary>
     /// Handles the solve iterations of a bunch of 1-contact one body manifold constraints.
     /// </summary>
     public class Contact1OneBodyTypeProcessor :
-        OneBodyContactTypeProcessor<Contact1OneBodyPrestepData, Contact1OneBodyProjection, Contact1AccumulatedImpulses, Contact1OneBodyFunctions>
+        OneBodyContactTypeProcessor<Contact1OneBodyPrestepData, Contact1AccumulatedImpulses, Contact1OneBodyFunctions>
     {
         //Matches UpdateConstraintForManifold's manifoldTypeAsConstraintType computation.
         public const int BatchTypeId = 0;
@@ -387,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<Contact2OneBodyPrestepData>.Get(ref batch.PrestepData, bundleIndex), innerIndex);
@@ -411,18 +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 static Type TypeProcessorType => typeof(Contact2OneBodyTypeProcessor);
+        public static TypeProcessor CreateTypeProcessor() => new Contact2OneBodyTypeProcessor();
 
     }
 
@@ -437,106 +414,80 @@ public struct Contact2OneBodyPrestepData : IConvexContactPrestep<Contact2OneBody
         public Vector3Wide Normal;
         public MaterialPropertiesWide MaterialProperties;
 		
-        public readonly int BodyCount => 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;
         }
 
     }
 
-    public unsafe struct Contact2OneBodyProjection
-    {
-        public BodyInertias InertiaA;
-        public Vector<float> PremultipliedFrictionCoefficient;
-        public Vector3Wide Normal;
-        public TangentFrictionOneBody.Projection Tangent;
-        public Vector<float> 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<float> LeverArm0;
-        public Vector<float> LeverArm1;
-        public TwistFrictionProjection Twist;
-    }
 
-    public struct Contact2OneBodyFunctions : IOneBodyContactConstraintFunctions<Contact2OneBodyPrestepData, Contact2OneBodyProjection, Contact2AccumulatedImpulses>
-    {
-        [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public void Prestep(Bodies bodies, ref Vector<int> bodyReferences, int count,
-            float dt, float inverseDt, ref BodyInertias inertiaA, 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);
-        }
+    public struct Contact2OneBodyFunctions : IOneBodyConstraintFunctions<Contact2OneBodyPrestepData, Contact2AccumulatedImpulses>
+    {       
+        public static bool RequiresIncrementalSubstepUpdates => true;
 
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public void WarmStart(ref BodyVelocities wsvA, ref Contact2OneBodyProjection projection, ref Contact2AccumulatedImpulses accumulatedImpulses)
+        public static void IncrementallyUpdateForSubstep(in Vector<float> dt, in BodyVelocityWide velocityA, ref Contact2OneBodyPrestepData prestep)
         {
-            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);
+            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 Solve(ref BodyVelocities wsvA, ref Contact2OneBodyProjection projection, ref Contact2AccumulatedImpulses accumulatedImpulses)
+               
+        [MethodImpl(MethodImplOptions.AggressiveInlining)] 
+        public static void WarmStart(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, ref Contact2OneBodyPrestepData prestep, ref Contact2AccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide 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);
+            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.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 IncrementallyUpdateContactData(in Vector<float> dt, in BodyVelocities 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);
-        }
+        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.
+            //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<float>(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<float>(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);
+            var maximumTwistImpulse = premultipliedFrictionCoefficient * (
+                accumulatedImpulses.Penetration0 * Vector3Wide.Distance(offsetToManifoldCenterA, prestep.Contact0.OffsetA) +
+                accumulatedImpulses.Penetration1 * Vector3Wide.Distance(offsetToManifoldCenterA, prestep.Contact1.OffsetA));
+            TwistFrictionOneBody.Solve(prestep.Normal, inertiaA, maximumTwistImpulse, ref accumulatedImpulses.Twist, ref wsvA);
+        }               
     }
     
     /// <summary>
     /// Handles the solve iterations of a bunch of 2-contact one body manifold constraints.
     /// </summary>
     public class Contact2OneBodyTypeProcessor :
-        OneBodyContactTypeProcessor<Contact2OneBodyPrestepData, Contact2OneBodyProjection, Contact2AccumulatedImpulses, Contact2OneBodyFunctions>
+        OneBodyContactTypeProcessor<Contact2OneBodyPrestepData, Contact2AccumulatedImpulses, Contact2OneBodyFunctions>
     {
         //Matches UpdateConstraintForManifold's manifoldTypeAsConstraintType computation.
         public const int BatchTypeId = 1;
@@ -570,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<Contact3OneBodyPrestepData>.Get(ref batch.PrestepData, bundleIndex), innerIndex);
@@ -596,18 +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 static Type TypeProcessorType => typeof(Contact3OneBodyTypeProcessor);
+        public static TypeProcessor CreateTypeProcessor() => new Contact3OneBodyTypeProcessor();
 
     }
 
@@ -623,114 +575,84 @@ public struct Contact3OneBodyPrestepData : IConvexContactPrestep<Contact3OneBody
         public Vector3Wide Normal;
         public MaterialPropertiesWide MaterialProperties;
 		
-        public readonly int BodyCount => 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;
         }
 
     }
 
-    public unsafe struct Contact3OneBodyProjection
-    {
-        public BodyInertias InertiaA;
-        public Vector<float> PremultipliedFrictionCoefficient;
-        public Vector3Wide Normal;
-        public TangentFrictionOneBody.Projection Tangent;
-        public Vector<float> 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<float> LeverArm0;
-        public Vector<float> LeverArm1;
-        public Vector<float> LeverArm2;
-        public TwistFrictionProjection Twist;
-    }
 
-    public struct Contact3OneBodyFunctions : IOneBodyContactConstraintFunctions<Contact3OneBodyPrestepData, Contact3OneBodyProjection, Contact3AccumulatedImpulses>
-    {
-        [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public void Prestep(Bodies bodies, ref Vector<int> bodyReferences, int count,
-            float dt, float inverseDt, ref BodyInertias inertiaA, 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 BodyVelocities 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 BodyVelocities 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<Contact3OneBodyPrestepData, Contact3AccumulatedImpulses>
+    {       
+        public static bool RequiresIncrementalSubstepUpdates => true;
 
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public void IncrementallyUpdateContactData(in Vector<float> dt, in BodyVelocities velocityA, ref Contact3OneBodyPrestepData prestep)
+        public static void IncrementallyUpdateForSubstep(in Vector<float> 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);
         }
+               
+        [MethodImpl(MethodImplOptions.AggressiveInlining)] 
+        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);
+            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 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.
+            //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<float>(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<float>(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) +
+                accumulatedImpulses.Penetration2 * Vector3Wide.Distance(offsetToManifoldCenterA, prestep.Contact2.OffsetA));
+            TwistFrictionOneBody.Solve(prestep.Normal, inertiaA, maximumTwistImpulse, ref accumulatedImpulses.Twist, ref wsvA);
+        }               
     }
     
     /// <summary>
     /// Handles the solve iterations of a bunch of 3-contact one body manifold constraints.
     /// </summary>
     public class Contact3OneBodyTypeProcessor :
-        OneBodyContactTypeProcessor<Contact3OneBodyPrestepData, Contact3OneBodyProjection, Contact3AccumulatedImpulses, Contact3OneBodyFunctions>
+        OneBodyContactTypeProcessor<Contact3OneBodyPrestepData, Contact3AccumulatedImpulses, Contact3OneBodyFunctions>
     {
         //Matches UpdateConstraintForManifold's manifoldTypeAsConstraintType computation.
         public const int BatchTypeId = 2;
@@ -767,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<Contact4OneBodyPrestepData>.Get(ref batch.PrestepData, bundleIndex), innerIndex);
@@ -795,18 +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 static Type TypeProcessorType => typeof(Contact4OneBodyTypeProcessor);
+        public static TypeProcessor CreateTypeProcessor() => new Contact4OneBodyTypeProcessor();
 
     }
 
@@ -823,122 +746,88 @@ public struct Contact4OneBodyPrestepData : IConvexContactPrestep<Contact4OneBody
         public Vector3Wide Normal;
         public MaterialPropertiesWide MaterialProperties;
 		
-        public readonly int BodyCount => 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;
         }
 
     }
 
-    public unsafe struct Contact4OneBodyProjection
-    {
-        public BodyInertias InertiaA;
-        public Vector<float> PremultipliedFrictionCoefficient;
-        public Vector3Wide Normal;
-        public TangentFrictionOneBody.Projection Tangent;
-        public Vector<float> 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<float> LeverArm0;
-        public Vector<float> LeverArm1;
-        public Vector<float> LeverArm2;
-        public Vector<float> LeverArm3;
-        public TwistFrictionProjection Twist;
-    }
-
-    public struct Contact4OneBodyFunctions : IOneBodyContactConstraintFunctions<Contact4OneBodyPrestepData, Contact4OneBodyProjection, Contact4AccumulatedImpulses>
-    {
-        [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public void Prestep(Bodies bodies, ref Vector<int> bodyReferences, int count,
-            float dt, float inverseDt, ref BodyInertias inertiaA, 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 BodyVelocities 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 BodyVelocities 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<Contact4OneBodyPrestepData, Contact4AccumulatedImpulses>
+    {       
+        public static bool RequiresIncrementalSubstepUpdates => true;
 
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public void IncrementallyUpdateContactData(in Vector<float> dt, in BodyVelocities velocityA, ref Contact4OneBodyPrestepData prestep)
+        public static void IncrementallyUpdateForSubstep(in Vector<float> 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);
         }
+               
+        [MethodImpl(MethodImplOptions.AggressiveInlining)] 
+        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);
+            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 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.
+            //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<float>(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<float>(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) +
+                accumulatedImpulses.Penetration2 * Vector3Wide.Distance(offsetToManifoldCenterA, prestep.Contact2.OffsetA) +
+                accumulatedImpulses.Penetration3 * Vector3Wide.Distance(offsetToManifoldCenterA, prestep.Contact3.OffsetA));
+            TwistFrictionOneBody.Solve(prestep.Normal, inertiaA, maximumTwistImpulse, ref accumulatedImpulses.Twist, ref wsvA);
+        }               
     }
     
     /// <summary>
     /// Handles the solve iterations of a bunch of 4-contact one body manifold constraints.
     /// </summary>
     public class Contact4OneBodyTypeProcessor :
-        OneBodyContactTypeProcessor<Contact4OneBodyPrestepData, Contact4OneBodyProjection, Contact4AccumulatedImpulses, Contact4OneBodyFunctions>
+        OneBodyContactTypeProcessor<Contact4OneBodyPrestepData, Contact4AccumulatedImpulses, Contact4OneBodyFunctions>
     {
         //Matches UpdateConstraintForManifold's manifoldTypeAsConstraintType computation.
         public const int BatchTypeId = 3;
@@ -968,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<Contact1PrestepData>.Get(ref batch.PrestepData, bundleIndex), innerIndex);
@@ -993,18 +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 static Type TypeProcessorType => typeof(Contact1TypeProcessor);
+        public static TypeProcessor CreateTypeProcessor() => new Contact1TypeProcessor();
 
     }
 
@@ -1019,109 +909,81 @@ public struct Contact1PrestepData : ITwoBodyConvexContactPrestep<Contact1Prestep
         public Vector3Wide Normal;
         public MaterialPropertiesWide MaterialProperties;
 		
-        public readonly int BodyCount => 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;
         }
     }
 
-    public unsafe struct Contact1Projection
-    {
-        public BodyInertias InertiaA;
-        public BodyInertias InertiaB;
-        public Vector<float> PremultipliedFrictionCoefficient;
-        public Vector3Wide Normal;
-        public TangentFriction.Projection Tangent;
-        public Vector<float> 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<float> LeverArm0;
-        public TwistFrictionProjection Twist;
-    }
 
-    public struct Contact1Functions : IContactConstraintFunctions<Contact1PrestepData, Contact1Projection, Contact1AccumulatedImpulses>
-    {
-        [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)
-        {
-            //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<float>.Zero, prestep.Contact0.Depth);
-            TwistFriction.Prestep(ref projection.InertiaA, ref projection.InertiaB, ref prestep.Normal, out projection.Twist);
-        }
+    public struct Contact1Functions : ITwoBodyConstraintFunctions<Contact1PrestepData, Contact1AccumulatedImpulses>
+    {       
+        public static bool RequiresIncrementalSubstepUpdates => true;
 
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public void WarmStart(ref BodyVelocities wsvA, ref BodyVelocities wsvB, ref Contact1Projection projection, ref Contact1AccumulatedImpulses accumulatedImpulses)
+        public static void IncrementallyUpdateForSubstep(in Vector<float> dt, in BodyVelocityWide velocityA, in BodyVelocityWide velocityB, ref Contact1PrestepData prestep)
         {
-            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);
+            PenetrationLimit.UpdatePenetrationDepth(dt, prestep.Contact0.OffsetA, prestep.OffsetB, prestep.Normal, velocityA, velocityB, ref prestep.Contact0.Depth);
         }
-
-        [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public void Solve(ref BodyVelocities wsvA, ref BodyVelocities wsvB, ref Contact1Projection projection, ref Contact1AccumulatedImpulses accumulatedImpulses)
+               
+        [MethodImpl(MethodImplOptions.AggressiveInlining)] 
+        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(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);
+            Helpers.BuildOrthonormalBasis(prestep.Normal, out var x, out var z);
+            Vector3Wide.Subtract(prestep.Contact0.OffsetA, prestep.OffsetB, out var offsetToManifoldCenterB);
+            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 IncrementallyUpdateContactData(in Vector<float> dt, in BodyVelocities velocityA, in BodyVelocities velocityB, ref Contact1PrestepData prestep)
-        {
-            PenetrationLimit.UpdatePenetrationDepth(dt, prestep.Contact0.OffsetA, prestep.OffsetB, prestep.Normal, velocityA, velocityB, ref prestep.Contact0.Depth);
-        }
+        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.
+            //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<float>(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);
+            //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<float>.Zero, prestep.Contact0.Depth);
+            TwistFriction.Solve(prestep.Normal, inertiaA, inertiaB, maximumTwistImpulse, ref accumulatedImpulses.Twist, ref wsvA, ref wsvB);
+        }               
     }
     
     /// <summary>
     /// Handles the solve iterations of a bunch of 1-contact two body manifold constraints.
     /// </summary>
     public class Contact1TypeProcessor :
-        TwoBodyContactTypeProcessor<Contact1PrestepData, Contact1Projection, Contact1AccumulatedImpulses, Contact1Functions>
+        TwoBodyContactTypeProcessor<Contact1PrestepData, Contact1AccumulatedImpulses, Contact1Functions>
     {
         //Matches UpdateConstraintForManifold's manifoldTypeAsConstraintType computation.
         public const int BatchTypeId = 4;
@@ -1154,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<Contact2PrestepData>.Get(ref batch.PrestepData, bundleIndex), innerIndex);
@@ -1181,18 +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 static Type TypeProcessorType => typeof(Contact2TypeProcessor);
+        public static TypeProcessor CreateTypeProcessor() => new Contact2TypeProcessor();
 
     }
 
@@ -1208,117 +1071,87 @@ public struct Contact2PrestepData : ITwoBodyConvexContactPrestep<Contact2Prestep
         public Vector3Wide Normal;
         public MaterialPropertiesWide MaterialProperties;
 		
-        public readonly int BodyCount => 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;
         }
     }
 
-    public unsafe struct Contact2Projection
-    {
-        public BodyInertias InertiaA;
-        public BodyInertias InertiaB;
-        public Vector<float> PremultipliedFrictionCoefficient;
-        public Vector3Wide Normal;
-        public TangentFriction.Projection Tangent;
-        public Vector<float> 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<float> LeverArm0;
-        public Vector<float> LeverArm1;
-        public TwistFrictionProjection Twist;
-    }
 
-    public struct Contact2Functions : IContactConstraintFunctions<Contact2PrestepData, Contact2Projection, Contact2AccumulatedImpulses>
-    {
-        [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)
-        {
-            //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);
-        }
+    public struct Contact2Functions : ITwoBodyConstraintFunctions<Contact2PrestepData, Contact2AccumulatedImpulses>
+    {       
+        public static bool RequiresIncrementalSubstepUpdates => true;
 
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public void WarmStart(ref BodyVelocities wsvA, ref BodyVelocities wsvB, ref Contact2Projection projection, ref Contact2AccumulatedImpulses accumulatedImpulses)
+        public static void IncrementallyUpdateForSubstep(in Vector<float> dt, in BodyVelocityWide velocityA, in BodyVelocityWide velocityB, ref Contact2PrestepData prestep)
         {
-            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);
+            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 Solve(ref BodyVelocities wsvA, ref BodyVelocities wsvB, ref Contact2Projection projection, ref Contact2AccumulatedImpulses accumulatedImpulses)
+               
+        [MethodImpl(MethodImplOptions.AggressiveInlining)] 
+        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(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);
+            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.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 IncrementallyUpdateContactData(in Vector<float> dt, in BodyVelocities velocityA, in BodyVelocities 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);
-        }
+        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.
+            //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<float>(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<float>(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);
+            var maximumTwistImpulse = premultipliedFrictionCoefficient * (
+                accumulatedImpulses.Penetration0 * Vector3Wide.Distance(offsetToManifoldCenterA, prestep.Contact0.OffsetA) +
+                accumulatedImpulses.Penetration1 * Vector3Wide.Distance(offsetToManifoldCenterA, prestep.Contact1.OffsetA));
+            TwistFriction.Solve(prestep.Normal, inertiaA, inertiaB, maximumTwistImpulse, ref accumulatedImpulses.Twist, ref wsvA, ref wsvB);
+        }               
     }
     
     /// <summary>
     /// Handles the solve iterations of a bunch of 2-contact two body manifold constraints.
     /// </summary>
     public class Contact2TypeProcessor :
-        TwoBodyContactTypeProcessor<Contact2PrestepData, Contact2Projection, Contact2AccumulatedImpulses, Contact2Functions>
+        TwoBodyContactTypeProcessor<Contact2PrestepData, Contact2AccumulatedImpulses, Contact2Functions>
     {
         //Matches UpdateConstraintForManifold's manifoldTypeAsConstraintType computation.
         public const int BatchTypeId = 5;
@@ -1354,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<Contact3PrestepData>.Get(ref batch.PrestepData, bundleIndex), innerIndex);
@@ -1383,18 +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 static Type TypeProcessorType => typeof(Contact3TypeProcessor);
+        public static TypeProcessor CreateTypeProcessor() => new Contact3TypeProcessor();
 
     }
 
@@ -1411,126 +1245,91 @@ public struct Contact3PrestepData : ITwoBodyConvexContactPrestep<Contact3Prestep
         public Vector3Wide Normal;
         public MaterialPropertiesWide MaterialProperties;
 		
-        public readonly int BodyCount => 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;
         }
     }
 
-    public unsafe struct Contact3Projection
-    {
-        public BodyInertias InertiaA;
-        public BodyInertias InertiaB;
-        public Vector<float> PremultipliedFrictionCoefficient;
-        public Vector3Wide Normal;
-        public TangentFriction.Projection Tangent;
-        public Vector<float> 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<float> LeverArm0;
-        public Vector<float> LeverArm1;
-        public Vector<float> LeverArm2;
-        public TwistFrictionProjection Twist;
-    }
-
-    public struct Contact3Functions : IContactConstraintFunctions<Contact3PrestepData, Contact3Projection, Contact3AccumulatedImpulses>
-    {
-        [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)
-        {
-            //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 BodyVelocities wsvA, ref BodyVelocities 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 BodyVelocities wsvA, ref BodyVelocities 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<Contact3PrestepData, Contact3AccumulatedImpulses>
+    {       
+        public static bool RequiresIncrementalSubstepUpdates => true;
 
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public void IncrementallyUpdateContactData(in Vector<float> dt, in BodyVelocities velocityA, in BodyVelocities velocityB, ref Contact3PrestepData prestep)
+        public static void IncrementallyUpdateForSubstep(in Vector<float> 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);
         }
+               
+        [MethodImpl(MethodImplOptions.AggressiveInlining)] 
+        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);
+            Vector3Wide.Subtract(offsetToManifoldCenterA, prestep.OffsetB, out var offsetToManifoldCenterB);
+            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 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.
+            //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<float>(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<float>(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);
+            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.Solve(prestep.Normal, inertiaA, inertiaB, maximumTwistImpulse, ref accumulatedImpulses.Twist, ref wsvA, ref wsvB);
+        }               
     }
     
     /// <summary>
     /// Handles the solve iterations of a bunch of 3-contact two body manifold constraints.
     /// </summary>
     public class Contact3TypeProcessor :
-        TwoBodyContactTypeProcessor<Contact3PrestepData, Contact3Projection, Contact3AccumulatedImpulses, Contact3Functions>
+        TwoBodyContactTypeProcessor<Contact3PrestepData, Contact3AccumulatedImpulses, Contact3Functions>
     {
         //Matches UpdateConstraintForManifold's manifoldTypeAsConstraintType computation.
         public const int BatchTypeId = 6;
@@ -1569,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<Contact4PrestepData>.Get(ref batch.PrestepData, bundleIndex), innerIndex);
@@ -1600,18 +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 static Type TypeProcessorType => typeof(Contact4TypeProcessor);
+        public static TypeProcessor CreateTypeProcessor() => new Contact4TypeProcessor();
 
     }
 
@@ -1629,135 +1429,95 @@ public struct Contact4PrestepData : ITwoBodyConvexContactPrestep<Contact4Prestep
         public Vector3Wide Normal;
         public MaterialPropertiesWide MaterialProperties;
 		
-        public readonly int BodyCount => 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;
         }
     }
 
-    public unsafe struct Contact4Projection
-    {
-        public BodyInertias InertiaA;
-        public BodyInertias InertiaB;
-        public Vector<float> PremultipliedFrictionCoefficient;
-        public Vector3Wide Normal;
-        public TangentFriction.Projection Tangent;
-        public Vector<float> 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<float> LeverArm0;
-        public Vector<float> LeverArm1;
-        public Vector<float> LeverArm2;
-        public Vector<float> LeverArm3;
-        public TwistFrictionProjection Twist;
-    }
 
-    public struct Contact4Functions : IContactConstraintFunctions<Contact4PrestepData, Contact4Projection, Contact4AccumulatedImpulses>
-    {
+    public struct Contact4Functions : ITwoBodyConstraintFunctions<Contact4PrestepData, Contact4AccumulatedImpulses>
+    {       
+        public static bool RequiresIncrementalSubstepUpdates => true;
+
         [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)
-        {
-            //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 BodyVelocities wsvA, ref BodyVelocities 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 BodyVelocities wsvA, ref BodyVelocities 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);
-        }
-
-        [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public void IncrementallyUpdateContactData(in Vector<float> dt, in BodyVelocities velocityA, in BodyVelocities velocityB, ref Contact4PrestepData prestep)
+        public static void IncrementallyUpdateForSubstep(in Vector<float> 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);
         }
+               
+        [MethodImpl(MethodImplOptions.AggressiveInlining)] 
+        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);
+            Vector3Wide.Subtract(offsetToManifoldCenterA, prestep.OffsetB, out var offsetToManifoldCenterB);
+            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 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.
+            //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<float>(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<float>(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) +
+                accumulatedImpulses.Penetration2 * Vector3Wide.Distance(offsetToManifoldCenterA, prestep.Contact2.OffsetA) +
+                accumulatedImpulses.Penetration3 * Vector3Wide.Distance(offsetToManifoldCenterA, prestep.Contact3.OffsetA));
+            TwistFriction.Solve(prestep.Normal, inertiaA, inertiaB, maximumTwistImpulse, ref accumulatedImpulses.Twist, ref wsvA, ref wsvB);
+        }               
     }
     
     /// <summary>
     /// Handles the solve iterations of a bunch of 4-contact two body manifold constraints.
     /// </summary>
     public class Contact4TypeProcessor :
-        TwoBodyContactTypeProcessor<Contact4PrestepData, Contact4Projection, Contact4AccumulatedImpulses, Contact4Functions>
+        TwoBodyContactTypeProcessor<Contact4PrestepData, Contact4AccumulatedImpulses, Contact4Functions>
     {
         //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 90a97577e..243e2215d 100644
--- a/BepuPhysics/Constraints/Contact/ContactConvexTypes.tt
+++ b/BepuPhysics/Constraints/Contact/ContactConvexTypes.tt
@@ -24,23 +24,23 @@ namespace BepuPhysics.Constraints.Contact
         public Vector<float> 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<float> GetTwistFriction(ref Contact<#= contactCount #>AccumulatedImpulses impulses)
+        public static ref Vector<float> GetTwistFriction(ref Contact<#= contactCount #>AccumulatedImpulses impulses)
         {
             return ref impulses.Twist;
         }
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public ref Vector<float> GetPenetrationImpulseForContact(ref Contact<#= contactCount #>AccumulatedImpulses impulses, int index)
+        public static ref Vector<float> 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<Contact<#=contactCount#><#=suffix#>PrestepData>.Get(ref batch.PrestepData, bundleIndex), innerIndex);
@@ -155,18 +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 static Type TypeProcessorType => typeof(Contact<#= contactCount #><#=suffix#>TypeProcessor);
+        public static TypeProcessor CreateTypeProcessor() => new Contact<#= contactCount #><#=suffix#>TypeProcessor();
 
     }
 
@@ -185,67 +186,53 @@ 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;
         }
 <#}#>
     }
 
-    public unsafe struct Contact<#= contactCount #><#=suffix#>Projection
-    {
-        public BodyInertias InertiaA;
-<# if (bodyCount == 2) { #>
-        public BodyInertias InertiaB;
-<#}#>
-        public Vector<float> PremultipliedFrictionCoefficient;
-        public Vector3Wide Normal;
-        public TangentFriction<#=suffix#>.Projection Tangent;
-        public Vector<float> 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<float> LeverArm<#=i#>;
-<#}#>
-        public TwistFrictionProjection Twist;
-    }
 
-    public struct Contact<#=contactCount#><#=suffix#>Functions : I<#=suffix#>ContactConstraintFunctions<Contact<#=contactCount#><#=suffix#>PrestepData, Contact<#=contactCount#><#=suffix#>Projection, Contact<#=contactCount#>AccumulatedImpulses>
-    {
+    public struct Contact<#=contactCount#><#=suffix#>Functions : I<#=bodyCount == 1 ? "OneBody" : "TwoBody"#>ConstraintFunctions<Contact<#=contactCount#><#=suffix#>PrestepData, Contact<#=contactCount#>AccumulatedImpulses>
+    {       
+        public static bool RequiresIncrementalSubstepUpdates => true;
+
         [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 static void IncrementallyUpdateForSubstep(in Vector<float> dt, in BodyVelocityWide velocityA,<#if(bodyCount == 2) {#> in BodyVelocityWide velocityB,<#}#> ref Contact<#=contactCount#><#=suffix#>PrestepData prestep)
         {
-            //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;
+<#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 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) {#>
             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) {#>
@@ -254,78 +241,58 @@ for (int i = 0; i < contactCount; ++i)
 <#} 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;
+            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#>.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#>.WarmStart(prestep.Normal, inertiaA, <#if(bodyCount == 2) {#>inertiaB, <#}#>accumulatedImpulses.Twist, ref wsvA<#if(bodyCount == 2) {#>, ref wsvB<#}#>);
+        }
+        
+        [MethodImpl(MethodImplOptions.AggressiveInlining)]
+        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.
+            //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<float>(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<float>(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(prestep.Contact<#=i#>.OffsetA, prestep.OffsetB, out contactOffsetB);
+            Vector3Wide.Subtract(offsetToManifoldCenterA, prestep.OffsetB, out var offsetToManifoldCenterB);
 <#}#>
-            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#>);
+<#} else if(bodyCount == 2) {#>
+            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<#}#>);
 <#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<float>.Zero, prestep.Contact0.Depth);
+            var maximumTwistImpulse = <#=contactCount > 1 ? "premultipliedFrictionCoefficient" : "prestep.MaterialProperties.FrictionCoefficient"#> * accumulatedImpulses.Penetration0 * Vector.Max(Vector<float>.Zero, prestep.Contact0.Depth);
 <#} else {#>
+            var maximumTwistImpulse = <#=contactCount > 1 ? "premultipliedFrictionCoefficient" : "prestep.MaterialProperties.FrictionCoefficient"#> * (
 <#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 BodyVelocities wsvA, <#if(bodyCount == 2) {#>ref BodyVelocities 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<#}#>);
+                accumulatedImpulses.Penetration<#=i#> * Vector3Wide.Distance(offsetToManifoldCenterA, prestep.Contact<#=i#>.OffsetA)<#=i == contactCount - 1 ? ");" : " +"#>
 <#}#>
-            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 BodyVelocities wsvA, <#if(bodyCount == 2) {#>ref BodyVelocities 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<#}#>);
-        }
-
-        [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public void IncrementallyUpdateContactData(in Vector<float> dt, in BodyVelocities velocityA,<#if(bodyCount == 2) {#> in BodyVelocities 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);
-<#}#>
-        }
+            TwistFriction<#=suffix#>.Solve(prestep.Normal, inertiaA, <#if (bodyCount == 2) {#>inertiaB, <#}#>maximumTwistImpulse, ref accumulatedImpulses.Twist, ref wsvA<#if (bodyCount == 2) {#>, ref wsvB<#}#>);
+        }               
     }
     
     /// <summary>
     /// Handles the solve iterations of a bunch of <#= contactCount #>-contact <#Write(bodyCount == 1 ? "one" : "two");#> body manifold constraints.
     /// </summary>
     public class Contact<#= contactCount #><#=suffix#>TypeProcessor :
-        <#Write(bodyCount == 2 ? "Two" : "One");#>BodyContactTypeProcessor<Contact<#= contactCount #><#=suffix#>PrestepData, Contact<#= contactCount #><#=suffix#>Projection, Contact<#= contactCount #>AccumulatedImpulses, Contact<#= contactCount #><#=suffix#>Functions>
+        <#Write(bodyCount == 2 ? "Two" : "One");#>BodyContactTypeProcessor<Contact<#= contactCount #><#=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 9cbd0dea1..1387be063 100644
--- a/BepuPhysics/Constraints/Contact/ContactNonconvexCommon.cs
+++ b/BepuPhysics/Constraints/Contact/ContactNonconvexCommon.cs
@@ -17,19 +17,19 @@ public struct NonconvexContactPrestepData
 
     public interface INonconvexContactPrestep<TPrestep> : IContactPrestep<TPrestep> where TPrestep : struct, INonconvexContactPrestep<TPrestep>
     {
-        ref NonconvexContactPrestepData GetContact(ref TPrestep prestep, int index);
+        static abstract ref NonconvexContactPrestepData GetContact(ref TPrestep prestep, int index);
 
     }
 
     public interface ITwoBodyNonconvexContactPrestep<TPrestep> : INonconvexContactPrestep<TPrestep> where TPrestep : struct, ITwoBodyNonconvexContactPrestep<TPrestep>
     {
-        ref Vector3Wide GetOffsetB(ref TPrestep prestep);
+        static abstract ref Vector3Wide GetOffsetB(ref TPrestep prestep);
     }
 
 
     public interface INonconvexContactAccumulatedImpulses<TAccumulatedImpulses> : IContactAccumulatedImpulses<TAccumulatedImpulses> where TAccumulatedImpulses : struct, INonconvexContactAccumulatedImpulses<TAccumulatedImpulses>
     {
-        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<TDescription, TPrestep>(ref TDescript
               where TPrestep : unmanaged, ITwoBodyNonconvexContactPrestep<TPrestep>
               where TDescription : unmanaged, INonconvexTwoBodyContactConstraintDescription<TDescription>
         {
-            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<TPrestep>.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<TDescription, TPrestep>(ref TDescript
               where TPrestep : unmanaged, INonconvexContactPrestep<TPrestep>
               where TDescription : unmanaged, INonconvexOneBodyContactConstraintDescription<TDescription>
         {
-            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<TPrestep>.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<TDescription, TPrestep>(ref TypeBatch
               where TPrestep : unmanaged, ITwoBodyNonconvexContactPrestep<TPrestep>
               where TDescription : unmanaged, INonconvexTwoBodyContactConstraintDescription<TDescription>
         {
-            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<TPrestep>.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<TDescription, TPrestep>(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<TDescription, TPrestep>(ref TypeBatch
               where TPrestep : unmanaged, INonconvexContactPrestep<TPrestep>
               where TDescription : unmanaged, INonconvexOneBodyContactConstraintDescription<TDescription>
         {
-            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<TPrestep>.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,229 +156,145 @@ public static void BuildOneBodyDescription<TDescription, TPrestep>(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);
         }
     }
 
-
     public struct NonconvexAccumulatedImpulses
     {
         public Vector2Wide Tangent;
         public Vector<float> Penetration;
     }
 
-    public struct NonconvexOneBodyProjectionCommon
-    {
-        public BodyInertias InertiaA;
-        public Vector<float> FrictionCoefficient;
-        public Vector<float> SoftnessImpulseScale;
-    }
-    public struct NonconvexTwoBodyProjectionCommon
-    {
-        public BodyInertias InertiaA;
-        public BodyInertias InertiaB;
-        public Vector<float> FrictionCoefficient;
-        public Vector<float> 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<TProjection> where TProjection : INonconvexOneBodyProjection<TProjection>
-    {
-        ref ContactNonconvexOneBodyProjection GetFirstContact(ref TProjection description);
-        int ContactCount { get; }
-
-        ref NonconvexOneBodyProjectionCommon GetCommonProperties(ref TProjection projection);
-    }
-    public interface INonconvexTwoBodyProjection<TProjection> where TProjection : INonconvexTwoBodyProjection<TProjection>
-    {
-        ref ContactNonconvexTwoBodyProjection GetFirstContact(ref TProjection description);
-        int ContactCount { get; }
-
-        ref NonconvexTwoBodyProjectionCommon GetCommonProperties(ref TProjection projection);
-    }
-
-    public struct ContactNonconvexOneBodyFunctions<TPrestep, TProjection, TAccumulatedImpulses> :
-        IOneBodyContactConstraintFunctions<TPrestep, TProjection, TAccumulatedImpulses>
+    public struct ContactNonconvexOneBodyFunctions<TPrestep, TAccumulatedImpulses> :
+        IOneBodyConstraintFunctions<TPrestep, TAccumulatedImpulses>
         where TPrestep : struct, INonconvexContactPrestep<TPrestep>
-        where TProjection : struct, INonconvexOneBodyProjection<TProjection>
         where TAccumulatedImpulses : struct
     {
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public void Prestep(Bodies bodies, ref Vector<int> bodyReferences, int count,
-            float dt, float inverseDt, ref BodyInertias inertia, ref TPrestep prestep, out TProjection projection)
+        public void IncrementallyUpdateContactData(in Vector<float> dt, in BodyVelocityWide velocity, ref TPrestep prestep)
         {
-            //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 prestepContactStart = ref TPrestep.GetContact(ref prestep, 0);
+            for (int i = 0; i < TPrestep.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);
+                PenetrationLimitOneBody.UpdatePenetrationDepth(dt, prestepContact.Offset, prestepContact.Normal, velocity, ref prestepContact.Depth);
             }
         }
 
-        [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public unsafe void WarmStart(ref BodyVelocities wsvA, ref TProjection projection, ref TAccumulatedImpulses accumulatedImpulses)
+        public static void WarmStart(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, 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 common = ref projection.GetCommonProperties(ref projection);
-            ref var contactStart = ref projection.GetFirstContact(ref projection);
+            ref var prestepMaterial = ref TPrestep.GetMaterialProperties(ref prestep);
+            ref var prestepContactStart = ref TPrestep.GetContact(ref prestep, 0);
             ref var accumulatedImpulsesStart = ref Unsafe.As<TAccumulatedImpulses, NonconvexAccumulatedImpulses>(ref accumulatedImpulses);
-            for (int i = 0; i < projection.ContactCount; ++i)
+            for (int i = 0; i < TPrestep.ContactCount; ++i)
             {
-                ref var contact = ref Unsafe.Add(ref contactStart, 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);
-                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);
+                TangentFrictionOneBody.WarmStart(x, z, prestepContact.Offset, inertiaA, contactImpulse.Tangent, ref wsvA);
+                PenetrationLimitOneBody.WarmStart(inertiaA, prestepContact.Normal, prestepContact.Offset, contactImpulse.Penetration, ref wsvA);
             }
         }
 
-        [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public void Solve(ref BodyVelocities wsvA, ref TProjection projection, ref TAccumulatedImpulses accumulatedImpulses)
+        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 common = ref projection.GetCommonProperties(ref projection);
-            ref var contactStart = ref projection.GetFirstContact(ref projection);
+            ref var prestepMaterial = ref TPrestep.GetMaterialProperties(ref prestep);
             ref var accumulatedImpulsesStart = ref Unsafe.As<TAccumulatedImpulses, NonconvexAccumulatedImpulses>(ref accumulatedImpulses);
-            for (int i = 0; i < projection.ContactCount; ++i)
+            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<float>(inverseDt);
+            for (int i = 0; i < TPrestep.ContactCount; ++i)
             {
-                ref var contact = ref Unsafe.Add(ref contactStart, i);
+                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 = 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);
+                var maximumTangentImpulse = prestepMaterial.FrictionCoefficient * contactImpulse.Penetration;
+                TangentFrictionOneBody.Solve(x, z, contact.Offset, inertiaA, maximumTangentImpulse, ref contactImpulse.Tangent, ref wsvA);
             }
         }
 
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public void IncrementallyUpdateContactData(in Vector<float> dt, in BodyVelocities velocity, ref TPrestep prestep)
+        public void UpdateForNewPose(
+            in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in BodyVelocityWide wsvA,
+            in Vector<float> dt, in TAccumulatedImpulses accumulatedImpulses, ref TPrestep prestep)
+        {
+            throw new System.NotImplementedException();
+        }
+
+        public static bool RequiresIncrementalSubstepUpdates => true;
+        public static void IncrementallyUpdateForSubstep(in Vector<float> 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, velocity, ref prestepContact.Depth);
+                PenetrationLimitOneBody.UpdatePenetrationDepth(dt, prestepContact.Offset, prestepContact.Normal, wsvA, ref prestepContact.Depth);
             }
         }
     }
 
-    public struct ContactNonconvexTwoBodyFunctions<TPrestep, TProjection, TAccumulatedImpulses> :
-        IContactConstraintFunctions<TPrestep, TProjection, TAccumulatedImpulses>
+    public struct ContactNonconvexTwoBodyFunctions<TPrestep, TAccumulatedImpulses> :
+        ITwoBodyConstraintFunctions<TPrestep, TAccumulatedImpulses>
         where TPrestep : struct, ITwoBodyNonconvexContactPrestep<TPrestep>
-        where TProjection : struct, INonconvexTwoBodyProjection<TProjection>
         where TAccumulatedImpulses : struct
     {
-        [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public void Prestep(Bodies bodies, ref TwoBodyReferences bodyReferences, int count,
-            float dt, float inverseDt, ref BodyInertias inertiaA, ref BodyInertias inertiaB, ref TPrestep prestep, out TProjection projection)
+        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)
         {
-            //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 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<TAccumulatedImpulses, NonconvexAccumulatedImpulses>(ref accumulatedImpulses);
+            for (int i = 0; i < TPrestep.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 BodyVelocities wsvA, ref BodyVelocities 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<TAccumulatedImpulses, NonconvexAccumulatedImpulses>(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);
+                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);
             }
         }
 
-        [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public void Solve(ref BodyVelocities wsvA, ref BodyVelocities wsvB, ref TProjection projection, ref TAccumulatedImpulses accumulatedImpulses)
+        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 common = ref projection.GetCommonProperties(ref projection);
-            ref var contactStart = ref projection.GetFirstContact(ref projection);
+            ref var prestepOffsetB = ref TPrestep.GetOffsetB(ref prestep);
+            ref var prestepMaterial = ref TPrestep.GetMaterialProperties(ref prestep);
             ref var accumulatedImpulsesStart = ref Unsafe.As<TAccumulatedImpulses, NonconvexAccumulatedImpulses>(ref accumulatedImpulses);
-            for (int i = 0; i < projection.ContactCount; ++i)
+            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<float>(inverseDt);
+            for (int i = 0; i < TPrestep.ContactCount; ++i)
             {
-                ref var contact = ref Unsafe.Add(ref contactStart, 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);
+                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 = 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);
+                var maximumTangentImpulse = prestepMaterial.FrictionCoefficient * contactImpulse.Penetration;
+                TangentFriction.Solve(x, z, contact.Offset, contactOffsetB, inertiaA, inertiaB, maximumTangentImpulse, ref contactImpulse.Tangent, ref wsvA, ref wsvB);
             }
         }
 
-        [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public void IncrementallyUpdateContactData(in Vector<float> dt, in BodyVelocities velocityA, in BodyVelocities velocityB, ref TPrestep prestep)
+
+        public static bool RequiresIncrementalSubstepUpdates => true;
+        public static void IncrementallyUpdateForSubstep(in Vector<float> 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, velocityA, velocityB, ref prestepContact.Depth);
+                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 a206af275..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<Contact2Nonconvex, Contact2NonconvexPrestepData>(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<Contact2Nonconvex, Contact2NonconvexPrestepData>(ref batch, bundleIndex, innerIndex, out description);
         }
@@ -31,26 +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 static Type TypeProcessorType => typeof(Contact2NonconvexTypeProcessor);
+        public static TypeProcessor CreateTypeProcessor() => new Contact2NonconvexTypeProcessor();
 
     }
 
@@ -63,67 +64,46 @@ public struct Contact2NonconvexPrestepData : ITwoBodyNonconvexContactPrestep<Con
         public NonconvexContactPrestepData Contact1;
 
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public ref MaterialPropertiesWide GetMaterialProperties(ref Contact2NonconvexPrestepData prestep)
+        public static ref MaterialPropertiesWide GetMaterialProperties(ref Contact2NonconvexPrestepData prestep)
         {
             return ref prestep.MaterialProperties;
         }
 
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public ref Vector3Wide GetOffsetB(ref Contact2NonconvexPrestepData prestep)
+        public static ref Vector3Wide GetOffsetB(ref Contact2NonconvexPrestepData prestep)
         {
             return ref prestep.OffsetB;
         }
 
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public ref NonconvexContactPrestepData GetContact(ref Contact2NonconvexPrestepData prestep, int index)
+        public static ref NonconvexContactPrestepData GetContact(ref Contact2NonconvexPrestepData prestep, int index)
         {
             return ref Unsafe.Add(ref prestep.Contact0, index);
         }
         
-        public readonly int ContactCount => 2;
-        public readonly int BodyCount => 2;
+        public static int ContactCount => 2;
+        public static int BodyCount => 2;
     }
 
     public struct Contact2NonconvexAccumulatedImpulses : INonconvexContactAccumulatedImpulses<Contact2NonconvexAccumulatedImpulses>
     {
         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);
         }
     }
-
-    public unsafe struct Contact2NonconvexProjection : INonconvexTwoBodyProjection<Contact2NonconvexProjection>
-    {
-        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;
-    }
-    
+        
     /// <summary>
     /// Handles the solve iterations of a bunch of 2-contact nonconvex two body manifold constraints.
     /// </summary>
     public class Contact2NonconvexTypeProcessor :
-        TwoBodyContactTypeProcessor<Contact2NonconvexPrestepData, Contact2NonconvexProjection, Contact2NonconvexAccumulatedImpulses,
-            ContactNonconvexTwoBodyFunctions<Contact2NonconvexPrestepData, Contact2NonconvexProjection, Contact2NonconvexAccumulatedImpulses>>
+        TwoBodyContactTypeProcessor<Contact2NonconvexPrestepData, Contact2NonconvexAccumulatedImpulses,
+            ContactNonconvexTwoBodyFunctions<Contact2NonconvexPrestepData, Contact2NonconvexAccumulatedImpulses>>
     {
         //Matches UpdateConstraintForManifold's manifoldTypeAsConstraintType computation.
         public const int BatchTypeId = 15;
@@ -140,7 +120,7 @@ public void ApplyDescription(ref TypeBatch batch, int bundleIndex, int innerInde
             NonconvexConstraintHelpers.ApplyOneBodyDescription<Contact2NonconvexOneBody, Contact2NonconvexOneBodyPrestepData>(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<Contact2NonconvexOneBody, Contact2NonconvexOneBodyPrestepData>(ref batch, bundleIndex, innerIndex, out description);
         }
@@ -154,26 +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 static Type TypeProcessorType => typeof(Contact2NonconvexOneBodyTypeProcessor);
+        public static TypeProcessor CreateTypeProcessor() => new Contact2NonconvexOneBodyTypeProcessor();
 
     }
 
@@ -185,48 +166,27 @@ public struct Contact2NonconvexOneBodyPrestepData : INonconvexContactPrestep<Con
         public NonconvexContactPrestepData Contact1;
 
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public ref MaterialPropertiesWide GetMaterialProperties(ref Contact2NonconvexOneBodyPrestepData prestep)
+        public static ref MaterialPropertiesWide GetMaterialProperties(ref Contact2NonconvexOneBodyPrestepData prestep)
         {
             return ref prestep.MaterialProperties;
         }
 
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public ref NonconvexContactPrestepData GetContact(ref Contact2NonconvexOneBodyPrestepData prestep, int index)
+        public static ref NonconvexContactPrestepData GetContact(ref Contact2NonconvexOneBodyPrestepData prestep, int index)
         {
             return ref Unsafe.Add(ref prestep.Contact0, index);
         }
                 
-        public readonly int ContactCount => 2;
-        public readonly int BodyCount => 1;
-    }
-
-    public unsafe struct Contact2NonconvexOneBodyProjection : INonconvexOneBodyProjection<Contact2NonconvexOneBodyProjection>
-    {
-        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;
-    }
+        public static int ContactCount => 2;
+        public static int BodyCount => 1;
+    }    
 
     /// <summary>
     /// Handles the solve iterations of a bunch of 2-contact nonconvex one body manifold constraints.
     /// </summary>
     public class Contact2NonconvexOneBodyTypeProcessor :
-        OneBodyContactTypeProcessor<Contact2NonconvexOneBodyPrestepData, Contact2NonconvexOneBodyProjection, Contact2NonconvexAccumulatedImpulses,
-            ContactNonconvexOneBodyFunctions<Contact2NonconvexOneBodyPrestepData, Contact2NonconvexOneBodyProjection, Contact2NonconvexAccumulatedImpulses>>
+        OneBodyContactTypeProcessor<Contact2NonconvexOneBodyPrestepData, Contact2NonconvexAccumulatedImpulses,
+            ContactNonconvexOneBodyFunctions<Contact2NonconvexOneBodyPrestepData, Contact2NonconvexAccumulatedImpulses>>
     {
         //Matches UpdateConstraintForManifold's manifoldTypeAsConstraintType computation.
         public const int BatchTypeId = 8;
@@ -245,7 +205,7 @@ public void ApplyDescription(ref TypeBatch batch, int bundleIndex, int innerInde
             NonconvexConstraintHelpers.ApplyTwoBodyDescription<Contact3Nonconvex, Contact3NonconvexPrestepData>(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<Contact3Nonconvex, Contact3NonconvexPrestepData>(ref batch, bundleIndex, innerIndex, out description);
         }
@@ -260,26 +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 static Type TypeProcessorType => typeof(Contact3NonconvexTypeProcessor);
+        public static TypeProcessor CreateTypeProcessor() => new Contact3NonconvexTypeProcessor();
 
     }
 
@@ -293,25 +254,25 @@ public struct Contact3NonconvexPrestepData : ITwoBodyNonconvexContactPrestep<Con
         public NonconvexContactPrestepData Contact2;
 
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public ref MaterialPropertiesWide GetMaterialProperties(ref Contact3NonconvexPrestepData prestep)
+        public static ref MaterialPropertiesWide GetMaterialProperties(ref Contact3NonconvexPrestepData prestep)
         {
             return ref prestep.MaterialProperties;
         }
 
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public ref Vector3Wide GetOffsetB(ref Contact3NonconvexPrestepData prestep)
+        public static ref Vector3Wide GetOffsetB(ref Contact3NonconvexPrestepData prestep)
         {
             return ref prestep.OffsetB;
         }
 
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public ref NonconvexContactPrestepData GetContact(ref Contact3NonconvexPrestepData prestep, int index)
+        public static ref NonconvexContactPrestepData GetContact(ref Contact3NonconvexPrestepData prestep, int index)
         {
             return ref Unsafe.Add(ref prestep.Contact0, index);
         }
         
-        public readonly int ContactCount => 3;
-        public readonly int BodyCount => 2;
+        public static int ContactCount => 3;
+        public static int BodyCount => 2;
     }
 
     public struct Contact3NonconvexAccumulatedImpulses : INonconvexContactAccumulatedImpulses<Contact3NonconvexAccumulatedImpulses>
@@ -319,43 +280,21 @@ 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);
         }
     }
-
-    public unsafe struct Contact3NonconvexProjection : INonconvexTwoBodyProjection<Contact3NonconvexProjection>
-    {
-        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;
-    }
-    
+        
     /// <summary>
     /// Handles the solve iterations of a bunch of 3-contact nonconvex two body manifold constraints.
     /// </summary>
     public class Contact3NonconvexTypeProcessor :
-        TwoBodyContactTypeProcessor<Contact3NonconvexPrestepData, Contact3NonconvexProjection, Contact3NonconvexAccumulatedImpulses,
-            ContactNonconvexTwoBodyFunctions<Contact3NonconvexPrestepData, Contact3NonconvexProjection, Contact3NonconvexAccumulatedImpulses>>
+        TwoBodyContactTypeProcessor<Contact3NonconvexPrestepData, Contact3NonconvexAccumulatedImpulses,
+            ContactNonconvexTwoBodyFunctions<Contact3NonconvexPrestepData, Contact3NonconvexAccumulatedImpulses>>
     {
         //Matches UpdateConstraintForManifold's manifoldTypeAsConstraintType computation.
         public const int BatchTypeId = 16;
@@ -373,7 +312,7 @@ public void ApplyDescription(ref TypeBatch batch, int bundleIndex, int innerInde
             NonconvexConstraintHelpers.ApplyOneBodyDescription<Contact3NonconvexOneBody, Contact3NonconvexOneBodyPrestepData>(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<Contact3NonconvexOneBody, Contact3NonconvexOneBodyPrestepData>(ref batch, bundleIndex, innerIndex, out description);
         }
@@ -387,26 +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 static Type TypeProcessorType => typeof(Contact3NonconvexOneBodyTypeProcessor);
+        public static TypeProcessor CreateTypeProcessor() => new Contact3NonconvexOneBodyTypeProcessor();
 
     }
 
@@ -419,49 +359,27 @@ public struct Contact3NonconvexOneBodyPrestepData : INonconvexContactPrestep<Con
         public NonconvexContactPrestepData Contact2;
 
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public ref MaterialPropertiesWide GetMaterialProperties(ref Contact3NonconvexOneBodyPrestepData prestep)
+        public static ref MaterialPropertiesWide GetMaterialProperties(ref Contact3NonconvexOneBodyPrestepData prestep)
         {
             return ref prestep.MaterialProperties;
         }
 
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public ref NonconvexContactPrestepData GetContact(ref Contact3NonconvexOneBodyPrestepData prestep, int index)
+        public static ref NonconvexContactPrestepData GetContact(ref Contact3NonconvexOneBodyPrestepData prestep, int index)
         {
             return ref Unsafe.Add(ref prestep.Contact0, index);
         }
                 
-        public readonly int ContactCount => 3;
-        public readonly int BodyCount => 1;
-    }
-
-    public unsafe struct Contact3NonconvexOneBodyProjection : INonconvexOneBodyProjection<Contact3NonconvexOneBodyProjection>
-    {
-        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;
-    }
+        public static int ContactCount => 3;
+        public static int BodyCount => 1;
+    }    
 
     /// <summary>
     /// Handles the solve iterations of a bunch of 3-contact nonconvex one body manifold constraints.
     /// </summary>
     public class Contact3NonconvexOneBodyTypeProcessor :
-        OneBodyContactTypeProcessor<Contact3NonconvexOneBodyPrestepData, Contact3NonconvexOneBodyProjection, Contact3NonconvexAccumulatedImpulses,
-            ContactNonconvexOneBodyFunctions<Contact3NonconvexOneBodyPrestepData, Contact3NonconvexOneBodyProjection, Contact3NonconvexAccumulatedImpulses>>
+        OneBodyContactTypeProcessor<Contact3NonconvexOneBodyPrestepData, Contact3NonconvexAccumulatedImpulses,
+            ContactNonconvexOneBodyFunctions<Contact3NonconvexOneBodyPrestepData, Contact3NonconvexAccumulatedImpulses>>
     {
         //Matches UpdateConstraintForManifold's manifoldTypeAsConstraintType computation.
         public const int BatchTypeId = 9;
@@ -481,7 +399,7 @@ public void ApplyDescription(ref TypeBatch batch, int bundleIndex, int innerInde
             NonconvexConstraintHelpers.ApplyTwoBodyDescription<Contact4Nonconvex, Contact4NonconvexPrestepData>(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<Contact4Nonconvex, Contact4NonconvexPrestepData>(ref batch, bundleIndex, innerIndex, out description);
         }
@@ -496,26 +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 static Type TypeProcessorType => typeof(Contact4NonconvexTypeProcessor);
+        public static TypeProcessor CreateTypeProcessor() => new Contact4NonconvexTypeProcessor();
 
     }
 
@@ -530,25 +449,25 @@ public struct Contact4NonconvexPrestepData : ITwoBodyNonconvexContactPrestep<Con
         public NonconvexContactPrestepData Contact3;
 
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public ref MaterialPropertiesWide GetMaterialProperties(ref Contact4NonconvexPrestepData prestep)
+        public static ref MaterialPropertiesWide GetMaterialProperties(ref Contact4NonconvexPrestepData prestep)
         {
             return ref prestep.MaterialProperties;
         }
 
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public ref Vector3Wide GetOffsetB(ref Contact4NonconvexPrestepData prestep)
+        public static ref Vector3Wide GetOffsetB(ref Contact4NonconvexPrestepData prestep)
         {
             return ref prestep.OffsetB;
         }
 
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public ref NonconvexContactPrestepData GetContact(ref Contact4NonconvexPrestepData prestep, int index)
+        public static ref NonconvexContactPrestepData GetContact(ref Contact4NonconvexPrestepData prestep, int index)
         {
             return ref Unsafe.Add(ref prestep.Contact0, index);
         }
         
-        public readonly int ContactCount => 4;
-        public readonly int BodyCount => 2;
+        public static int ContactCount => 4;
+        public static int BodyCount => 2;
     }
 
     public struct Contact4NonconvexAccumulatedImpulses : INonconvexContactAccumulatedImpulses<Contact4NonconvexAccumulatedImpulses>
@@ -557,44 +476,21 @@ 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);
         }
     }
-
-    public unsafe struct Contact4NonconvexProjection : INonconvexTwoBodyProjection<Contact4NonconvexProjection>
-    {
-        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;
-    }
-    
+        
     /// <summary>
     /// Handles the solve iterations of a bunch of 4-contact nonconvex two body manifold constraints.
     /// </summary>
     public class Contact4NonconvexTypeProcessor :
-        TwoBodyContactTypeProcessor<Contact4NonconvexPrestepData, Contact4NonconvexProjection, Contact4NonconvexAccumulatedImpulses,
-            ContactNonconvexTwoBodyFunctions<Contact4NonconvexPrestepData, Contact4NonconvexProjection, Contact4NonconvexAccumulatedImpulses>>
+        TwoBodyContactTypeProcessor<Contact4NonconvexPrestepData, Contact4NonconvexAccumulatedImpulses,
+            ContactNonconvexTwoBodyFunctions<Contact4NonconvexPrestepData, Contact4NonconvexAccumulatedImpulses>>
     {
         //Matches UpdateConstraintForManifold's manifoldTypeAsConstraintType computation.
         public const int BatchTypeId = 17;
@@ -613,7 +509,7 @@ public void ApplyDescription(ref TypeBatch batch, int bundleIndex, int innerInde
             NonconvexConstraintHelpers.ApplyOneBodyDescription<Contact4NonconvexOneBody, Contact4NonconvexOneBodyPrestepData>(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<Contact4NonconvexOneBody, Contact4NonconvexOneBodyPrestepData>(ref batch, bundleIndex, innerIndex, out description);
         }
@@ -627,26 +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 static Type TypeProcessorType => typeof(Contact4NonconvexOneBodyTypeProcessor);
+        public static TypeProcessor CreateTypeProcessor() => new Contact4NonconvexOneBodyTypeProcessor();
 
     }
 
@@ -660,50 +557,27 @@ public struct Contact4NonconvexOneBodyPrestepData : INonconvexContactPrestep<Con
         public NonconvexContactPrestepData Contact3;
 
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public ref MaterialPropertiesWide GetMaterialProperties(ref Contact4NonconvexOneBodyPrestepData prestep)
+        public static ref MaterialPropertiesWide GetMaterialProperties(ref Contact4NonconvexOneBodyPrestepData prestep)
         {
             return ref prestep.MaterialProperties;
         }
 
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public ref NonconvexContactPrestepData GetContact(ref Contact4NonconvexOneBodyPrestepData prestep, int index)
+        public static ref NonconvexContactPrestepData GetContact(ref Contact4NonconvexOneBodyPrestepData prestep, int index)
         {
             return ref Unsafe.Add(ref prestep.Contact0, index);
         }
                 
-        public readonly int ContactCount => 4;
-        public readonly int BodyCount => 1;
-    }
-
-    public unsafe struct Contact4NonconvexOneBodyProjection : INonconvexOneBodyProjection<Contact4NonconvexOneBodyProjection>
-    {
-        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;
-    }
+        public static int ContactCount => 4;
+        public static int BodyCount => 1;
+    }    
 
     /// <summary>
     /// Handles the solve iterations of a bunch of 4-contact nonconvex one body manifold constraints.
     /// </summary>
     public class Contact4NonconvexOneBodyTypeProcessor :
-        OneBodyContactTypeProcessor<Contact4NonconvexOneBodyPrestepData, Contact4NonconvexOneBodyProjection, Contact4NonconvexAccumulatedImpulses,
-            ContactNonconvexOneBodyFunctions<Contact4NonconvexOneBodyPrestepData, Contact4NonconvexOneBodyProjection, Contact4NonconvexAccumulatedImpulses>>
+        OneBodyContactTypeProcessor<Contact4NonconvexOneBodyPrestepData, Contact4NonconvexAccumulatedImpulses,
+            ContactNonconvexOneBodyFunctions<Contact4NonconvexOneBodyPrestepData, Contact4NonconvexAccumulatedImpulses>>
     {
         //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..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.ApplyTwoBodyDescription<Contact<#= contactCount #>Nonconvex, 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.BuildTwoBodyDescription<Contact<#= contactCount #>Nonconvex, Contact<#= contactCount #>NonconvexPrestepData>(ref batch, bundleIndex, innerIndex, out description);
         }
@@ -44,26 +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 static Type TypeProcessorType => typeof(Contact<#= contactCount #>NonconvexTypeProcessor);
+        public static TypeProcessor CreateTypeProcessor() => new Contact<#= contactCount #>NonconvexTypeProcessor();
 
     }
 
@@ -79,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;
         }
@@ -96,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 : INonconvexContactAccumulatedImpulses<Contact<#= contactCount #>NonconvexAccumulatedImpulses>
@@ -107,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)
@@ -115,37 +116,13 @@ for (int i = 0; i < contactCount ; ++i)
             return ref Unsafe.Add(ref impulses.Contact0, index);
         }
     }
-
-    public unsafe struct Contact<#= contactCount #>NonconvexProjection : INonconvexTwoBodyProjection<Contact<#= contactCount #>NonconvexProjection>
-    {
-        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 #>;
-    }
-    
+        
     /// <summary>
     /// Handles the solve iterations of a bunch of <#= contactCount #>-contact nonconvex two body manifold constraints.
     /// </summary>
     public class Contact<#= contactCount #>NonconvexTypeProcessor :
-        TwoBodyContactTypeProcessor<Contact<#= contactCount #>NonconvexPrestepData, Contact<#= contactCount #>NonconvexProjection, Contact<#= contactCount #>NonconvexAccumulatedImpulses,
-            ContactNonconvexTwoBodyFunctions<Contact<#= contactCount #>NonconvexPrestepData, Contact<#= contactCount #>NonconvexProjection, Contact<#= contactCount #>NonconvexAccumulatedImpulses>>
+        TwoBodyContactTypeProcessor<Contact<#= contactCount #>NonconvexPrestepData, Contact<#= contactCount #>NonconvexAccumulatedImpulses,
+            ContactNonconvexTwoBodyFunctions<Contact<#= contactCount #>NonconvexPrestepData, Contact<#= contactCount #>NonconvexAccumulatedImpulses>>
     {
         //Matches UpdateConstraintForManifold's manifoldTypeAsConstraintType computation.
         public const int BatchTypeId = <#= 13 + contactCount #>;
@@ -165,7 +142,7 @@ for (int i = 0; i < contactCount ; ++i)
             NonconvexConstraintHelpers.ApplyOneBodyDescription<Contact<#= contactCount #>NonconvexOneBody, 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.BuildOneBodyDescription<Contact<#= contactCount #>NonconvexOneBody, Contact<#= contactCount #>NonconvexOneBodyPrestepData>(ref batch, bundleIndex, innerIndex, out description);
         }
@@ -179,26 +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 static Type TypeProcessorType => typeof(Contact<#= contactCount #>NonconvexOneBodyTypeProcessor);
+        public static TypeProcessor CreateTypeProcessor() => new Contact<#= contactCount #>NonconvexOneBodyTypeProcessor();
 
     }
 
@@ -213,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;
         }
@@ -224,40 +202,16 @@ 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 unsafe struct Contact<#= contactCount #>NonconvexOneBodyProjection : INonconvexOneBodyProjection<Contact<#= contactCount #>NonconvexOneBodyProjection>
-    {
-        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 #>;
-    }
+        public static int ContactCount => <#= contactCount #>;
+        public static int BodyCount => 1;
+    }    
 
     /// <summary>
     /// Handles the solve iterations of a bunch of <#= contactCount #>-contact nonconvex one body manifold constraints.
     /// </summary>
     public class Contact<#= contactCount #>NonconvexOneBodyTypeProcessor :
-        OneBodyContactTypeProcessor<Contact<#= contactCount #>NonconvexOneBodyPrestepData, Contact<#= contactCount #>NonconvexOneBodyProjection, Contact<#= contactCount #>NonconvexAccumulatedImpulses,
-            ContactNonconvexOneBodyFunctions<Contact<#= contactCount #>NonconvexOneBodyPrestepData, Contact<#= contactCount #>NonconvexOneBodyProjection, Contact<#= contactCount #>NonconvexAccumulatedImpulses>>
+        OneBodyContactTypeProcessor<Contact<#= contactCount #>NonconvexOneBodyPrestepData, Contact<#= contactCount #>NonconvexAccumulatedImpulses,
+            ContactNonconvexOneBodyFunctions<Contact<#= contactCount #>NonconvexOneBodyPrestepData, Contact<#= contactCount #>NonconvexAccumulatedImpulses>>
     {
         //Matches UpdateConstraintForManifold's manifoldTypeAsConstraintType computation.
         public const int BatchTypeId = <#= 6 + contactCount #>;
diff --git a/BepuPhysics/Constraints/Contact/IContactConstraintDescription.cs b/BepuPhysics/Constraints/Contact/IContactConstraintDescription.cs
index b1a90df04..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
 {
@@ -15,13 +12,13 @@ public interface IConvexOneBodyContactConstraintDescription<TDescription> : IOne
         where TDescription : unmanaged, IConvexOneBodyContactConstraintDescription<TDescription>
     {
         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<TDescription> : ITwoBodyConstraintDescription<TDescription> 
         where TDescription : unmanaged, IConvexTwoBodyContactConstraintDescription<TDescription>
     {
         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 +46,19 @@ public interface INonconvexOneBodyContactConstraintDescription<TDescription> : I
         where TDescription : unmanaged, INonconvexOneBodyContactConstraintDescription<TDescription>
     {
         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<TDescription> : ITwoBodyConstraintDescription<TDescription> 
         where TDescription : unmanaged, INonconvexTwoBodyContactConstraintDescription<TDescription>
     {
         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/Contact/PenetrationLimit.cs b/BepuPhysics/Constraints/Contact/PenetrationLimit.cs
index 56255bcc9..9b9a09151 100644
--- a/BepuPhysics/Constraints/Contact/PenetrationLimit.cs
+++ b/BepuPhysics/Constraints/Contact/PenetrationLimit.cs
@@ -1,30 +1,85 @@
 using BepuUtilities;
-using System;
 using System.Numerics;
 using System.Runtime.CompilerServices;
-using System.Runtime.InteropServices;
 
 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<float> EffectiveMass;
-        public Vector<float> BiasVelocity;
-    }
-
     public static class PenetrationLimit
     {
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public static void Prestep(in BodyInertias inertiaA, in BodyInertias inertiaB,
-            in Vector3Wide contactOffsetA, in Vector3Wide contactOffsetB, in Vector3Wide normal, in Vector<float> depth,
-            in Vector<float> positionErrorToVelocity, in Vector<float> effectiveMassCFMScale, in Vector<float> maximumRecoveryVelocity,
-            float inverseDt, out PenetrationLimitProjection projection)
+        public static void ComputeCorrectiveImpulse(in BodyVelocityWide wsvA, in BodyVelocityWide wsvB,
+            in Vector3Wide normal, in Vector3Wide angularA, in Vector3Wide angularB, in Vector<float> biasVelocity, in Vector<float> softnessImpulseScale, in Vector<float> effectiveMass,
+            ref Vector<float> accumulatedImpulse, out Vector<float> 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, angularA, out var csvaAngular);
+            Vector3Wide.Dot(wsvB.Linear, normal, out var negatedCSVBLinear);
+            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 - biasVelocity) * effectiveMass;
+
+            var previousAccumulated = accumulatedImpulse;
+            accumulatedImpulse = Vector.Max(Vector<float>.Zero, accumulatedImpulse - negatedCSI);
+
+            correctiveCSI = accumulatedImpulse - previousAccumulated;
+        }
+
+        [MethodImpl(MethodImplOptions.AggressiveInlining)]
+        public static void UpdatePenetrationDepth(in Vector<float> dt, in Vector3Wide contactOffsetA, in Vector3Wide offsetB, in Vector3Wide normal, in BodyVelocityWide velocityA, in BodyVelocityWide velocityB, ref Vector<float> 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)
+            Vector3Wide.CrossWithoutOverlap(velocityA.Angular, contactOffsetA, out var wxra);
+            Vector3Wide.Add(wxra, velocityA.Linear, out var contactVelocityA);
+
+            Vector3Wide.Subtract(contactOffsetA, offsetB, out var contactOffsetB);
+            Vector3Wide.CrossWithoutOverlap(velocityB.Angular, contactOffsetB, 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);
+            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<float> 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 WarmStart(
+            in BodyInertiaWide inertiaA, in BodyInertiaWide inertiaB, in Vector3Wide normal, in Vector3Wide contactOffsetA, in Vector3Wide contactOffsetB,
+            in Vector<float> accumulatedImpulse, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB)
         {
-            //We directly take the prestep data here since the jacobians and error don't undergo any processing.
+            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);
+        }
+
 
+        [MethodImpl(MethodImplOptions.AggressiveInlining)]
+        public static void Solve(
+            in BodyInertiaWide inertiaA, in BodyInertiaWide inertiaB, in Vector3Wide normal, in Vector3Wide contactOffsetA, in Vector3Wide contactOffsetB,
+            in Vector<float> depth, in Vector<float> positionErrorToVelocity, in Vector<float> effectiveMassCFMScale, in Vector<float> maximumRecoveryVelocity, in Vector<float> inverseDt, in Vector<float> softnessImpulseScale,
+            ref Vector<float> accumulatedImpulse, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB)
+        {
             //The contact penetration constraint takes the form:
             //dot(positionA + offsetA, N) >= dot(positionB + offsetB, N)
             //Or:
@@ -52,103 +107,27 @@ public static void Prestep(in BodyInertias inertiaA, in BodyInertias inertiaB,
             //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);
+            Vector3Wide.CrossWithoutOverlap(contactOffsetA, normal, out var angularA);
+            Vector3Wide.CrossWithoutOverlap(normal, contactOffsetB, out var angularB);
 
             //effective mass
-            Symmetric3x3Wide.VectorSandwich(projection.AngularA, inertiaA.InverseInertiaTensor, out var angularA0);
-            Symmetric3x3Wide.VectorSandwich(projection.AngularB, inertiaB.InverseInertiaTensor, out var angularB0);
+            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.
-            projection.EffectiveMass = effectiveMassCFMScale / (linear + angularA0 + angularB0);
+            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.
-            projection.BiasVelocity = Vector.Min(
-                depth * new Vector<float>(inverseDt),
+            var biasVelocity = Vector.Min(
+                depth * inverseDt,
                 Vector.Min(depth * positionErrorToVelocity, maximumRecoveryVelocity));
-        }
 
-
-        /// <summary>
-        /// Transforms an impulse from constraint space to world space, uses it to modify the cached world space velocities of the bodies.
-        /// </summary>
-        [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public static void ApplyImpulse(in PenetrationLimitProjection projection, in BodyInertias inertiaA, in BodyInertias inertiaB, in Vector3Wide normal,
-            in Vector<float> correctiveImpulse,
-            ref BodyVelocities wsvA, ref BodyVelocities 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 BodyInertias inertiaA, in BodyInertias inertiaB, in Vector3Wide normal,
-            in Vector<float> accumulatedImpulse, ref BodyVelocities wsvA, ref BodyVelocities wsvB)
-        {
-            ApplyImpulse(projection, inertiaA, inertiaB, normal, accumulatedImpulse, ref wsvA, ref wsvB);
-        }
-
-        [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public static void ComputeCorrectiveImpulse(in BodyVelocities wsvA, in BodyVelocities wsvB,
-            in PenetrationLimitProjection projection,
-            in Vector3Wide normal, in Vector<float> softnessImpulseScale,
-            ref Vector<float> accumulatedImpulse, out Vector<float> 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(wsvB.Linear, normal, out var negatedCSVBLinear);
-            Vector3Wide.Dot(wsvB.Angular, projection.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 previousAccumulated = accumulatedImpulse;
-            accumulatedImpulse = Vector.Max(Vector<float>.Zero, accumulatedImpulse - negatedCSI);
-
-            correctiveCSI = accumulatedImpulse - previousAccumulated;
-
-        }
-
-        [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public static void Solve(in PenetrationLimitProjection projection, in BodyInertias inertiaA, in BodyInertias inertiaB, in Vector3Wide normal,
-            in Vector<float> softnessImpulseScale, ref Vector<float> accumulatedImpulse, ref BodyVelocities wsvA, ref BodyVelocities 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<float> dt, in Vector3Wide contactOffsetA, in Vector3Wide offsetB, in Vector3Wide normal, in BodyVelocities velocityA, in BodyVelocities velocityB, ref Vector<float> 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)
-            Vector3Wide.CrossWithoutOverlap(velocityA.Angular, contactOffsetA, out var wxra);
-            Vector3Wide.Add(wxra, velocityA.Linear, out var contactVelocityA);
-
-            Vector3Wide.Subtract(contactOffsetA, offsetB, out var contactOffsetB);
-            Vector3Wide.CrossWithoutOverlap(velocityB.Angular, contactOffsetB, 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);
-            penetrationDepth -= estimatedDepthChangeVelocity * dt;
+            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 7a338825a..205d6f5a9 100644
--- a/BepuPhysics/Constraints/Contact/PenetrationLimitOneBody.cs
+++ b/BepuPhysics/Constraints/Contact/PenetrationLimitOneBody.cs
@@ -1,53 +1,47 @@
 using BepuUtilities;
-using System;
 using System.Numerics;
 using System.Runtime.CompilerServices;
-using System.Runtime.InteropServices;
 
 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<float> EffectiveMass;
-        public Vector<float> BiasVelocity;
-    }
-
     public static class PenetrationLimitOneBody
     {
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public static void Prestep(in BodyInertias inertiaA,
-            in Vector3Wide contactOffsetA, in Vector3Wide normal, in Vector<float> depth,
-            in Vector<float> positionErrorToVelocity, in Vector<float> effectiveMassCFMScale, in Vector<float> maximumRecoveryVelocity,
-            float inverseDt, out PenetrationLimitOneBodyProjection projection)
+        public static void ComputeCorrectiveImpulse(in BodyVelocityWide wsvA,
+            in Vector3Wide normal, in Vector3Wide angularA, in Vector<float> biasVelocity, in Vector<float> softnessImpulseScale, in Vector<float> effectiveMass,
+            ref Vector<float> accumulatedImpulse, out Vector<float> correctiveCSI)
         {
-            //See PenetrationLimit.cs for a derivation.
-            Vector3Wide.CrossWithoutOverlap(contactOffsetA, normal, out projection.AngularA);
+            //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, angularA, out var csvaAngular);
+            //Compute negated version to avoid the need for an explicit negate.
+            var negatedCSI = accumulatedImpulse * softnessImpulseScale + (csvaLinear + csvaAngular - biasVelocity) * effectiveMass;
 
-            //effective mass
-            Symmetric3x3Wide.VectorSandwich(projection.AngularA, inertiaA.InverseInertiaTensor, out var angularA0);
+            var previousAccumulated = accumulatedImpulse;
+            accumulatedImpulse = Vector.Max(Vector<float>.Zero, accumulatedImpulse - negatedCSI);
 
-            //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);
+            correctiveCSI = accumulatedImpulse - previousAccumulated;
+        }
 
-            //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<float>(inverseDt),
-                Vector.Min(depth * positionErrorToVelocity, maximumRecoveryVelocity));
+        [MethodImpl(MethodImplOptions.AggressiveInlining)]
+        public static void UpdatePenetrationDepth(in Vector<float> dt, in Vector3Wide contactOffset, in Vector3Wide normal, in BodyVelocityWide velocity, ref Vector<float> 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.
+            //estimatedPenetrationDepthChange = dot(normal, velocityDtA.Linear + velocityDtA.Angular x contactOffsetA)
+            Vector3Wide.CrossWithoutOverlap(velocity.Angular, contactOffset, out var wxr);
+            Vector3Wide.Add(wxr, velocity.Linear, out var contactVelocity);
+            Vector3Wide.Dot(normal, contactVelocity, out var estimatedDepthChange);
+            penetrationDepth -= estimatedDepthChange * dt;
         }
 
 
-        /// <summary>
-        /// Transforms an impulse from constraint space to world space, uses it to modify the cached world space velocities of the bodies.
-        /// </summary>
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public static void ApplyImpulse(in PenetrationLimitOneBodyProjection projection, in BodyInertias inertiaA, in Vector3Wide normal,
-            in Vector<float> correctiveImpulse, ref BodyVelocities wsvA)
+        public static void ApplyImpulse(in BodyInertiaWide inertiaA, in Vector3Wide normal, in Vector3Wide angularA, in Vector<float> correctiveImpulse, ref BodyVelocityWide wsvA)
         {
             var linearVelocityChangeA = correctiveImpulse * inertiaA.InverseMass;
             Vector3Wide.Scale(normal, linearVelocityChangeA, out var correctiveVelocityALinearVelocity);
-            Vector3Wide.Scale(projection.AngularA, correctiveImpulse, out var correctiveAngularImpulseA);
+            Vector3Wide.Scale(angularA, correctiveImpulse, out var correctiveAngularImpulseA);
             Symmetric3x3Wide.TransformWithoutOverlap(correctiveAngularImpulseA, inertiaA.InverseInertiaTensor, out var correctiveVelocityAAngularVelocity);
 
             Vector3Wide.Add(wsvA.Linear, correctiveVelocityALinearVelocity, out wsvA.Linear);
@@ -55,51 +49,64 @@ 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<float> accumulatedImpulse, ref BodyVelocities wsvA)
+        public static void WarmStart(in BodyInertiaWide inertiaA, in Vector3Wide normal, in Vector3Wide contactOffsetA, in Vector<float> accumulatedImpulse, ref BodyVelocityWide wsvA)
         {
-            ApplyImpulse(projection, inertiaA, normal, accumulatedImpulse, ref wsvA);
+            Vector3Wide.CrossWithoutOverlap(contactOffsetA, normal, out var angularA);
+            ApplyImpulse(inertiaA, normal, angularA, accumulatedImpulse, ref wsvA);
         }
 
+
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public static void ComputeCorrectiveImpulse(in BodyVelocities wsvA,
-            in PenetrationLimitOneBodyProjection projection,
-            in Vector3Wide normal, in Vector<float> softnessImpulseScale,
-            ref Vector<float> accumulatedImpulse, out Vector<float> correctiveCSI)
+        public static void Solve(
+            in BodyInertiaWide inertiaA, in Vector3Wide normal, in Vector3Wide contactOffsetA,
+            in Vector<float> depth, in Vector<float> positionErrorToVelocity, in Vector<float> effectiveMassCFMScale, in Vector<float> maximumRecoveryVelocity, in Vector<float> inverseDt, in Vector<float> softnessImpulseScale,
+            ref Vector<float> accumulatedImpulse, ref BodyVelocityWide wsvA)
         {
-            //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);
-            //Compute negated version to avoid the need for an explicit negate.
-            var negatedCSI = accumulatedImpulse * softnessImpulseScale + (csvaLinear + csvaAngular - projection.BiasVelocity) * projection.EffectiveMass;
+            //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);
 
-            var previousAccumulated = accumulatedImpulse;
-            accumulatedImpulse = Vector.Max(Vector<float>.Zero, accumulatedImpulse - negatedCSI);
-
-            correctiveCSI = accumulatedImpulse - previousAccumulated;
+            //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);
 
-        [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public static void Solve(in PenetrationLimitOneBodyProjection projection, in BodyInertias inertiaA, in Vector3Wide normal,
-            in Vector<float> softnessImpulseScale, ref Vector<float> accumulatedImpulse, ref BodyVelocities wsvA)
-        {
-            ComputeCorrectiveImpulse(wsvA, projection, normal, softnessImpulseScale, ref accumulatedImpulse, out var correctiveCSI);
-            ApplyImpulse(projection, inertiaA, normal, correctiveCSI, ref wsvA);
-        }
+            //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));
 
-        [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public static void UpdatePenetrationDepth(in Vector<float> dt, in Vector3Wide contactOffset, in Vector3Wide normal, in BodyVelocities velocity, ref Vector<float> 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.
-            //estimatedPenetrationDepthChange = dot(normal, velocityDtA.Linear + velocityDtA.Angular x contactOffsetA)
-            Vector3Wide.CrossWithoutOverlap(velocity.Angular, contactOffset, out var wxr);
-            Vector3Wide.Add(wxr, velocity.Linear, out var contactVelocity);
-            Vector3Wide.Dot(normal, contactVelocity, out var estimatedDepthChange);
-            penetrationDepth -= estimatedDepthChange * dt;
+            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 87b3d8cc1..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;
 
@@ -10,16 +9,6 @@ namespace BepuPhysics.Constraints.Contact
     /// </summary>
     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;
@@ -28,7 +17,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: 
@@ -63,41 +52,17 @@ public static void ComputeJacobians(ref Vector3Wide tangentX, ref Vector3Wide ta
             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 BodyInertias inertiaA, ref BodyInertias inertiaB,
-            out Projection projection)
-        {
-            ComputeJacobians(ref tangentX, ref tangentY, ref offsetA, ref 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.
-        }
-
         /// <summary>
         /// Transforms an impulse from constraint space to world space, uses it to modify the cached world space velocities of the bodies.
         /// </summary>
         [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(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);
             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,18 +74,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)
-        {
-            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.
-            //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);
-        }
-
-        [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public static void ComputeCorrectiveImpulse(ref BodyVelocities wsvA, ref BodyVelocities wsvB, ref TangentFriction.Projection data, ref Jacobians jacobians,
-            ref Vector<float> 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<float> 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 +89,7 @@ public static void ComputeCorrectiveImpulse(ref BodyVelocities wsvA, ref BodyVel
             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);
@@ -149,13 +104,36 @@ 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<float> maximumImpulse, ref Vector2Wide accumulatedImpulse, ref BodyVelocities wsvA, ref BodyVelocities wsvB)
+        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(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);
-            ApplyImpulse(ref jacobians, ref inertiaA, ref inertiaB, ref correctiveCSI, ref wsvA, ref 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 Solve(in Vector3Wide tangentX, in Vector3Wide tangentY, in Vector3Wide offsetToManifoldCenterA, in Vector3Wide offsetToManifoldCenterB, in BodyInertiaWide inertiaA, in BodyInertiaWide inertiaB,
+            in Vector<float> maximumImpulse, ref Vector2Wide accumulatedImpulse, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB)
+        {
+            ComputeJacobians(tangentX, tangentY, offsetToManifoldCenterA, offsetToManifoldCenterB, 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 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 cec88f363..68b909a43 100644
--- a/BepuPhysics/Constraints/Contact/TangentFrictionOneBody.cs
+++ b/BepuPhysics/Constraints/Contact/TangentFrictionOneBody.cs
@@ -14,18 +14,10 @@ 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)]
-        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;
@@ -34,33 +26,16 @@ public static void ComputeJacobians(ref Vector3Wide tangentX, ref Vector3Wide ta
             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 BodyInertias inertiaA,
-            out Projection projection)
-        {
-            ComputeJacobians(ref tangentX, ref tangentY, ref 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.
-        }
-
         /// <summary>
         /// Transforms an impulse from constraint space to world space, uses it to modify the cached world space velocities of the bodies.
         /// </summary>
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public static void ApplyImpulse(ref Jacobians jacobians, ref BodyInertias inertiaA,
-            ref Vector2Wide correctiveImpulse, ref BodyVelocities 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);
-            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,24 +43,14 @@ 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)
-        {
-            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.
-            //To compensate for this, the accumulated impulse should be scaled if dt changes.
-            ApplyImpulse(ref jacobians, ref inertiaA, ref accumulatedImpulse, ref wsvA);
-        }
-
-        [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public static void ComputeCorrectiveImpulse(ref BodyVelocities wsvA, ref Projection data, ref Jacobians jacobians,
-            ref Vector<float> maximumImpulse, ref Vector2Wide accumulatedImpulse, out Vector2Wide correctiveCSI)
+        public static void ComputeCorrectiveImpulse(in BodyVelocityWide wsvA, in Symmetric2x2Wide effectiveMass, in Jacobians jacobians,
+            in Vector<float> 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);
@@ -100,13 +65,30 @@ 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<float> maximumImpulse, ref Vector2Wide accumulatedImpulse, ref BodyVelocities 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.
+            //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 Solve(in Vector3Wide tangentX, in Vector3Wide tangentY, in Vector3Wide offsetToManifoldCenterA, in BodyInertiaWide inertiaA,
+            in Vector<float> 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);
-            ApplyImpulse(ref jacobians, ref inertiaA, ref correctiveCSI, ref 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 7c956b551..5cd7ced4d 100644
--- a/BepuPhysics/Constraints/Contact/TwistFriction.cs
+++ b/BepuPhysics/Constraints/Contact/TwistFriction.cs
@@ -1,51 +1,20 @@
 using BepuUtilities;
-using System.Diagnostics;
 using System.Numerics;
 using System.Runtime.CompilerServices;
 
 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<float> EffectiveMass;
-    }
-
     /// <summary>
     /// Handles the tangent friction implementation.
     /// </summary>
     public static class TwistFriction
     {
-        [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public static void Prestep(ref BodyInertias inertiaA, ref BodyInertias 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<float>.Zero, inverseEffectiveMass);
-            projection.EffectiveMass = Vector.ConditionalSelect(inverseIsZero, Vector<float>.Zero, Vector<float>.One / inverseEffectiveMass);
-
-            //Note that friction constraints have no bias velocity. They target zero velocity.
-        }
-
         /// <summary>
         /// Transforms an impulse from constraint space to world space, uses it to modify the cached world space velocities of the bodies.
         /// </summary>
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public static void ApplyImpulse(ref Vector3Wide angularJacobianA, ref BodyInertias inertiaA, ref BodyInertias inertiaB,
-            ref Vector<float> correctiveImpulse, ref BodyVelocities wsvA, ref BodyVelocities wsvB)
+        public static void ApplyImpulse(in Vector3Wide angularJacobianA, in BodyInertiaWide inertiaA, in BodyInertiaWide inertiaB,
+            in Vector<float> correctiveImpulse, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB)
         {
             Vector3Wide.Scale(angularJacobianA, correctiveImpulse, out var worldCorrectiveImpulseA);
             Symmetric3x3Wide.TransformWithoutOverlap(worldCorrectiveImpulseA, inertiaA.InverseInertiaTensor, out var worldCorrectiveVelocityA);
@@ -55,21 +24,14 @@ 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<float> accumulatedImpulse, ref BodyVelocities wsvA, ref BodyVelocities 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<float> maximumImpulse,
+        public static void ComputeCorrectiveImpulse(in Vector3Wide angularJacobianA, in Vector<float> effectiveMass,
+            in BodyVelocityWide wsvA, in BodyVelocityWide wsvB, in Vector<float> maximumImpulse,
             ref Vector<float> accumulatedImpulse, out Vector<float> 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));
 
@@ -78,11 +40,33 @@ 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<float> maximumImpulse, ref Vector<float> accumulatedImpulse, ref BodyVelocities wsvA, ref BodyVelocities wsvB)
+        public static void WarmStart(in Vector3Wide angularJacobianA, in BodyInertiaWide inertiaA, in BodyInertiaWide inertiaB,
+            in Vector<float> 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, accumulatedImpulse, ref wsvA, ref wsvB);
+        }
+
+        [MethodImpl(MethodImplOptions.AggressiveInlining)]
+        public static void Solve(in Vector3Wide angularJacobianA, in BodyInertiaWide inertiaA, in BodyInertiaWide inertiaB,
+            in Vector<float> maximumImpulse, ref Vector<float> 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<float>.Zero, inverseEffectiveMass);
+            var effectiveMass = Vector.ConditionalSelect(inverseIsZero, Vector<float>.Zero, Vector<float>.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 7758f95e2..ad00025fe 100644
--- a/BepuPhysics/Constraints/Contact/TwistFrictionOneBody.cs
+++ b/BepuPhysics/Constraints/Contact/TwistFrictionOneBody.cs
@@ -12,32 +12,12 @@ namespace BepuPhysics.Constraints.Contact
     /// </summary>
     public static class TwistFrictionOneBody
     {
-        [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public static void Prestep(ref BodyInertias 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<float>.Zero, inverseEffectiveMass);
-            projection.EffectiveMass = Vector.ConditionalSelect(inverseIsZero, Vector<float>.Zero, Vector<float>.One / inverseEffectiveMass);
-
-            //Note that friction constraints have no bias velocity. They target zero velocity.
-
-
-        }
-
         /// <summary>
         /// Transforms an impulse from constraint space to world space, uses it to modify the cached world space velocities of the bodies.
         /// </summary>
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public static void ApplyImpulse(ref Vector3Wide angularJacobianA, ref BodyInertias inertiaA,
-            ref Vector<float> correctiveImpulse, ref BodyVelocities wsvA)
+        public static void ApplyImpulse(in Vector3Wide angularJacobianA, in BodyInertiaWide inertiaA,
+            in Vector<float> correctiveImpulse, ref BodyVelocityWide wsvA)
         {
             Vector3Wide.Scale(angularJacobianA, correctiveImpulse, out var worldCorrectiveImpulseA);
             Symmetric3x3Wide.TransformWithoutOverlap(worldCorrectiveImpulseA, inertiaA.InverseInertiaTensor, out var worldCorrectiveVelocityA);
@@ -45,19 +25,12 @@ public static void ApplyImpulse(ref Vector3Wide angularJacobianA, ref BodyInerti
         }
 
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public static void WarmStart(ref Vector3Wide angularJacobianA, ref BodyInertias inertiaA,
-            ref Vector<float> accumulatedImpulse, ref BodyVelocities 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<float> maximumImpulse,
+        public static void ComputeCorrectiveImpulse(in Vector3Wide angularJacobianA, in Vector<float> effectiveMass,
+            in BodyVelocityWide wsvA, in Vector<float> maximumImpulse,
             ref Vector<float> accumulatedImpulse, out Vector<float> 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.
@@ -68,13 +41,30 @@ 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<float> maximumImpulse, ref Vector<float> accumulatedImpulse, ref BodyVelocities wsvA)
+        public static void WarmStart(in Vector3Wide angularJacobianA, in BodyInertiaWide inertiaA, in Vector<float> 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, accumulatedImpulse, ref wsvA);
         }
 
+        [MethodImpl(MethodImplOptions.AggressiveInlining)]
+        public static void Solve(in Vector3Wide angularJacobianA, in BodyInertiaWide inertiaA, in Vector<float> maximumImpulse, ref Vector<float> 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<float>.Zero, angularA);
+            var effectiveMass = Vector.ConditionalSelect(inverseIsZero, Vector<float>.Zero, Vector<float>.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);
+
+        }
     }
 }
diff --git a/BepuPhysics/Constraints/DistanceLimit.cs b/BepuPhysics/Constraints/DistanceLimit.cs
index 8858dd767..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;
@@ -44,7 +43,7 @@ public struct DistanceLimit : ITwoBodyConstraintDescription<DistanceLimit>
         /// <param name="maximumDistance">Maximum distance permitted between the point on A and the point on B.</param>
         /// <param name="springSettings">Spring frequency and damping parameters.</param>
         [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;
@@ -52,8 +51,8 @@ public DistanceLimit(in Vector3 localOffsetA, in Vector3 localOffsetB, float min
             MaximumDistance = maximumDistance;
             SpringSettings = springSettings;
         }
-        
-        public readonly int ConstraintTypeId
+
+        public static int ConstraintTypeId
         {
             [MethodImpl(MethodImplOptions.AggressiveInlining)]
             get
@@ -62,7 +61,8 @@ public readonly int ConstraintTypeId
             }
         }
 
-        public readonly Type TypeProcessorType => typeof(DistanceLimitTypeProcessor);
+        public static Type TypeProcessorType => typeof(DistanceLimitTypeProcessor);
+        public static TypeProcessor CreateTypeProcessor() => new DistanceLimitTypeProcessor();
 
         public readonly void ApplyDescription(ref TypeBatch batch, int bundleIndex, int innerIndex)
         {
@@ -79,7 +79,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<DistanceLimitPrestepData>.Get(ref batch.PrestepData, bundleIndex), innerIndex);
@@ -100,72 +100,87 @@ public struct DistanceLimitPrestepData
         public SpringSettingsWide SpringSettings;
     }
 
-    public struct DistanceLimitProjection
+    public struct DistanceLimitFunctions : ITwoBodyConstraintFunctions<DistanceLimitPrestepData, Vector<float>>
     {
-        public Vector3Wide LinearVelocityToImpulseA;
-        public Vector3Wide AngularVelocityToImpulseA;
-        public Vector3Wide AngularVelocityToImpulseB;
-        public Vector<float> BiasImpulse;
-        public Vector<float> SoftnessImpulseScale;
-        public Vector3Wide LinearImpulseToVelocityA;
-        public Vector3Wide AngularImpulseToVelocityA;
-        public Vector3Wide LinearImpulseToVelocityB;
-        public Vector3Wide AngularImpulseToVelocityB;
-    }
+        [MethodImpl(MethodImplOptions.AggressiveInlining)]
+        public static void ApplyImpulse(in Vector3Wide linearJacobianA, in Vector3Wide angularJacobianA, in Vector3Wide angularJacobianB, in BodyInertiaWide inertiaA, in BodyInertiaWide inertiaB,
+            in Vector<float> 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;
+        }
 
-    public struct DistanceLimitFunctions : IConstraintFunctions<DistanceLimitPrestepData, DistanceLimitProjection, Vector<float>>
-    {
         [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 static void ComputeJacobians(
+            in Vector3Wide localOffsetA, in Vector3Wide positionA, in QuaternionWide orientationA, in Vector3Wide localOffsetB, in Vector3Wide positionB, in QuaternionWide orientationB,
+            in Vector<float> minimumDistance, in Vector<float> maximumDistance, out Vector<int> useMinimum, out Vector<float> distance, out Vector3Wide direction, out Vector3Wide angularJA, out Vector3Wide angularJB)
         {
-            DistanceServoFunctions.GetDistance(bodies, ref bodyReferences, count, prestep.LocalOffsetA, prestep.LocalOffsetB,
-                out var anchorOffsetA, out var anchorOffsetB, out var anchorOffset, out var distance);
+            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.
-            var useMinimum = Vector.LessThan(Vector.Abs(distance - prestep.MinimumDistance), Vector.Abs(distance - prestep.MaximumDistance));
+            useMinimum = Vector.LessThan(Vector.Abs(distance - minimumDistance), Vector.Abs(distance - maximumDistance));
             var sign = Vector.ConditionalSelect(useMinimum, new Vector<float>(-1f), Vector<float>.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;
-
+            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<float>(1e-9f));
+            direction.X = Vector.ConditionalSelect(needFallback, Vector<float>.One, direction.X);
+            direction.Y = Vector.ConditionalSelect(needFallback, Vector<float>.Zero, direction.Y);
+            direction.Z = Vector.ConditionalSelect(needFallback, Vector<float>.Zero, direction.Z);
+
+            Vector3Wide.CrossWithoutOverlap(offsetA, direction, out angularJA);
+            Vector3Wide.CrossWithoutOverlap(direction, offsetB, out angularJB); //Note flip negation.
         }
 
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public void WarmStart(ref BodyVelocities velocityA, ref BodyVelocities velocityB, ref DistanceLimitProjection projection, ref Vector<float> accumulatedImpulse)
+        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<float> accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB)
         {
-            DistanceServoFunctions.ApplyImpulse(ref velocityA, ref velocityB, 
-                projection.LinearImpulseToVelocityA, projection.AngularImpulseToVelocityA, projection.LinearImpulseToVelocityB, projection.AngularImpulseToVelocityB, ref accumulatedImpulse);
+            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(ref BodyVelocities velocityA, ref BodyVelocities velocityB, ref DistanceLimitProjection projection, ref Vector<float> accumulatedImpulse)
+        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<float> 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);
+
             //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, ref csi);
+            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);
         }
 
+        public static bool RequiresIncrementalSubstepUpdates => false;
+        [MethodImpl(MethodImplOptions.AggressiveInlining)]
+        public static void IncrementallyUpdateForSubstep(in Vector<float> dt, in BodyVelocityWide wsvA, in BodyVelocityWide wsvB, ref DistanceLimitPrestepData prestepData) { }
     }
 
 
     /// <summary>
     /// Handles the solve iterations of a bunch of distance servos.
     /// </summary>
-    public class DistanceLimitTypeProcessor : TwoBodyTypeProcessor<DistanceLimitPrestepData, DistanceLimitProjection, Vector<float>, DistanceLimitFunctions>
+    public class DistanceLimitTypeProcessor : TwoBodyTypeProcessor<DistanceLimitPrestepData, Vector<float>, DistanceLimitFunctions, AccessAll, AccessAll, AccessAll, AccessAll>
     {
         public const int BatchTypeId = 34;
     }
diff --git a/BepuPhysics/Constraints/DistanceServo.cs b/BepuPhysics/Constraints/DistanceServo.cs
index 07a2ed414..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;
@@ -44,7 +43,7 @@ public struct DistanceServo : ITwoBodyConstraintDescription<DistanceServo>
         /// <param name="springSettings">Spring frequency and damping parameters.</param>
         /// <param name="servoSettings">Servo control parameters.</param>
         [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,12 +53,12 @@ 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)
         {
         }
 
-        public readonly int ConstraintTypeId
+        public static int ConstraintTypeId
         {
             [MethodImpl(MethodImplOptions.AggressiveInlining)]
             get
@@ -68,7 +67,8 @@ public readonly int ConstraintTypeId
             }
         }
 
-        public readonly Type TypeProcessorType => typeof(DistanceServoTypeProcessor);
+        public static Type TypeProcessorType => typeof(DistanceServoTypeProcessor);
+        public static TypeProcessor CreateTypeProcessor() => new DistanceServoTypeProcessor();
 
         public readonly void ApplyDescription(ref TypeBatch batch, int bundleIndex, int innerIndex)
         {
@@ -83,7 +83,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<DistanceServoPrestepData>.Get(ref batch.PrestepData, bundleIndex), innerIndex);
@@ -104,42 +104,37 @@ public struct DistanceServoPrestepData
         public SpringSettingsWide SpringSettings;
     }
 
-    public struct DistanceServoProjection
+    public struct DistanceServoFunctions : ITwoBodyConstraintFunctions<DistanceServoPrestepData, Vector<float>>
     {
-        public Vector3Wide LinearVelocityToImpulseA;
-        public Vector3Wide AngularVelocityToImpulseA;
-        public Vector3Wide AngularVelocityToImpulseB;
-        public Vector<float> BiasImpulse;
-        public Vector<float> SoftnessImpulseScale;
-        public Vector<float> MaximumImpulse;
-        public Vector3Wide LinearImpulseToVelocityA;
-        public Vector3Wide AngularImpulseToVelocityA;
-        public Vector3Wide LinearImpulseToVelocityB;
-        public Vector3Wide AngularImpulseToVelocityB;
-    }
-
-    public struct DistanceServoFunctions : IConstraintFunctions<DistanceServoPrestepData, DistanceServoProjection, Vector<float>>
-    {
-        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<float> 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);
         }
 
+        public static void ComputeJacobian(in Vector<float> 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<float>(1e-9f));
+            direction.X = Vector.ConditionalSelect(needFallback, Vector<float>.One, direction.X);
+            direction.Y = Vector.ConditionalSelect(needFallback, Vector<float>.Zero, direction.Y);
+            direction.Z = Vector.ConditionalSelect(needFallback, Vector<float>.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 BodyInertias inertiaA, in BodyInertias inertiaB, in Vector3Wide anchorOffsetA, in Vector3Wide anchorOffsetB, in Vector<float> distance, ref Vector3Wide direction,
+            in BodyInertiaWide inertiaA, in BodyInertiaWide inertiaB, in Vector3Wide anchorOffsetA, in Vector3Wide anchorOffsetB, in Vector<float> distance, ref Vector3Wide direction,
             float dt, in SpringSettingsWide springSettings,
             out Vector<float> positionErrorToVelocity, out Vector<float> softnessImpulseScale, out Vector<float> effectiveMass,
-            out Vector3Wide linearVelocityToImpulseA, out Vector3Wide angularVelocityToImpulseA, out Vector3Wide angularVelocityToImpulseB,
-            out Vector3Wide linearImpulseToVelocityA, out Vector3Wide angularImpulseToVelocityA, out Vector3Wide linearImpulseToVelocityB, out Vector3Wide angularImpulseToVelocityB)
+            out Vector3Wide angularJA, out Vector3Wide angularJB, out Vector3Wide angularImpulseToVelocityA, out Vector3Wide angularImpulseToVelocityB)
         {
             //Position constraint:
             //||positionA + localOffsetA * orientationA - positionB - localOffsetB * orientationB|| = distance
@@ -158,98 +153,79 @@ public static void ComputeTransforms(
 
             //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<float>(1e-9f));
-            direction.X = Vector.ConditionalSelect(needFallback, Vector<float>.One, direction.X);
-            direction.Y = Vector.ConditionalSelect(needFallback, Vector<float>.Zero, direction.Y);
-            direction.Z = Vector.ConditionalSelect(needFallback, Vector<float>.Zero, direction.Z);
-
-            Vector3Wide.CrossWithoutOverlap(anchorOffsetA, direction, out var angularJA);
-            Vector3Wide.CrossWithoutOverlap(direction, anchorOffsetB, out var angularJB); //Note flip negation.
+            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.VectorSandwich(angularJA, inertiaA.InverseInertiaTensor, out var angularContributionA);
-            Symmetric3x3Wide.VectorSandwich(angularJB, inertiaB.InverseInertiaTensor, out var angularContributionB);
+            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;
-            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(Bodies bodies, ref TwoBodyReferences bodyReferences, int count, float dt, float inverseDt, ref BodyInertias inertiaA, ref BodyInertias inertiaB,
-            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);
-
-            Vector3Wide.Scale(anchorOffset, Vector<float>.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 BodyVelocities velocityA, ref BodyVelocities velocityB,
-            in Vector3Wide linearImpulseToVelocityA, in Vector3Wide angularImpulseToVelocityA, in Vector3Wide linearImpulseToVelocityB, in Vector3Wide angularImpulseToVelocityB,
-            ref Vector<float> csi)
+        public static void ApplyImpulse(
+            in Vector<float> inverseMassA, in Vector<float> inverseMassB, in Vector3Wide direction, in Vector3Wide angularImpulseToVelocityA, in Vector3Wide angularImpulseToVelocityB,
+            in Vector<float> csi, ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB)
         {
-            Vector3Wide.Scale(linearImpulseToVelocityA, csi, out var linearVelocityChangeA);
+            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(linearImpulseToVelocityB, csi, out var linearVelocityChangeB);
+            Vector3Wide.Scale(direction, csi * inverseMassB, out var negatedLinearVelocityChangeB);
             Vector3Wide.Scale(angularImpulseToVelocityB, csi, out var angularVelocityChangeB);
-            Vector3Wide.Add(linearVelocityChangeB, velocityB.Linear, out velocityB.Linear);
+            Vector3Wide.Subtract(velocityB.Linear, negatedLinearVelocityChangeB, out velocityB.Linear);
             Vector3Wide.Add(angularVelocityChangeB, velocityB.Angular, out velocityB.Angular);
         }
 
-        [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public void WarmStart(ref BodyVelocities velocityA, ref BodyVelocities velocityB, ref DistanceServoProjection projection, ref Vector<float> accumulatedImpulse)
+
+        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<float> accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB)
         {
-            ApplyImpulse(ref velocityA, ref velocityB,
-                projection.LinearImpulseToVelocityA, projection.AngularImpulseToVelocityA, projection.LinearImpulseToVelocityB, projection.AngularImpulseToVelocityB, ref accumulatedImpulse);
+            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<float>.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);
         }
 
-        [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public void Solve(ref BodyVelocities velocityA, ref BodyVelocities velocityB, ref DistanceServoProjection projection, ref Vector<float> accumulatedImpulse)
+        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<float> accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB)
         {
-            //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);
+            GetDistance(orientationA, positionB - positionA, orientationB, prestep.LocalOffsetA, prestep.LocalOffsetB, out var anchorOffsetA, out var anchorOffsetB, out var anchorOffset, out var distance);
 
-            ApplyImpulse(ref velocityA, ref velocityB,
-                projection.LinearImpulseToVelocityA, projection.AngularImpulseToVelocityA, projection.LinearImpulseToVelocityB, projection.AngularImpulseToVelocityB, ref csi);
+            Vector3Wide.Scale(anchorOffset, Vector<float>.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);
         }
 
+        public static bool RequiresIncrementalSubstepUpdates => false;
+        [MethodImpl(MethodImplOptions.AggressiveInlining)]
+        public static void IncrementallyUpdateForSubstep(in Vector<float> dt, in BodyVelocityWide wsvA, in BodyVelocityWide wsvB, ref DistanceServoPrestepData prestepData) { }
     }
 
 
     /// <summary>
     /// Handles the solve iterations of a bunch of distance servos.
     /// </summary>
-    public class DistanceServoTypeProcessor : TwoBodyTypeProcessor<DistanceServoPrestepData, DistanceServoProjection, Vector<float>, DistanceServoFunctions>
+    public class DistanceServoTypeProcessor : TwoBodyTypeProcessor<DistanceServoPrestepData, Vector<float>, DistanceServoFunctions, AccessAll, AccessAll, AccessAll, AccessAll>
     {
         public const int BatchTypeId = 33;
     }
diff --git a/BepuPhysics/Constraints/FourBodyTypeProcessor.cs b/BepuPhysics/Constraints/FourBodyTypeProcessor.cs
index 93dc7c486..cfbc535f4 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;
 
@@ -24,39 +22,51 @@ public struct FourBodyReferences
     /// </summary>
     /// <typeparam name="TPrestepData">Type of the prestep data used by the constraint.</typeparam>
     /// <typeparam name="TAccumulatedImpulse">Type of the accumulated impulses used by the constraint.</typeparam>
-    /// <typeparam name="TProjection">Type of the projection to input.</typeparam>
-    public interface IFourBodyConstraintFunctions<TPrestepData, TProjection, TAccumulatedImpulse>
+    public interface IFourBodyConstraintFunctions<TPrestepData, TAccumulatedImpulse>
     {
-        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 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);
+        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);
+        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,
+            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);
+
+        /// <summary>
+        /// Gets whether this constraint type requires incremental updates for each substep taken beyond the first.
+        /// </summary>
+        static abstract bool RequiresIncrementalSubstepUpdates { get; }
+        static abstract void IncrementallyUpdateForSubstep(in Vector<float> dt, in BodyVelocityWide wsvA, in BodyVelocityWide wsvB, in BodyVelocityWide wsvC, in BodyVelocityWide wsvD, ref TPrestepData prestepData);
     }
 
     /// <summary>
     /// Shared implementation across all four body constraints.
     /// </summary>
-    public abstract class FourBodyTypeProcessor<TPrestepData, TProjection, TAccumulatedImpulse, TConstraintFunctions>
-        : TypeProcessor<FourBodyReferences, TPrestepData, TProjection, TAccumulatedImpulse>
-        where TPrestepData : unmanaged where TProjection : unmanaged where TAccumulatedImpulse : unmanaged
-        where TConstraintFunctions : unmanaged, IFourBodyConstraintFunctions<TPrestepData, TProjection, TAccumulatedImpulse>
+    public abstract class FourBodyTypeProcessor<TPrestepData, TAccumulatedImpulse, TConstraintFunctions,
+        TWarmStartAccessFilterA, TWarmStartAccessFilterB, TWarmStartAccessFilterC, TWarmStartAccessFilterD, TSolveAccessFilterA, TSolveAccessFilterB, TSolveAccessFilterC, TSolveAccessFilterD>
+        : TypeProcessor<FourBodyReferences, TPrestepData, TAccumulatedImpulse>
+        where TPrestepData : unmanaged where TAccumulatedImpulse : unmanaged
+        where TConstraintFunctions : unmanaged, IFourBodyConstraintFunctions<TPrestepData, TAccumulatedImpulse>
+        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;
 
-        public sealed unsafe override void EnumerateConnectedBodyIndices<TEnumerator>(ref TypeBatch typeBatch, int indexInTypeBatch, ref TEnumerator enumerator)
-        {
-            BundleIndexing.GetBundleIndices(indexInTypeBatch, out var constraintBundleIndex, out var constraintInnerIndex);
-            ref var indices = ref GatherScatter.GetOffsetInstance(ref Buffer<FourBodyReferences>.Get(typeBatch.BodyReferences.Memory, constraintBundleIndex), constraintInnerIndex);
-            //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<FourBodyReferences>
         {
             [MethodImpl(MethodImplOptions.AggressiveInlining)]
-            public int GetSortKey(int constraintIndex, ref Buffer<FourBodyReferences> bodyReferences)
+            public static int GetSortKey(int constraintIndex, ref Buffer<FourBodyReferences> bodyReferences)
             {
                 BundleIndexing.GetBundleIndices(constraintIndex, out var bundleIndex, out var innerIndex);
                 ref var bundleReferences = ref bodyReferences[bundleIndex];
@@ -78,7 +88,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<byte> bodyReferencesCache)
         {
             GenerateSortKeysAndCopyReferences<FourBodySortKeyGenerator>(
                 ref typeBatch,
@@ -92,132 +102,91 @@ internal sealed override void VerifySortRegion(ref TypeBatch typeBatch, int bund
             VerifySortRegion<FourBodySortKeyGenerator>(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)
+        public override void WarmStart<TIntegratorCallbacks, TBatchIntegrationMode, TAllowPoseIntegration>(
+            ref TypeBatch typeBatch, ref Buffer<IndexSet> integrationFlags, Bodies bodies, ref TIntegratorCallbacks integratorCallbacks,
+            float dt, float inverseDt, int startBundle, int exclusiveEndBundle, int workerIndex)
         {
-            ref var prestepBase = ref Unsafe.AsRef<TPrestepData>(typeBatch.PrestepData.Memory);
-            ref var bodyReferencesBase = ref Unsafe.AsRef<FourBodyReferences>(typeBatch.BodyReferences.Memory);
-            ref var projectionBase = ref Unsafe.AsRef<TProjection>(typeBatch.Projection.Memory);
-            var function = default(TConstraintFunctions);
+            var prestepBundles = typeBatch.PrestepData.As<TPrestepData>();
+            var bodyReferencesBundles = typeBatch.BodyReferences.As<FourBodyReferences>();
+            var accumulatedImpulsesBundles = typeBatch.AccumulatedImpulses.As<TAccumulatedImpulse>();
             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.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);
-            }
-        }
+                ref var prestep = ref prestepBundles[i];
+                ref var accumulatedImpulses = ref accumulatedImpulsesBundles[i];
+                ref var references = ref bodyReferencesBundles[i];
+                GatherAndIntegrate<TIntegratorCallbacks, TBatchIntegrationMode, TWarmStartAccessFilterA, TAllowPoseIntegration>(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<TIntegratorCallbacks, TBatchIntegrationMode, TWarmStartAccessFilterB, TAllowPoseIntegration>(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<TIntegratorCallbacks, TBatchIntegrationMode, TWarmStartAccessFilterC, TAllowPoseIntegration>(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<TIntegratorCallbacks, TBatchIntegrationMode, TWarmStartAccessFilterD, TAllowPoseIntegration>(bodies, ref integratorCallbacks, ref integrationFlags, 3, dt, workerIndex, i, ref references.IndexD,
+                    out var positionD, out var orientationD, out var wsvD, out var inertiaD);
 
-        public unsafe override void WarmStart(ref TypeBatch typeBatch, ref Buffer<BodyVelocity> bodyVelocities, int startBundle, int exclusiveEndBundle)
-        {
-            ref var bodyReferencesBase = ref Unsafe.AsRef<FourBodyReferences>(typeBatch.BodyReferences.Memory);
-            ref var accumulatedImpulsesBase = ref Unsafe.AsRef<TAccumulatedImpulse>(typeBatch.AccumulatedImpulses.Memory);
-            ref var projectionBase = ref Unsafe.AsRef<TProjection>(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);
-                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);
-                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);
+                //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<float>(dt), accumulatedImpulses, ref prestep);
 
-            }
-        }
+                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);
 
-        public unsafe override void SolveIteration(ref TypeBatch typeBatch, ref Buffer<BodyVelocity> bodyVelocities, int startBundle, int exclusiveEndBundle)
-        {
-            ref var bodyReferencesBase = ref Unsafe.AsRef<FourBodyReferences>(typeBatch.BodyReferences.Memory);
-            ref var accumulatedImpulsesBase = ref Unsafe.AsRef<TAccumulatedImpulse>(typeBatch.AccumulatedImpulses.Memory);
-            ref var projectionBase = ref Unsafe.AsRef<TProjection>(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);
-                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);
-                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);
-            }
-        }
+                if (typeof(TBatchIntegrationMode) == typeof(BatchShouldNeverIntegrate))
+                {
+                    bodies.ScatterVelocities<TWarmStartAccessFilterA>(ref wsvA, ref references.IndexA);
+                    bodies.ScatterVelocities<TWarmStartAccessFilterB>(ref wsvB, ref references.IndexB);
+                    bodies.ScatterVelocities<TWarmStartAccessFilterC>(ref wsvC, ref references.IndexC);
+                    bodies.ScatterVelocities<TWarmStartAccessFilterD>(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<AccessAll>(ref wsvA, ref references.IndexA);
+                    bodies.ScatterVelocities<AccessAll>(ref wsvB, ref references.IndexB);
+                    bodies.ScatterVelocities<AccessAll>(ref wsvC, ref references.IndexC);
+                    bodies.ScatterVelocities<AccessAll>(ref wsvD, ref references.IndexD);
+                }
 
-        public unsafe override void JacobiPrestep(ref TypeBatch typeBatch, Bodies bodies, ref FallbackBatch jacobiBatch, float dt, float inverseDt, int startBundle, int exclusiveEndBundle)
-        {
-            ref var prestepBase = ref Unsafe.AsRef<TPrestepData>(typeBatch.PrestepData.Memory);
-            ref var bodyReferencesBase = ref Unsafe.AsRef<FourBodyReferences>(typeBatch.BodyReferences.Memory);
-            ref var projectionBase = ref Unsafe.AsRef<TProjection>(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.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 projection);
             }
         }
-        public unsafe override void JacobiWarmStart(ref TypeBatch typeBatch, ref Buffer<BodyVelocity> bodyVelocities, ref FallbackTypeBatchResults jacobiResults, int startBundle, int exclusiveEndBundle)
+
+        public override void Solve(ref TypeBatch typeBatch, Bodies bodies, float dt, float inverseDt, int startBundle, int exclusiveEndBundle)
         {
-            ref var bodyReferencesBase = ref Unsafe.AsRef<FourBodyReferences>(typeBatch.BodyReferences.Memory);
-            ref var accumulatedImpulsesBase = ref Unsafe.AsRef<TAccumulatedImpulse>(typeBatch.AccumulatedImpulses.Memory);
-            ref var projectionBase = ref Unsafe.AsRef<TProjection>(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);
+            var prestepBundles = typeBatch.PrestepData.As<TPrestepData>();
+            var bodyReferencesBundles = typeBatch.BodyReferences.As<FourBodyReferences>();
+            var accumulatedImpulsesBundles = typeBatch.AccumulatedImpulses.As<TAccumulatedImpulse>();
             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.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);
+                ref var prestep = ref prestepBundles[i];
+                ref var accumulatedImpulses = ref accumulatedImpulsesBundles[i];
+                ref var references = ref bodyReferencesBundles[i];
+                bodies.GatherState<TSolveAccessFilterA>(references.IndexA, true, out var positionA, out var orientationA, out var wsvA, out var inertiaA);
+                bodies.GatherState<TSolveAccessFilterB>(references.IndexB, true, out var positionB, out var orientationB, out var wsvB, out var inertiaB);
+                bodies.GatherState<TSolveAccessFilterC>(references.IndexC, true, out var positionC, out var orientationC, out var wsvC, out var inertiaC);
+                bodies.GatherState<TSolveAccessFilterD>(references.IndexD, true, out var positionD, out var orientationD, out var wsvD, out var inertiaD);
+
+                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<TSolveAccessFilterA>(ref wsvA, ref references.IndexA);
+                bodies.ScatterVelocities<TSolveAccessFilterB>(ref wsvB, ref references.IndexB);
+                bodies.ScatterVelocities<TSolveAccessFilterC>(ref wsvC, ref references.IndexC);
+                bodies.ScatterVelocities<TSolveAccessFilterD>(ref wsvD, ref references.IndexD);
             }
         }
-        public unsafe override void JacobiSolveIteration(ref TypeBatch typeBatch, ref Buffer<BodyVelocity> bodyVelocities, ref FallbackTypeBatchResults jacobiResults, 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)
         {
-            ref var bodyReferencesBase = ref Unsafe.AsRef<FourBodyReferences>(typeBatch.BodyReferences.Memory);
-            ref var accumulatedImpulsesBase = ref Unsafe.AsRef<TAccumulatedImpulse>(typeBatch.AccumulatedImpulses.Memory);
-            ref var projectionBase = ref Unsafe.AsRef<TProjection>(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);
+            var prestepBundles = typeBatch.PrestepData.As<TPrestepData>();
+            var bodyReferencesBundles = typeBatch.BodyReferences.As<FourBodyReferences>();
+            var dtWide = new Vector<float>(dt);
             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.GatherVelocities(ref bodyVelocities, ref bodyReferences, count, out wsvA, out wsvB, out wsvC, out wsvD);
-                function.Solve(ref wsvA, ref wsvB, ref wsvC, ref wsvD, ref projection, ref accumulatedImpulses);
+                ref var prestep = ref prestepBundles[i];
+                ref var references = ref bodyReferencesBundles[i];
+                bodies.GatherState<AccessOnlyVelocity>(references.IndexA, true, out _, out _, out var wsvA, out _);
+                bodies.GatherState<AccessOnlyVelocity>(references.IndexB, true, out _, out _, out var wsvB, out _);
+                bodies.GatherState<AccessOnlyVelocity>(references.IndexC, true, out _, out _, out var wsvC, out _);
+                bodies.GatherState<AccessOnlyVelocity>(references.IndexD, true, out _, out _, out var wsvD, out _);
+                TConstraintFunctions.IncrementallyUpdateForSubstep(dtWide, wsvA, wsvB, wsvC, wsvD, ref prestep);
             }
         }
 
diff --git a/BepuPhysics/Constraints/Hinge.cs b/BepuPhysics/Constraints/Hinge.cs
index 5e9ae27a1..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;
@@ -34,7 +33,7 @@ public struct Hinge : ITwoBodyConstraintDescription<Hinge>
         /// </summary>
         public SpringSettings SpringSettings;
 
-        public readonly int ConstraintTypeId
+        public static int ConstraintTypeId
         {
             [MethodImpl(MethodImplOptions.AggressiveInlining)]
             get
@@ -43,7 +42,8 @@ public readonly int ConstraintTypeId
             }
         }
 
-        public readonly Type TypeProcessorType => typeof(HingeTypeProcessor);
+        public static Type TypeProcessorType => typeof(HingeTypeProcessor);
+        public static TypeProcessor CreateTypeProcessor() => new HingeTypeProcessor();
 
         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 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<HingePrestepData>.Get(ref batch.PrestepData, bundleIndex), innerIndex);
@@ -80,35 +80,53 @@ 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<float> SoftnessImpulseScale;
-        public BodyInertias InertiaA;
-        public BodyInertias InertiaB;
-    }
-
     public struct HingeAccumulatedImpulses
     {
         public Vector3Wide BallSocket;
         public Vector2Wide Hinge;
     }
 
-    public struct HingeFunctions : IConstraintFunctions<HingePrestepData, HingeProjection, HingeAccumulatedImpulses>
+    public struct HingeFunctions : ITwoBodyConstraintFunctions<HingePrestepData, HingeAccumulatedImpulses>
     {
         [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)
+        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)
         {
-            bodies.GatherPose(ref bodyReferences, count, out var ab, out var orientationA, out var orientationB);
-            projection.InertiaA = inertiaA;
-            projection.InertiaB = inertiaB;
+            //[ 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 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);
+            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 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)    ]
             //[ 0, constraintAxisAX, 0, -constraintAxisAX ]
@@ -116,118 +134,94 @@ public void Prestep(Bodies bodies, ref TwoBodyReferences bodyReferences, int cou
 
             Matrix3x3Wide.CreateFromQuaternion(orientationA, out var orientationMatrixA);
             Matrix3x3Wide.CreateFromQuaternion(orientationB, out var orientationMatrixB);
-            Matrix3x3Wide.TransformWithoutOverlap(prestep.LocalOffsetA, orientationMatrixA, out projection.OffsetA);
+            Matrix3x3Wide.TransformWithoutOverlap(prestep.LocalOffsetA, orientationMatrixA, out var offsetA);
             Matrix3x3Wide.TransformWithoutOverlap(prestep.LocalHingeAxisA, orientationMatrixA, out var hingeAxisA);
-            Matrix3x3Wide.TransformWithoutOverlap(prestep.LocalOffsetB, orientationMatrixB, out projection.OffsetB);
+            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);
-            Matrix3x3Wide.TransformWithoutOverlap(localAX, orientationMatrixA, out projection.HingeJacobian.X);
-            Matrix3x3Wide.TransformWithoutOverlap(localAY, orientationMatrixA, out projection.HingeJacobian.Y);
+            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(projection.OffsetA, inertiaA.InverseInertiaTensor, out var ballSocketContributionAngularA);
-            Symmetric3x3Wide.SkewSandwichWithoutOverlap(projection.OffsetB, inertiaB.InverseInertiaTensor, out var ballSocketContributionAngularB);
+            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 = projection.InertiaA.InverseMass + projection.InertiaB.InverseMass;
+            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(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);
+            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, 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.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 projection.EffectiveMass);
-            SpringSettingsWide.ComputeSpringiness(prestep.SpringSettings, dt, out var positionErrorToVelocity, out var effectiveMassCFMScale, out projection.SoftnessImpulseScale);
-            Symmetric5x5Wide.Scale(projection.EffectiveMass, effectiveMassCFMScale, out projection.EffectiveMass);
+            //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.
 
             //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);
+            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, projection.HingeJacobian, out var errorAngles);
+            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 projection.HingeBiasVelocity);
-        }
-        
-        [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        private static void ApplyImpulse(ref BodyVelocities velocityA, ref BodyVelocities 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);
+            Vector2Wide.Scale(errorAngles, -positionErrorToVelocity, out var hingeBiasVelocity);
 
-            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 BodyVelocities velocityA, ref BodyVelocities 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)
-        {
             //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.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(velocityA.Linear, velocityB.Linear, out var ballSocketLinearCSV);
+            Vector3Wide.Subtract(wsvA.Linear, wsvB.Linear, out var ballSocketLinearCSV);
             Vector3Wide.Add(ballSocketAngularCSV, ballSocketLinearCSV, out var ballSocketCSV);
-            Vector3Wide.Subtract(projection.BallSocketBiasVelocity, ballSocketCSV, out ballSocketCSV);
+            Vector3Wide.Subtract(ballSocketBiasVelocity, ballSocketCSV, out ballSocketCSV);
             Vector2Wide.Subtract(hingeCSVA, negatedHingeCSVB, out var hingeCSV);
-            Vector2Wide.Subtract(projection.HingeBiasVelocity, hingeCSV, out hingeCSV);
+            Vector2Wide.Subtract(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);
+            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(accumulatedImpulse.Hinge, projection.SoftnessImpulseScale, out var hingeSoftnessContribution);
+            Vector2Wide.Scale(accumulatedImpulses.Hinge, softnessImpulseScale, out var hingeSoftnessContribution);
             Vector2Wide.Subtract(csi.Hinge, hingeSoftnessContribution, out csi.Hinge);
 
-            ApplyImpulse(ref velocityA, ref velocityB, ref projection, ref csi);
+            accumulatedImpulses.BallSocket += csi.BallSocket;
+            accumulatedImpulses.Hinge += csi.Hinge;
+
+            ApplyImpulse(offsetA, offsetB, hingeJacobian, inertiaA, inertiaB, csi, ref wsvA, ref wsvB);
         }
 
+        public static bool RequiresIncrementalSubstepUpdates => false;
+        [MethodImpl(MethodImplOptions.AggressiveInlining)]
+        public static void IncrementallyUpdateForSubstep(in Vector<float> dt, in BodyVelocityWide wsvA, in BodyVelocityWide wsvB, ref HingePrestepData prestepData) { }
     }
 
-    public class HingeTypeProcessor : TwoBodyTypeProcessor<HingePrestepData, HingeProjection, HingeAccumulatedImpulses, HingeFunctions>
+    public class HingeTypeProcessor : TwoBodyTypeProcessor<HingePrestepData, HingeAccumulatedImpulses, HingeFunctions, AccessNoPosition, AccessNoPosition, AccessAll, AccessAll>
     {
         public const int BatchTypeId = 47;
     }
diff --git a/BepuPhysics/Constraints/IBatchIntegrationMode.cs b/BepuPhysics/Constraints/IBatchIntegrationMode.cs
new file mode 100644
index 000000000..7a20b6f25
--- /dev/null
+++ b/BepuPhysics/Constraints/IBatchIntegrationMode.cs
@@ -0,0 +1,60 @@
+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.
+
+    /// <summary>
+    /// Marks a type as determining the integration mode for a solver batch.
+    /// </summary>
+    public interface IBatchIntegrationMode
+    {
+    }
+
+    /// <summary>
+    /// The batch was determined to have only constraints with integration responsibilities, so there's no need to check.
+    /// </summary>
+    public struct BatchShouldAlwaysIntegrate : IBatchIntegrationMode
+    {
+
+    }
+    /// <summary>
+    /// The batch was determined to have no constraints with integration responsibilities, so there's no need to check.
+    /// </summary>
+    public struct BatchShouldNeverIntegrate : IBatchIntegrationMode
+    {
+
+    }
+    /// <summary>
+    /// The batch was determined to have some constraints with integration responsibilities.
+    /// </summary>
+    public struct BatchShouldConditionallyIntegrate : IBatchIntegrationMode
+    {
+
+    }
+
+    /// <summary>
+    /// Marks a type as determining whether pose integration should be performed on bodies within the constraint batch.
+    /// </summary>
+    public interface IBatchPoseIntegrationAllowed
+    {
+
+    }
+
+    /// <summary>
+    /// 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.
+    /// </summary>
+    public struct AllowPoseIntegration : IBatchPoseIntegrationAllowed
+    {
+    }
+
+    /// <summary>
+    /// Marks a batch as not integrating poses for any bodies within the constraint batch.
+    /// </summary>
+    public struct DisallowPoseIntegration : IBatchPoseIntegrationAllowed
+    {
+    }
+
+}
diff --git a/BepuPhysics/Constraints/IBodyAccessFilter.cs b/BepuPhysics/Constraints/IBodyAccessFilter.cs
new file mode 100644
index 000000000..4a6abb168
--- /dev/null
+++ b/BepuPhysics/Constraints/IBodyAccessFilter.cs
@@ -0,0 +1,127 @@
+namespace BepuPhysics.Constraints
+{
+    /// <summary>
+    /// Constrains which body properties should be accessed in a body during constraint data gathering/scattering.
+    /// </summary>
+    public interface IBodyAccessFilter
+    {
+        /// <summary>
+        /// Gets whether position is loaded by the constraint.
+        /// </summary>
+        public bool GatherPosition { get; }
+        /// <summary>
+        /// Gets whether orientation is loaded by the constraint.
+        /// </summary>
+        public bool GatherOrientation { get; }
+        /// <summary>
+        /// Gets whether body mass is loaded by this constraint.
+        /// </summary>
+        public bool GatherMass { get; }
+        /// <summary>
+        /// Gets whether body inertia tensor is loaded by this constraint.
+        /// </summary>
+        public bool GatherInertiaTensor { get; }
+        /// <summary>
+        /// Gets whether to load or store body linear velocity in this constraint.
+        /// </summary>
+        public bool AccessLinearVelocity { get; }
+        /// <summary>
+        /// Gets whether to load or store body linear velocity in this constraint.
+        /// </summary>
+        public bool AccessAngularVelocity { get; }
+    }
+
+
+    /// <summary>
+    /// Marks all body properties as necessary for gather/scatter.
+    /// </summary>
+    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;
+    }
+
+    /// <summary>
+    /// Used for kinematic integration; the inertias are known ahead of time and there's no reason to gather them.
+    /// </summary>
+    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;
+        public bool GatherOrientation => false;
+        public bool GatherMass => true;
+        public bool GatherInertiaTensor => true;
+        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;
+        public bool GatherOrientation => false;
+        public bool GatherMass => true;
+        public bool GatherInertiaTensor => true;
+        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
+    {
+        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 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;
+        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/IConstraintDescription.cs b/BepuPhysics/Constraints/IConstraintDescription.cs
index b3a3e67c7..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
 {
@@ -32,16 +28,20 @@ public interface IConstraintDescription<TDescription>
         /// <param name="bundleIndex">Index of the source constraint's bundle.</param>
         /// <param name="innerIndex">Index of the source constraint within its bundle.</param>
         /// <param name="description">Description of the constraint.</param>
-        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);
         
         /// <summary>
         /// Gets the type id of the constraint that this is a description of.
         /// </summary>
-        int ConstraintTypeId { get; }
+        static abstract int ConstraintTypeId { get; }
         /// <summary>
         /// Gets the type of the type batch which contains described constraints.
         /// </summary>
-        Type TypeProcessorType  { get; }
+        static abstract Type TypeProcessorType  { get; }
+        /// <summary>
+        /// Creates a type processor for this constraint type.
+        /// </summary>
+        static abstract TypeProcessor CreateTypeProcessor();
     }
 
     /// <summary>
diff --git a/BepuPhysics/Constraints/Inequality1DOF.cs b/BepuPhysics/Constraints/Inequality1DOF.cs
deleted file mode 100644
index 9deb29527..000000000
--- a/BepuPhysics/Constraints/Inequality1DOF.cs
+++ /dev/null
@@ -1,374 +0,0 @@
-using BepuUtilities;
-using System;
-using System.Numerics;
-using System.Runtime.CompilerServices;
-using System.Runtime.InteropServices;
-
-namespace BepuPhysics.Constraints
-{
- 
-    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<float> BiasImpulse;
-        //And once again, CFM becomes CFM * EffectiveMass- massively cancels out due to the derivation of CFM. (See prestep notes.)
-        public Vector<float> 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 BodyInertias inertiaA, ref BodyInertias inertiaB, ref TwoBody1DOFJacobians jacobians, ref SpringSettingsWide springSettings, ref Vector<float> maximumRecoveryVelocity,
-            ref Vector<float> 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<float>.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
-
-
-        /// <summary>
-        /// Transforms an impulse from constraint space to world space, uses it to modify the cached world space velocities of the bodies.
-        /// </summary>
-        [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public static void ApplyImpulse(ref Projection2Body1DOF data, ref Vector<float> correctiveImpulse,
-            ref BodyVelocities wsvA, ref BodyVelocities 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;
-            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<float> accumulatedImpulse, ref BodyVelocities wsvA, ref BodyVelocities 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 BodyVelocities wsvA, ref BodyVelocities wsvB, ref Projection2Body1DOF projection, ref Vector<float> accumulatedImpulse,
-            out Vector<float> 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<float>.Zero, accumulatedImpulse + csi);
-
-            correctiveCSI = accumulatedImpulse - previousAccumulated;
-
-        }
-
-        [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public static void Solve(ref Projection2Body1DOF projection, ref Vector<float> accumulatedImpulse, ref BodyVelocities wsvA, ref BodyVelocities 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/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 0bd08f21e..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;
@@ -38,7 +37,7 @@ public struct LinearAxisLimit : ITwoBodyConstraintDescription<LinearAxisLimit>
         /// </summary>
         public SpringSettings SpringSettings;
 
-        public readonly int ConstraintTypeId
+        public static int ConstraintTypeId
         {
             [MethodImpl(MethodImplOptions.AggressiveInlining)]
             get
@@ -47,7 +46,8 @@ public readonly int ConstraintTypeId
             }
         }
 
-        public readonly Type TypeProcessorType => typeof(LinearAxisLimitTypeProcessor);
+        public static Type TypeProcessorType => typeof(LinearAxisLimitTypeProcessor);
+        public static TypeProcessor CreateTypeProcessor() => new LinearAxisLimitTypeProcessor();
 
         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 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<LinearAxisLimitPrestepData>.Get(ref batch.PrestepData, bundleIndex), innerIndex);
@@ -87,71 +87,70 @@ public struct LinearAxisLimitPrestepData
         public SpringSettingsWide SpringSettings;
     }
 
-    public struct LinearAxisLimitFunctions : IConstraintFunctions<LinearAxisLimitPrestepData, LinearAxisServoProjection, Vector<float>>
+    public struct LinearAxisLimitFunctions : ITwoBodyConstraintFunctions<LinearAxisLimitPrestepData, Vector<float>>
     {
-        public struct LimitJacobianModifier : LinearAxisServoFunctions.IJacobianModifier
+        [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<float> minimumOffset, in Vector<float> maximumOffset,
+            out Vector<float> error, out Vector3Wide normal, out Vector3Wide angularJA, out Vector3Wide angularJB)
         {
-            public Vector<float> MinimumOffset;
-            public Vector<float> MaximumOffset;
-            public Vector<float> 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);
+            //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);
 
-                var minimumError = MinimumOffset - planeNormalDot;
-                var maximumError = planeNormalDot - MaximumOffset;
-                var useMin = Vector.LessThan(Vector.Abs(minimumError), Vector.Abs(maximumError));
+            //The limit chooses the normal's sign depending on which limit is closer.
+            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);
 
-                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. 
+            var offsetFromAToClosetPointOnPlaneToB = anchorB - planeNormalDot * normal;
+            Vector3Wide.CrossWithoutOverlap(offsetFromAToClosetPointOnPlaneToB, normal, out angularJA);
+            Vector3Wide.CrossWithoutOverlap(normal, offsetB, out angularJB);
         }
 
-        [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 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<float> accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB)
         {
-            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, bodies, ref bodyReferences, count, prestep.LocalOffsetA, prestep.LocalOffsetB, prestep.LocalPlaneNormal, inertiaA, 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;
+            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);
         }
 
-        [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public void WarmStart(ref BodyVelocities velocityA, ref BodyVelocities velocityB, ref LinearAxisServoProjection projection, ref Vector<float> accumulatedImpulse)
+        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<float> accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB)
         {
-            LinearAxisServoFunctions.ApplyImpulse(ref velocityA, ref velocityB,
-                projection.LinearImpulseToVelocityA, projection.AngularImpulseToVelocityA, projection.NegatedLinearImpulseToVelocityB, projection.AngularImpulseToVelocityB,
-                ref accumulatedImpulse);
-        }
+            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);
 
-        [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public void Solve(ref BodyVelocities velocityA, ref BodyVelocities velocityB, ref LinearAxisServoProjection projection, ref Vector<float> 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);
+            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);
+
+            //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);
         }
 
+        public static bool RequiresIncrementalSubstepUpdates => false;
+        [MethodImpl(MethodImplOptions.AggressiveInlining)]
+        public static void IncrementallyUpdateForSubstep(in Vector<float> dt, in BodyVelocityWide wsvA, in BodyVelocityWide wsvB, ref LinearAxisLimitPrestepData prestepData) { }
     }
 
-    public class LinearAxisLimitTypeProcessor : TwoBodyTypeProcessor<LinearAxisLimitPrestepData, LinearAxisServoProjection, Vector<float>, LinearAxisLimitFunctions>
+    public class LinearAxisLimitTypeProcessor : TwoBodyTypeProcessor<LinearAxisLimitPrestepData, Vector<float>, LinearAxisLimitFunctions, AccessAll, AccessAll, AccessAll, AccessAll>
     {
         public const int BatchTypeId = 40;
     }
diff --git a/BepuPhysics/Constraints/LinearAxisMotor.cs b/BepuPhysics/Constraints/LinearAxisMotor.cs
index 5cb6d4069..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;
@@ -34,7 +33,7 @@ public struct LinearAxisMotor : ITwoBodyConstraintDescription<LinearAxisMotor>
         /// </summary>
         public MotorSettings Settings;
 
-        public readonly int ConstraintTypeId
+        public static int ConstraintTypeId
         {
             [MethodImpl(MethodImplOptions.AggressiveInlining)]
             get
@@ -43,7 +42,8 @@ public readonly int ConstraintTypeId
             }
         }
 
-        public readonly Type TypeProcessorType => typeof(LinearAxisMotorTypeProcessor);
+        public static Type TypeProcessorType => typeof(LinearAxisMotorTypeProcessor);
+        public static TypeProcessor CreateTypeProcessor() => new LinearAxisMotorTypeProcessor();
 
         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
             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<LinearAxisMotorPrestepData>.Get(ref batch.PrestepData, bundleIndex), innerIndex);
@@ -78,45 +78,38 @@ public struct LinearAxisMotorPrestepData
         public Vector<float> TargetVelocity;
         public MotorSettingsWide Settings;
     }
-    
-    public struct LinearAxisMotorFunctions : IConstraintFunctions<LinearAxisMotorPrestepData, LinearAxisServoProjection, Vector<float>>
+
+    public struct LinearAxisMotorFunctions : ITwoBodyConstraintFunctions<LinearAxisMotorPrestepData, Vector<float>>
     {
-        [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)
-        {
-            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,
-                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 BodyVelocities velocityA, ref BodyVelocities velocityB, ref LinearAxisServoProjection projection, ref Vector<float> accumulatedImpulse)
+        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<float> accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB)
         {
-            LinearAxisServoFunctions.ApplyImpulse(ref velocityA, ref velocityB,
-                projection.LinearImpulseToVelocityA, projection.AngularImpulseToVelocityA, projection.NegatedLinearImpulseToVelocityB, projection.AngularImpulseToVelocityB,
-                ref accumulatedImpulse);
+            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);
         }
-        
-        [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public void Solve(ref BodyVelocities velocityA, ref BodyVelocities velocityB, ref LinearAxisServoProjection projection, ref Vector<float> accumulatedImpulse)
+
+        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<float> accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB)
         {
-            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);
+            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);
+
+            //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);
         }
 
+        public static bool RequiresIncrementalSubstepUpdates => false;
+        [MethodImpl(MethodImplOptions.AggressiveInlining)]
+        public static void IncrementallyUpdateForSubstep(in Vector<float> dt, in BodyVelocityWide wsvA, in BodyVelocityWide wsvB, ref LinearAxisMotorPrestepData prestepData) { }
     }
 
-    public class LinearAxisMotorTypeProcessor : TwoBodyTypeProcessor<LinearAxisMotorPrestepData, LinearAxisServoProjection, Vector<float>, LinearAxisMotorFunctions>
+    public class LinearAxisMotorTypeProcessor : TwoBodyTypeProcessor<LinearAxisMotorPrestepData, Vector<float>, LinearAxisMotorFunctions, AccessAll, AccessAll, AccessAll, AccessAll>
     {
         public const int BatchTypeId = 39;
     }
diff --git a/BepuPhysics/Constraints/LinearAxisServo.cs b/BepuPhysics/Constraints/LinearAxisServo.cs
index 4b46a458d..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;
@@ -38,7 +37,7 @@ public struct LinearAxisServo : ITwoBodyConstraintDescription<LinearAxisServo>
         /// </summary>
         public SpringSettings SpringSettings;
 
-        public readonly int ConstraintTypeId
+        public static int ConstraintTypeId
         {
             [MethodImpl(MethodImplOptions.AggressiveInlining)]
             get
@@ -47,7 +46,8 @@ public readonly int ConstraintTypeId
             }
         }
 
-        public readonly Type TypeProcessorType => typeof(LinearAxisServoTypeProcessor);
+        public static Type TypeProcessorType => typeof(LinearAxisServoTypeProcessor);
+        public static TypeProcessor CreateTypeProcessor() => new LinearAxisServoTypeProcessor();
 
         public readonly void ApplyDescription(ref TypeBatch batch, int bundleIndex, int innerIndex)
         {
@@ -63,7 +63,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<LinearAxisServoPrestepData>.Get(ref batch.PrestepData, bundleIndex), innerIndex);
@@ -86,21 +86,7 @@ public struct LinearAxisServoPrestepData
         public SpringSettingsWide SpringSettings;
     }
 
-    public struct LinearAxisServoProjection
-    {
-        public Vector3Wide LinearVelocityToImpulseA;
-        public Vector3Wide AngularVelocityToImpulseA;
-        public Vector3Wide AngularVelocityToImpulseB;
-        public Vector<float> BiasImpulse;
-        public Vector<float> SoftnessImpulseScale;
-        public Vector<float> MaximumImpulse;
-        public Vector3Wide LinearImpulseToVelocityA;
-        public Vector3Wide NegatedLinearImpulseToVelocityB;
-        public Vector3Wide AngularImpulseToVelocityA;
-        public Vector3Wide AngularImpulseToVelocityB;
-    }
-
-    public struct LinearAxisServoFunctions : IConstraintFunctions<LinearAxisServoPrestepData, LinearAxisServoProjection, Vector<float>>
+    public struct LinearAxisServoFunctions : ITwoBodyConstraintFunctions<LinearAxisServoPrestepData, Vector<float>>
     {
         public interface IJacobianModifier
         {
@@ -116,9 +102,10 @@ public void Modify(in Vector3Wide anchorA, in Vector3Wide anchorB, ref Vector3Wi
         }
 
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public static void ComputeTransforms<TJacobianModifier>(ref TJacobianModifier jacobianModifier, Bodies bodies, ref TwoBodyReferences bodyReferences, int count,
+        public static void ComputeTransforms<TJacobianModifier>(ref TJacobianModifier jacobianModifier,
             in Vector3Wide localOffsetA, in Vector3Wide localOffsetB, in Vector3Wide localPlaneNormal,
-            in BodyInertias inertiaA, in BodyInertias inertiaB, in Vector<float> effectiveMassCFMScale,
+            in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertiaWide inertiaB,
+            in Vector<float> effectiveMassCFMScale,
             out Vector3Wide anchorA, out Vector3Wide anchorB, out Vector3Wide normal, out Vector<float> effectiveMass,
             out Vector3Wide linearVelocityToImpulseA, out Vector3Wide angularVelocityToImpulseA, out Vector3Wide angularVelocityToImpulseB,
             out Vector3Wide linearImpulseToVelocityA, out Vector3Wide angularImpulseToVelocityA, out Vector3Wide negatedLinearImpulseToVelocityB, out Vector3Wide angularImpulseToVelocityB)
@@ -132,7 +119,6 @@ public static void ComputeTransforms<TJacobianModifier>(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);
@@ -160,28 +146,9 @@ public static void ComputeTransforms<TJacobianModifier>(ref TJacobianModifier ja
             Vector3Wide.Scale(angularB, effectiveMass, out angularVelocityToImpulseB);
         }
 
-        [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)
-        {
-            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,
-                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 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<float> csi)
         {
             Vector3Wide.Scale(linearImpulseToVelocityA, csi, out var linearChangeA);
@@ -195,15 +162,7 @@ public static void ApplyImpulse(ref BodyVelocities velocityA, ref BodyVelocities
             Vector3Wide.Add(angularChangeB, velocityB.Angular, out velocityB.Angular);
         }
 
-        [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public void WarmStart(ref BodyVelocities velocityA, ref BodyVelocities velocityB, ref LinearAxisServoProjection projection, ref Vector<float> 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<float> biasImpulse, in Vector<float> softnessImpulseScale, in Vector<float> accumulatedImpulse, out Vector<float> csi)
         {
@@ -216,20 +175,79 @@ public static void ComputeCorrectiveImpulse(ref BodyVelocities velocityA, ref Bo
             csi = biasImpulse - accumulatedImpulse * softnessImpulseScale - (linearA + angularA - negatedLinearB + angularB);
         }
 
+
+        [MethodImpl(MethodImplOptions.AggressiveInlining)]
+        public static void ApplyImpulse(in Vector3Wide linearJA, in Vector3Wide angularImpulseToVelocityA, in Vector3Wide angularImpulseToVelocityB, in BodyInertiaWide inertiaA, in BodyInertiaWide inertiaB,
+            in Vector<float> 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 localOffsetA, in Vector3Wide localOffsetB,
+            out Vector<float> 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.
+            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. 
+            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);
+        }
+
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public void Solve(ref BodyVelocities velocityA, ref BodyVelocities velocityB, ref LinearAxisServoProjection projection, ref Vector<float> accumulatedImpulse)
+        public static void ComputeEffectiveMass(in Vector3Wide angularJA, in Vector3Wide angularJB,
+            in BodyInertiaWide inertiaA, in BodyInertiaWide inertiaB,
+            in Vector<float> effectiveMassCFMScale, out Vector3Wide angularImpulseToVelocityA, out Vector3Wide angularImpulseToVelocityB, out Vector<float> 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);
+            effectiveMass = effectiveMassCFMScale / (inertiaA.InverseMass + inertiaB.InverseMass + angularContributionA + angularContributionB);
+        }
+
+        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<float> 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);
+            Symmetric3x3Wide.TransformWithoutOverlap(angularJB, inertiaB.InverseInertiaTensor, out var angularImpulseToVelocityB);
+            ApplyImpulse(normal, angularImpulseToVelocityA, angularImpulseToVelocityB, inertiaA, inertiaB, accumulatedImpulses, ref wsvA, ref 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<float> accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB)
         {
-            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);
+            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);
+
+
+            //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);
         }
 
+        public static bool RequiresIncrementalSubstepUpdates => false;
+        [MethodImpl(MethodImplOptions.AggressiveInlining)]
+        public static void IncrementallyUpdateForSubstep(in Vector<float> dt, in BodyVelocityWide wsvA, in BodyVelocityWide wsvB, ref LinearAxisServoPrestepData prestepData) { }
     }
 
-    public class LinearAxisServoTypeProcessor : TwoBodyTypeProcessor<LinearAxisServoPrestepData, LinearAxisServoProjection, Vector<float>, LinearAxisServoFunctions>
+    public class LinearAxisServoTypeProcessor : TwoBodyTypeProcessor<LinearAxisServoPrestepData, Vector<float>, LinearAxisServoFunctions, AccessAll, AccessAll, AccessAll, AccessAll>
     {
         public const int BatchTypeId = 38;
     }
diff --git a/BepuPhysics/Constraints/MotorSettings.cs b/BepuPhysics/Constraints/MotorSettings.cs
index 561998ca1..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
 {
@@ -93,10 +90,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<float>(dt);
+            var dtd = dtWide * settings.Damping;
+            maximumImpulse = settings.MaximumForce * dtWide;
             softnessImpulseScale = Vector<float>.One / (dtd + Vector<float>.One);
             effectiveMassCFMScale = dtd * softnessImpulseScale;
-            maximumImpulse = settings.MaximumForce * dt;
         }
     }
 }
diff --git a/BepuPhysics/Constraints/OneBodyAngularMotor.cs b/BepuPhysics/Constraints/OneBodyAngularMotor.cs
index d467d3f57..366cd120c 100644
--- a/BepuPhysics/Constraints/OneBodyAngularMotor.cs
+++ b/BepuPhysics/Constraints/OneBodyAngularMotor.cs
@@ -22,7 +22,7 @@ public struct OneBodyAngularMotor : IOneBodyConstraintDescription<OneBodyAngular
         /// </summary>
         public MotorSettings Settings;
 
-        public readonly int ConstraintTypeId
+        public static int ConstraintTypeId
         {
             [MethodImpl(MethodImplOptions.AggressiveInlining)]
             get
@@ -31,7 +31,8 @@ public readonly int ConstraintTypeId
             }
         }
 
-        public readonly Type TypeProcessorType => typeof(OneBodyAngularMotorTypeProcessor);
+        public static Type TypeProcessorType => typeof(OneBodyAngularMotorTypeProcessor);
+        public static TypeProcessor CreateTypeProcessor() => new OneBodyAngularMotorTypeProcessor();
 
         public readonly void ApplyDescription(ref TypeBatch batch, int bundleIndex, int innerIndex)
         {
@@ -42,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<OneBodyAngularMotorPrestepData>.Get(ref batch.PrestepData, bundleIndex), innerIndex);
@@ -57,23 +58,8 @@ public struct OneBodyAngularMotorPrestepData
         public MotorSettingsWide Settings;
     }
 
-    public struct OneBodyAngularMotorFunctions : IOneBodyConstraintFunctions<OneBodyAngularMotorPrestepData, OneBodyAngularServoProjection, Vector3Wide>
+    public struct OneBodyAngularMotorFunctions : IOneBodyConstraintFunctions<OneBodyAngularMotorPrestepData, Vector3Wide>
     {
-        [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public void Prestep(Bodies bodies, ref Vector<int> bodyReferences, int count, float dt, float inverseDt, ref BodyInertias inertiaA,
-            ref OneBodyAngularMotorPrestepData prestep, out OneBodyAngularServoProjection projection)
-        {
-            bodies.GatherOrientation(ref bodyReferences, count, out var orientationA);
-            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)
         {
@@ -81,38 +67,32 @@ public static void ApplyImpulse(ref Vector3Wide angularVelocity, in Symmetric3x3
             Vector3Wide.Add(angularVelocity, velocityChange, out angularVelocity);
         }
 
-        [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public void WarmStart(ref BodyVelocities velocityA, ref OneBodyAngularServoProjection projection, ref Vector3Wide accumulatedImpulse)
+        public static void WarmStart(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, ref OneBodyAngularMotorPrestepData prestep, ref Vector3Wide accumulatedImpulses, ref BodyVelocityWide wsvA)
         {
-            ApplyImpulse(ref velocityA.Angular, projection.ImpulseToVelocity, accumulatedImpulse);
+            ApplyImpulse(ref wsvA.Angular, inertiaA.InverseInertiaTensor, accumulatedImpulses);
         }
 
-        [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public static void Solve(ref BodyVelocities velocityA,
-            in Symmetric3x3Wide effectiveMass, in Vector<float> softnessImpulseScale, in Vector3Wide biasImpulse, in Vector<float> maximumImpulse,
-            in Symmetric3x3Wide impulseToVelocityA, ref Vector3Wide accumulatedImpulse)
+        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 I.
-            Symmetric3x3Wide.TransformWithoutOverlap(velocityA.Angular, effectiveMass, out var csiVelocityComponent);
+            //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;
-            Vector3Wide.Scale(accumulatedImpulse, softnessImpulseScale, out var softnessComponent);
-            Vector3Wide.Subtract(biasImpulse, softnessComponent, out var csi);
-            Vector3Wide.Subtract(csi, csiVelocityComponent, out csi);
+            Symmetric3x3Wide.TransformWithoutOverlap(prestep.TargetVelocity - wsvA.Angular, unsoftenedEffectiveMass, out var csi);
+            csi = csi * effectiveMassCFMScale - accumulatedImpulses * softnessImpulseScale;
 
-            ServoSettingsWide.ClampImpulse(maximumImpulse, ref accumulatedImpulse, ref csi);            
+            ServoSettingsWide.ClampImpulse(maximumImpulse, ref accumulatedImpulses, ref csi);
 
-            ApplyImpulse(ref velocityA.Angular, impulseToVelocityA, csi);
+            ApplyImpulse(ref wsvA.Angular, inertiaA.InverseInertiaTensor, csi);
         }
 
+        public static bool RequiresIncrementalSubstepUpdates => false;
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public void Solve(ref BodyVelocities velocityA, ref OneBodyAngularServoProjection projection, ref Vector3Wide accumulatedImpulse)
-        {
-            Solve(ref velocityA, projection.VelocityToImpulse, projection.SoftnessImpulseScale, projection.BiasImpulse,
-                projection.MaximumImpulse, projection.ImpulseToVelocity, ref accumulatedImpulse);
-        }
+        public static void IncrementallyUpdateForSubstep(in Vector<float> dt, in BodyVelocityWide wsvA, ref OneBodyAngularMotorPrestepData prestepData) { }
     }
 
-    public class OneBodyAngularMotorTypeProcessor : OneBodyTypeProcessor<OneBodyAngularMotorPrestepData, OneBodyAngularServoProjection, Vector3Wide, OneBodyAngularMotorFunctions>
+    public class OneBodyAngularMotorTypeProcessor : OneBodyTypeProcessor<OneBodyAngularMotorPrestepData, Vector3Wide, OneBodyAngularMotorFunctions, AccessOnlyAngularWithoutPose, AccessOnlyAngular>
     {
         public const int BatchTypeId = 43;
     }
diff --git a/BepuPhysics/Constraints/OneBodyAngularServo.cs b/BepuPhysics/Constraints/OneBodyAngularServo.cs
index c9abf2705..7b04aa38c 100644
--- a/BepuPhysics/Constraints/OneBodyAngularServo.cs
+++ b/BepuPhysics/Constraints/OneBodyAngularServo.cs
@@ -26,7 +26,7 @@ public struct OneBodyAngularServo : IOneBodyConstraintDescription<OneBodyAngular
         /// </summary>
         public ServoSettings ServoSettings;
 
-        public readonly int ConstraintTypeId
+        public static int ConstraintTypeId
         {
             [MethodImpl(MethodImplOptions.AggressiveInlining)]
             get
@@ -35,7 +35,8 @@ public readonly int ConstraintTypeId
             }
         }
 
-        public readonly Type TypeProcessorType => typeof(OneBodyAngularServoTypeProcessor);
+        public static Type TypeProcessorType => typeof(OneBodyAngularServoTypeProcessor);
+        public static TypeProcessor CreateTypeProcessor() => new OneBodyAngularServoTypeProcessor();
 
         public readonly void ApplyDescription(ref TypeBatch batch, int bundleIndex, int innerIndex)
         {
@@ -48,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<OneBodyAngularServoPrestepData>.Get(ref batch.PrestepData, bundleIndex), innerIndex);
@@ -65,78 +66,49 @@ public struct OneBodyAngularServoPrestepData
         public ServoSettingsWide ServoSettings;
     }
 
-    public struct OneBodyAngularServoProjection
+    public struct OneBodyAngularServoFunctions : IOneBodyConstraintFunctions<OneBodyAngularServoPrestepData, Vector3Wide>
     {
-        public Symmetric3x3Wide VelocityToImpulse;
-        public Vector3Wide BiasImpulse;
-        public Vector<float> SoftnessImpulseScale;
-        public Vector<float> MaximumImpulse;
-        public Symmetric3x3Wide ImpulseToVelocity;
-    }
-
-    public struct OneBodyAngularServoFunctions : IOneBodyConstraintFunctions<OneBodyAngularServoPrestepData, OneBodyAngularServoProjection, Vector3Wide>
-    {
-        [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public void Prestep(Bodies bodies, ref Vector<int> bodyReferences, int count, float dt, float inverseDt, ref BodyInertias inertiaA,
-            ref OneBodyAngularServoPrestepData prestep, out OneBodyAngularServoProjection projection)
-        {
-            bodies.GatherOrientation(ref bodyReferences, count, out var orientationA);
-            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)
+        public static void ApplyImpulse(in Symmetric3x3Wide inverseInertia, in Vector3Wide csi, ref Vector3Wide angularVelocity)
         {
-            Symmetric3x3Wide.TransformWithoutOverlap(csi, impulseToVelocity, out var velocityChange);
+            Symmetric3x3Wide.TransformWithoutOverlap(csi, inverseInertia, out var velocityChange);
             Vector3Wide.Add(angularVelocity, velocityChange, out angularVelocity);
         }
 
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public void WarmStart(ref BodyVelocities velocityA, ref OneBodyAngularServoProjection projection, ref Vector3Wide accumulatedImpulse)
+        public static void WarmStart(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, ref OneBodyAngularServoPrestepData prestep, ref Vector3Wide accumulatedImpulses, ref BodyVelocityWide wsvA)
         {
-            ApplyImpulse(ref velocityA.Angular, projection.ImpulseToVelocity, accumulatedImpulse);
+            ApplyImpulse(inertiaA.InverseInertiaTensor, accumulatedImpulses, ref wsvA.Angular);
         }
 
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public static void Solve(ref BodyVelocities velocityA,
-            in Symmetric3x3Wide effectiveMass, in Vector<float> softnessImpulseScale, in Vector3Wide biasImpulse, in Vector<float> maximumImpulse,
-            in Symmetric3x3Wide impulseToVelocityA, ref Vector3Wide accumulatedImpulse)
+        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 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);
+            //Jacobians are just the identity matrix.
+            QuaternionWide.Conjugate(orientationA, out var inverseOrientation);
+            QuaternionWide.ConcatenateWithoutOverlap(inverseOrientation, prestep.TargetOrientation, out var errorRotation);
+            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);
+
+            ServoSettingsWide.ComputeClampedBiasVelocity(errorAxis, errorLength, positionErrorToVelocity, prestep.ServoSettings, dt, inverseDt, out var clampedBiasVelocity, out var maximumImpulse);
 
-            ServoSettingsWide.ClampImpulse(maximumImpulse, ref accumulatedImpulse, ref csi);
+            //csi = projection.BiasImpulse - accumulatedImpulse * projection.SoftnessImpulseScale - csiaAngular;
+            var csv = clampedBiasVelocity - wsvA.Angular;
+            Symmetric3x3Wide.TransformWithoutOverlap(csv, effectiveMass, out var csi);
+            csi = csi * effectiveMassCFMScale - accumulatedImpulses * softnessImpulseScale;
 
-            ApplyImpulse(ref velocityA.Angular, impulseToVelocityA, csi);
+            ServoSettingsWide.ClampImpulse(maximumImpulse, ref accumulatedImpulses, ref csi);
+            ApplyImpulse(inertiaA.InverseInertiaTensor, csi, ref wsvA.Angular);
         }
 
+        public static bool RequiresIncrementalSubstepUpdates => false;
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public void Solve(ref BodyVelocities velocityA, ref OneBodyAngularServoProjection projection, ref Vector3Wide accumulatedImpulse)
-        {
-            Solve(ref velocityA, projection.VelocityToImpulse, projection.SoftnessImpulseScale, projection.BiasImpulse,
-                projection.MaximumImpulse, projection.ImpulseToVelocity, ref accumulatedImpulse);
-        }
+        public static void IncrementallyUpdateForSubstep(in Vector<float> dt, in BodyVelocityWide wsvA, ref OneBodyAngularServoPrestepData prestepData) { }
     }
 
-    public class OneBodyAngularServoTypeProcessor : OneBodyTypeProcessor<OneBodyAngularServoPrestepData, OneBodyAngularServoProjection, Vector3Wide, OneBodyAngularServoFunctions>
+    public class OneBodyAngularServoTypeProcessor : OneBodyTypeProcessor<OneBodyAngularServoPrestepData, Vector3Wide, OneBodyAngularServoFunctions, AccessOnlyAngular, AccessOnlyAngular>
     {
         public const int BatchTypeId = 42;
     }
diff --git a/BepuPhysics/Constraints/OneBodyLinearMotor.cs b/BepuPhysics/Constraints/OneBodyLinearMotor.cs
index 2af8b66a4..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;
@@ -26,7 +25,7 @@ public struct OneBodyLinearMotor : IOneBodyConstraintDescription<OneBodyLinearMo
         /// </summary>
         public MotorSettings Settings;
 
-        public readonly int ConstraintTypeId
+        public static int ConstraintTypeId
         {
             [MethodImpl(MethodImplOptions.AggressiveInlining)]
             get
@@ -35,7 +34,8 @@ public readonly int ConstraintTypeId
             }
         }
 
-        public readonly Type TypeProcessorType => typeof(OneBodyLinearMotorTypeProcessor);
+        public static Type TypeProcessorType => typeof(OneBodyLinearMotorTypeProcessor);
+        public static TypeProcessor CreateTypeProcessor() => new OneBodyLinearMotorTypeProcessor();
 
         public readonly void ApplyDescription(ref TypeBatch batch, int bundleIndex, int innerIndex)
         {
@@ -47,7 +47,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<OneBodyLinearMotorPrestepData>.Get(ref batch.PrestepData, bundleIndex), innerIndex);
@@ -64,38 +64,42 @@ public struct OneBodyLinearMotorPrestepData
         public MotorSettingsWide Settings;
     }
 
-    public struct OneBodyLinearMotorFunctions : IOneBodyConstraintFunctions<OneBodyLinearMotorPrestepData, OneBodyLinearServoProjection, Vector3Wide>
+    public struct OneBodyLinearMotorFunctions : IOneBodyConstraintFunctions<OneBodyLinearMotorPrestepData, Vector3Wide>
     {
-        [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public void Prestep(Bodies bodies, ref Vector<int> bodyReferences, int count, float dt, float inverseDt, ref BodyInertias inertia, ref OneBodyLinearMotorPrestepData prestep,
-            out OneBodyLinearServoProjection projection)
+        public static void WarmStart(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, ref OneBodyLinearMotorPrestepData prestep, ref Vector3Wide accumulatedImpulses, ref BodyVelocityWide wsvA)
         {
-            //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;
+            QuaternionWide.TransformWithoutOverlap(prestep.LocalOffset, orientationA, out var offset);
+            OneBodyLinearServoFunctions.ApplyImpulse(offset, inertiaA, ref wsvA, accumulatedImpulses);
+        }
 
-            MotorSettingsWide.ComputeSoftness(prestep.Settings, dt, out var effectiveMassCFMScale, out projection.SoftnessImpulseScale, out projection.MaximumImpulse);
+        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);
 
-            OneBodyLinearServoFunctions.ComputeTransforms(prestep.LocalOffset, orientation, effectiveMassCFMScale, inertia, out projection.Offset, out projection.EffectiveMass);
-            projection.BiasVelocity = prestep.TargetVelocity;
-        }
+            //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);
 
-        [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public void WarmStart(ref BodyVelocities velocityA, ref OneBodyLinearServoProjection projection, ref Vector3Wide accumulatedImpulse)
-        {
-            OneBodyLinearServoFunctions.ApplyImpulse(ref velocityA, projection, ref accumulatedImpulse);
-        }
+            //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;
 
-        [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public void Solve(ref BodyVelocities velocityA, ref OneBodyLinearServoProjection projection, ref Vector3Wide accumulatedImpulse)
-        {
-            OneBodyLinearServoFunctions.SharedSolve(ref velocityA, projection, ref accumulatedImpulse);
+            ServoSettingsWide.ClampImpulse(maximumImpulse, ref accumulatedImpulses, ref csi);
+            OneBodyLinearServoFunctions.ApplyImpulse(offset, inertiaA, ref wsvA, csi);
         }
-
+        
+        public static bool RequiresIncrementalSubstepUpdates => false;
+        [MethodImpl(MethodImplOptions.AggressiveInlining)]
+        public static void IncrementallyUpdateForSubstep(in Vector<float> dt, in BodyVelocityWide wsvA, ref OneBodyLinearMotorPrestepData prestepData) { }
     }
-
-    public class OneBodyLinearMotorTypeProcessor : OneBodyTypeProcessor<OneBodyLinearMotorPrestepData, OneBodyLinearServoProjection, Vector3Wide, OneBodyLinearMotorFunctions>
+    public class OneBodyLinearMotorTypeProcessor : OneBodyTypeProcessor<OneBodyLinearMotorPrestepData, Vector3Wide, OneBodyLinearMotorFunctions, AccessNoPosition, AccessNoPosition>
     {
         public const int BatchTypeId = 45;
     }
diff --git a/BepuPhysics/Constraints/OneBodyLinearServo.cs b/BepuPhysics/Constraints/OneBodyLinearServo.cs
index 6fc8cd882..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;
@@ -30,7 +29,7 @@ public struct OneBodyLinearServo : IOneBodyConstraintDescription<OneBodyLinearSe
         /// </summary>
         public ServoSettings ServoSettings;
 
-        public readonly int ConstraintTypeId
+        public static int ConstraintTypeId
         {
             [MethodImpl(MethodImplOptions.AggressiveInlining)]
             get
@@ -39,7 +38,8 @@ public readonly int ConstraintTypeId
             }
         }
 
-        public readonly Type TypeProcessorType => typeof(OneBodyLinearServoTypeProcessor);
+        public static Type TypeProcessorType => typeof(OneBodyLinearServoTypeProcessor);
+        public static TypeProcessor CreateTypeProcessor() => new OneBodyLinearServoTypeProcessor();
 
         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
             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<OneBodyLinearServoPrestepData>.Get(ref batch.PrestepData, bundleIndex), innerIndex);
@@ -74,21 +74,11 @@ public struct OneBodyLinearServoPrestepData
         public ServoSettingsWide ServoSettings;
     }
 
-    public struct OneBodyLinearServoProjection
-    {
-        public Vector3Wide Offset;
-        public Vector3Wide BiasVelocity;
-        public Symmetric3x3Wide EffectiveMass;
-        public Vector<float> SoftnessImpulseScale;
-        public Vector<float> MaximumImpulse;
-        public BodyInertias Inertia;
-    }
-
-    public struct OneBodyLinearServoFunctions : IOneBodyConstraintFunctions<OneBodyLinearServoPrestepData, OneBodyLinearServoProjection, Vector3Wide>
+    public struct OneBodyLinearServoFunctions : IOneBodyConstraintFunctions<OneBodyLinearServoPrestepData, Vector3Wide>
     {
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
         public static void ComputeTransforms(in Vector3Wide localOffset, in QuaternionWide orientation, in Vector<float> 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,75 +93,59 @@ public static void ComputeTransforms(in Vector3Wide localOffset, in QuaternionWi
         }
 
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public void Prestep(Bodies bodies, ref Vector<int> bodyReferences, int count, float dt, float inverseDt, ref BodyInertias inertia, 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;
-
-            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);
-
-            //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.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 BodyVelocities velocityA, in OneBodyLinearServoProjection projection, ref Vector3Wide csi)
+        public static void ApplyImpulse(in Vector3Wide offset, in BodyInertiaWide inertia, ref BodyVelocityWide velocityA, in Vector3Wide csi)
         {
-            Vector3Wide.CrossWithoutOverlap(projection.Offset, csi, out var wsi);
-            Symmetric3x3Wide.TransformWithoutOverlap(wsi, projection.Inertia.InverseInertiaTensor, out var change);
+            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, projection.Inertia.InverseMass, out change);
+            Vector3Wide.Scale(csi, inertia.InverseMass, out change);
             Vector3Wide.Add(velocityA.Linear, change, out velocityA.Linear);
         }
 
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public void WarmStart(ref BodyVelocities velocityA, ref OneBodyLinearServoProjection projection, ref Vector3Wide accumulatedImpulse)
+        public static void WarmStart(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, ref OneBodyLinearServoPrestepData prestep, ref Vector3Wide accumulatedImpulses, ref BodyVelocityWide wsvA)
         {
-            ApplyImpulse(ref velocityA, projection, ref accumulatedImpulse);
+            QuaternionWide.TransformWithoutOverlap(prestep.LocalOffset, orientationA, out var offset);
+            ApplyImpulse(offset, inertiaA, ref wsvA, accumulatedImpulses);
         }
 
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public static void SharedSolve(ref BodyVelocities velocities, in OneBodyLinearServoProjection projection, ref Vector3Wide accumulatedImpulse)
+        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);
+
+            //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);
-            Vector3Wide.CrossWithoutOverlap(velocities.Angular, projection.Offset, out var angularCSV);
-            Vector3Wide.Add(velocities.Linear, angularCSV, out var csv);
-            Vector3Wide.Subtract(projection.BiasVelocity, csv, out csv);
+            var csv = biasVelocity - Vector3Wide.Cross(wsvA.Angular, offset) - wsvA.Linear;
 
-            Symmetric3x3Wide.TransformWithoutOverlap(csv, projection.EffectiveMass, out var csi);
-            Vector3Wide.Scale(accumulatedImpulse, projection.SoftnessImpulseScale, out var softness);
-            Vector3Wide.Subtract(csi, softness, out csi);
+            //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(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<float>.Zero), accumulatedImpulse, previous, out accumulatedImpulse);
-            Vector3Wide.Subtract(accumulatedImpulse, previous, out csi);
-
-            ApplyImpulse(ref velocities, projection, ref csi);
+            ServoSettingsWide.ClampImpulse(maximumImpulse, ref accumulatedImpulses, ref csi);
+            ApplyImpulse(offset, inertiaA, ref wsvA, csi);
         }
 
+        public static bool RequiresIncrementalSubstepUpdates => false;
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public void Solve(ref BodyVelocities velocityA, ref OneBodyLinearServoProjection projection, ref Vector3Wide accumulatedImpulse)
-        {
-            SharedSolve(ref velocityA, projection, ref accumulatedImpulse);
-        }
-
+        public static void IncrementallyUpdateForSubstep(in Vector<float> dt, in BodyVelocityWide wsvA, ref OneBodyLinearServoPrestepData prestepData) { }
     }
 
-    public class OneBodyLinearServoTypeProcessor : OneBodyTypeProcessor<OneBodyLinearServoPrestepData, OneBodyLinearServoProjection, Vector3Wide, OneBodyLinearServoFunctions>
+    public class OneBodyLinearServoTypeProcessor : OneBodyTypeProcessor<OneBodyLinearServoPrestepData, Vector3Wide, OneBodyLinearServoFunctions, AccessAll, AccessAll>
     {
         public const int BatchTypeId = 44;
     }
diff --git a/BepuPhysics/Constraints/OneBodyTypeProcessor.cs b/BepuPhysics/Constraints/OneBodyTypeProcessor.cs
index b75c99d34..266916bb5 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;
 
@@ -13,23 +11,18 @@ namespace BepuPhysics.Constraints
     /// </summary>
     /// <typeparam name="TPrestepData">Type of the prestep data used by the constraint.</typeparam>
     /// <typeparam name="TAccumulatedImpulse">Type of the accumulated impulses used by the constraint.</typeparam>
-    /// <typeparam name="TProjection">Type of the projection to input.</typeparam>
-    public interface IOneBodyConstraintFunctions<TPrestepData, TProjection, TAccumulatedImpulse>
+    public interface IOneBodyConstraintFunctions<TPrestepData, TAccumulatedImpulse>
     {
-        void Prestep(Bodies bodies, ref Vector<int> bodyReferences, int count, float dt, float inverseDt, ref BodyInertias inertia, 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);
-    }
-
-    /// <summary>
-    /// Prestep, warm start, solve iteration, and incremental contact update functions for a one body contact constraint type.
-    /// </summary>
-    /// <typeparam name="TPrestepData">Type of the prestep data used by the constraint.</typeparam>
-    /// <typeparam name="TAccumulatedImpulse">Type of the accumulated impulses used by the constraint.</typeparam>
-    /// <typeparam name="TProjection">Type of the projection to input.</typeparam>
-    public interface IOneBodyContactConstraintFunctions<TPrestepData, TProjection, TAccumulatedImpulse> : IOneBodyConstraintFunctions<TPrestepData, TProjection, TAccumulatedImpulse>
-    {
-        void IncrementallyUpdateContactData(in Vector<float> dt, in BodyVelocities velocity, ref TPrestepData prestepData);
+        static abstract void WarmStart(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA,
+            ref TPrestepData prestep, ref TAccumulatedImpulse accumulatedImpulses, ref BodyVelocityWide wsvA);
+        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);
+
+        /// <summary>
+        /// Gets whether this constraint type requires incremental updates for each substep taken beyond the first.
+        /// </summary>
+        static abstract bool RequiresIncrementalSubstepUpdates { get; }
+        static abstract void IncrementallyUpdateForSubstep(in Vector<float> 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.
@@ -37,24 +30,20 @@ public interface IOneBodyContactConstraintFunctions<TPrestepData, TProjection, T
     /// <summary>
     /// Shared implementation across all one body constraints.
     /// </summary>
-    public abstract class OneBodyTypeProcessor<TPrestepData, TProjection, TAccumulatedImpulse, TConstraintFunctions>
-        : TypeProcessor<Vector<int>, TPrestepData, TProjection, TAccumulatedImpulse>
-        where TPrestepData : unmanaged where TProjection : unmanaged where TAccumulatedImpulse : unmanaged
-        where TConstraintFunctions : unmanaged, IOneBodyConstraintFunctions<TPrestepData, TProjection, TAccumulatedImpulse>
+    public abstract class OneBodyTypeProcessor<TPrestepData, TAccumulatedImpulse, TConstraintFunctions,
+        TWarmStartAccessFilterA, TSolveAccessFilterA>
+        : TypeProcessor<Vector<int>, TPrestepData, TAccumulatedImpulse>
+        where TPrestepData : unmanaged where TAccumulatedImpulse : unmanaged
+        where TConstraintFunctions : unmanaged, IOneBodyConstraintFunctions<TPrestepData, TAccumulatedImpulse>
+        where TWarmStartAccessFilterA : unmanaged, IBodyAccessFilter
+        where TSolveAccessFilterA : unmanaged, IBodyAccessFilter
     {
         protected sealed override int InternalBodiesPerConstraint => 1;
 
-        public sealed override void EnumerateConnectedBodyIndices<TEnumerator>(ref TypeBatch typeBatch, int indexInTypeBatch, ref TEnumerator enumerator)
-        {
-            BundleIndexing.GetBundleIndices(indexInTypeBatch, out var constraintBundleIndex, out var constraintInnerIndex);
-            enumerator.LoopBody(GatherScatter.Get(ref Buffer<Vector<int>>.Get(ref typeBatch.BodyReferences, constraintBundleIndex), constraintInnerIndex));
-        }
-
-
         struct OneBodySortKeyGenerator : ISortKeyGenerator<Vector<int>>
         {
             [MethodImpl(MethodImplOptions.AggressiveInlining)]
-            public int GetSortKey(int constraintIndex, ref Buffer<Vector<int>> bodyReferences)
+            public static int GetSortKey(int constraintIndex, ref Buffer<Vector<int>> bodyReferences)
             {
                 BundleIndexing.GetBundleIndices(constraintIndex, out var bundleIndex, out var innerIndex);
                 //We sort based on the body references within the constraint. 
@@ -68,7 +57,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<byte> bodyReferencesCache)
         {
             GenerateSortKeysAndCopyReferences<OneBodySortKeyGenerator>(
                 ref typeBatch,
@@ -89,139 +78,80 @@ 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<TPrestepData>(typeBatch.PrestepData.Memory);
-            ref var bodyReferencesBase = ref Unsafe.AsRef<Vector<int>>(typeBatch.BodyReferences.Memory);
-            ref var projectionBase = ref Unsafe.AsRef<TProjection>(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.GatherInertia(ref references, count, out var inertiaA);
-                function.Prestep(bodies, ref references, count, dt, inverseDt, ref inertiaA, ref prestep, out projection);
-            }
-        }
 
-        public unsafe override void WarmStart(ref TypeBatch typeBatch, ref Buffer<BodyVelocity> bodyVelocities, int startBundle, int exclusiveEndBundle)
+        public override void WarmStart<TIntegratorCallbacks, TBatchIntegrationMode, TAllowPoseIntegration>(
+            ref TypeBatch typeBatch, ref Buffer<IndexSet> integrationFlags, Bodies bodies, ref TIntegratorCallbacks integratorCallbacks,
+            float dt, float inverseDt, int startBundle, int exclusiveEndBundle, int workerIndex)
         {
-            ref var bodyReferencesBase = ref Unsafe.AsRef<Vector<int>>(typeBatch.BodyReferences.Memory);
-            ref var accumulatedImpulsesBase = ref Unsafe.AsRef<TAccumulatedImpulse>(typeBatch.AccumulatedImpulses.Memory);
-            ref var projectionBase = ref Unsafe.AsRef<TProjection>(typeBatch.Projection.Memory);
-            var function = default(TConstraintFunctions);
+            var prestepBundles = typeBatch.PrestepData.As<TPrestepData>();
+            var bodyReferencesBundles = typeBatch.BodyReferences.As<Vector<int>>();
+            var accumulatedImpulsesBundles = typeBatch.AccumulatedImpulses.As<TAccumulatedImpulse>();
             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);
-                Bodies.GatherVelocities(ref bodyVelocities, ref bodyReferences, count, out var wsvA);
-                function.WarmStart(ref wsvA, ref projection, ref accumulatedImpulses);
-                Bodies.ScatterVelocities(ref wsvA, ref bodyVelocities, ref bodyReferences, count);
+                ref var prestep = ref prestepBundles[i];
+                ref var accumulatedImpulses = ref accumulatedImpulsesBundles[i];
+                ref var references = ref bodyReferencesBundles[i];
+                GatherAndIntegrate<TIntegratorCallbacks, TBatchIntegrationMode, TWarmStartAccessFilterA, TAllowPoseIntegration>(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))
+                //    function.UpdateForNewPose(positionA, orientationA, inertiaA, wsvA, new Vector<float>(dt), accumulatedImpulses, ref prestep);
+
+                TConstraintFunctions.WarmStart(positionA, orientationA, inertiaA, ref prestep, ref accumulatedImpulses, ref wsvA);
+
+                if (typeof(TBatchIntegrationMode) == typeof(BatchShouldNeverIntegrate))
+                {
+                    bodies.ScatterVelocities<TWarmStartAccessFilterA>(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<AccessAll>(ref wsvA, ref references);
+                }
             }
         }
 
-        public unsafe override void SolveIteration(ref TypeBatch typeBatch, ref Buffer<BodyVelocity> bodyVelocities, int startBundle, int exclusiveEndBundle)
+        public override void Solve(ref TypeBatch typeBatch, Bodies bodies, float dt, float inverseDt, int startBundle, int exclusiveEndBundle)
         {
-            ref var bodyReferencesBase = ref Unsafe.AsRef<Vector<int>>(typeBatch.BodyReferences.Memory);
-            ref var accumulatedImpulsesBase = ref Unsafe.AsRef<TAccumulatedImpulse>(typeBatch.AccumulatedImpulses.Memory);
-            ref var projectionBase = ref Unsafe.AsRef<TProjection>(typeBatch.Projection.Memory);
-            var function = default(TConstraintFunctions);
+            var prestepBundles = typeBatch.PrestepData.As<TPrestepData>();
+            var bodyReferencesBundles = typeBatch.BodyReferences.As<Vector<int>>();
+            var accumulatedImpulsesBundles = typeBatch.AccumulatedImpulses.As<TAccumulatedImpulse>();
             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);
-                Bodies.GatherVelocities(ref bodyVelocities, ref bodyReferences, count, out var wsvA);
-                function.Solve(ref wsvA, ref projection, ref accumulatedImpulses);
-                Bodies.ScatterVelocities(ref wsvA, ref bodyVelocities, ref bodyReferences, count);
-            }
-        }
+                ref var prestep = ref prestepBundles[i];
+                ref var accumulatedImpulses = ref accumulatedImpulsesBundles[i];
+                ref var references = ref bodyReferencesBundles[i];
+                bodies.GatherState<TSolveAccessFilterA>(references, true, out var positionA, out var orientationA, out var wsvA, out var inertiaA);
 
-        public unsafe override void JacobiPrestep(ref TypeBatch typeBatch, Bodies bodies, ref FallbackBatch jacobiBatch, float dt, float inverseDt, int startBundle, int exclusiveEndBundle)
-        {
-            ref var prestepBase = ref Unsafe.AsRef<TPrestepData>(typeBatch.PrestepData.Memory);
-            ref var bodyReferencesBase = ref Unsafe.AsRef<Vector<int>>(typeBatch.BodyReferences.Memory);
-            ref var projectionBase = ref Unsafe.AsRef<TProjection>(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.GatherInertia(ref references, count, 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);
-            }
-        }
-        public unsafe override void JacobiWarmStart(ref TypeBatch typeBatch, ref Buffer<BodyVelocity> bodyVelocities, ref FallbackTypeBatchResults jacobiResults, int startBundle, int exclusiveEndBundle)
-        {
-            ref var bodyReferencesBase = ref Unsafe.AsRef<Vector<int>>(typeBatch.BodyReferences.Memory);
-            ref var accumulatedImpulsesBase = ref Unsafe.AsRef<TAccumulatedImpulse>(typeBatch.AccumulatedImpulses.Memory);
-            ref var projectionBase = ref Unsafe.AsRef<TProjection>(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.GatherVelocities(ref bodyVelocities, ref bodyReferences, count, out wsvA);
-                function.WarmStart(ref wsvA, ref projection, ref accumulatedImpulses);
+                TConstraintFunctions.Solve(positionA, orientationA, inertiaA, dt, inverseDt, ref prestep, ref accumulatedImpulses, ref wsvA);
+
+                bodies.ScatterVelocities<TSolveAccessFilterA>(ref wsvA, ref references);
             }
         }
-        public unsafe override void JacobiSolveIteration(ref TypeBatch typeBatch, ref Buffer<BodyVelocity> bodyVelocities, ref FallbackTypeBatchResults jacobiResults, 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)
         {
-            ref var bodyReferencesBase = ref Unsafe.AsRef<Vector<int>>(typeBatch.BodyReferences.Memory);
-            ref var accumulatedImpulsesBase = ref Unsafe.AsRef<TAccumulatedImpulse>(typeBatch.AccumulatedImpulses.Memory);
-            ref var projectionBase = ref Unsafe.AsRef<TProjection>(typeBatch.Projection.Memory);
-            var function = default(TConstraintFunctions);
-            ref var jacobiResultsBundlesA = ref jacobiResults.GetVelocitiesForBody(0);
+            var prestepBundles = typeBatch.PrestepData.As<TPrestepData>();
+            var bodyReferencesBundles = typeBatch.BodyReferences.As<Vector<int>>();
+            var dtWide = new Vector<float>(dt);
             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.GatherVelocities(ref bodyVelocities, ref bodyReferences, count, out wsvA);
-                function.Solve(ref wsvA, ref projection, ref accumulatedImpulses);
+                ref var prestep = ref prestepBundles[i];
+                ref var references = ref bodyReferencesBundles[i];
+                bodies.GatherState<AccessOnlyVelocity>(references, true, out _, out _, out var wsvA, out _);
+                TConstraintFunctions.IncrementallyUpdateForSubstep(dtWide, wsvA, ref prestep);
             }
         }
-
     }
 
-    public abstract class OneBodyContactTypeProcessor<TPrestepData, TProjection, TAccumulatedImpulse, TConstraintFunctions>
-        : OneBodyTypeProcessor<TPrestepData, TProjection, TAccumulatedImpulse, TConstraintFunctions>
-        where TPrestepData : unmanaged where TProjection : unmanaged where TAccumulatedImpulse : unmanaged
-        where TConstraintFunctions : unmanaged, IOneBodyContactConstraintFunctions<TPrestepData, TProjection, TAccumulatedImpulse>
+    public abstract class OneBodyContactTypeProcessor<TPrestepData, TAccumulatedImpulse, TConstraintFunctions>
+        : OneBodyTypeProcessor<TPrestepData, TAccumulatedImpulse, TConstraintFunctions, AccessNoPose, AccessNoPose>
+        where TPrestepData : unmanaged where TAccumulatedImpulse : unmanaged
+        where TConstraintFunctions : unmanaged, IOneBodyConstraintFunctions<TPrestepData, TAccumulatedImpulse>
     {
-        public unsafe override void IncrementallyUpdateContactData(ref TypeBatch typeBatch, Bodies bodies, float dt, float inverseDt, int startBundle, int exclusiveEndBundle)
-        {
-            ref var prestepBase = ref Unsafe.AsRef<TPrestepData>(typeBatch.PrestepData.Memory);
-            ref var bodyReferencesBase = ref Unsafe.AsRef<Vector<int>>(typeBatch.BodyReferences.Memory);
-            ref var bodyVelocities = ref bodies.ActiveSet.Velocities;
-            var function = default(TConstraintFunctions);
-            var dtWide = new Vector<float>(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);
-                var count = GetCountInBundle(ref typeBatch, i);
-                Bodies.GatherVelocities(ref bodyVelocities, ref references, count, out var velocityA);
-                function.IncrementallyUpdateContactData(dtWide, velocityA, ref prestep);
-            }
-        }
+
     }
 
 }
diff --git a/BepuPhysics/Constraints/PointOnLineServo.cs b/BepuPhysics/Constraints/PointOnLineServo.cs
index 7c24afce0..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;
@@ -34,7 +33,7 @@ public struct PointOnLineServo : ITwoBodyConstraintDescription<PointOnLineServo>
         /// </summary>
         public SpringSettings SpringSettings;
 
-        public readonly int ConstraintTypeId
+        public static int ConstraintTypeId
         {
             [MethodImpl(MethodImplOptions.AggressiveInlining)]
             get
@@ -43,7 +42,8 @@ public readonly int ConstraintTypeId
             }
         }
 
-        public readonly Type TypeProcessorType => typeof(PointOnLineServoTypeProcessor);
+        public static Type TypeProcessorType => typeof(PointOnLineServoTypeProcessor);
+        public static TypeProcessor CreateTypeProcessor() => new PointOnLineServoTypeProcessor();
 
         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
             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<PointOnLineServoPrestepData>.Get(ref batch.PrestepData, bundleIndex), innerIndex);
@@ -79,32 +79,59 @@ public struct PointOnLineServoPrestepData
         public SpringSettingsWide SpringSettings;
     }
 
-    public struct PointOnLineServoProjection
+    public struct PointOnLineServoFunctions : ITwoBodyConstraintFunctions<PointOnLineServoPrestepData, Vector2Wide>
     {
-        public Matrix2x3Wide LinearJacobian;
-        public Vector3Wide OffsetA;
-        public Vector3Wide OffsetB;
-        public Vector2Wide BiasVelocity;
-        public Symmetric2x2Wide EffectiveMass;
-        public Vector<float> SoftnessImpulseScale;
-        public Vector<float> MaximumImpulse;
-        public BodyInertias InertiaA;
-        public BodyInertias InertiaB;
-    }
-
-    public struct PointOnLineServoFunctions : IConstraintFunctions<PointOnLineServoPrestepData, PointOnLineServoProjection, Vector2Wide>
-    {
-        static void GetAngularJacobians(in Matrix2x3Wide linearJacobians, in Vector3Wide offsetA, in Vector3Wide offsetB, out Matrix2x3Wide angularJacobianA, out Matrix2x3Wide angularJacobianB)
+        [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)
         {
-            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);
+            Matrix2x3Wide.Transform(csi, linearJacobian, out var linearImpulseA);
+            Matrix2x3Wide.Transform(csi, angularJacobianA, out var angularImpulseA);
+            Matrix2x3Wide.Transform(csi, angularJacobianB, out var angularImpulseB);
+            Symmetric3x3Wide.TransformWithoutOverlap(angularImpulseA, inertiaA.InverseInertiaTensor, out var angularChangeA);
+            Symmetric3x3Wide.TransformWithoutOverlap(angularImpulseB, inertiaB.InverseInertiaTensor, out var angularChangeB);
+            Vector3Wide.Scale(linearImpulseA, inertiaA.InverseMass, out var linearChangeA);
+            Vector3Wide.Scale(linearImpulseA, inertiaB.InverseMass, out var negatedLinearChangeB);
+
+            Vector3Wide.Add(linearChangeA, velocityA.Linear, out velocityA.Linear);
+            Vector3Wide.Add(angularChangeA, velocityA.Angular, out velocityA.Angular);
+            Vector3Wide.Subtract(velocityB.Linear, negatedLinearChangeB, out velocityB.Linear);
+            Vector3Wide.Add(angularChangeB, velocityB.Angular, out velocityB.Angular);
         }
 
+
         [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 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 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 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:
@@ -127,104 +154,45 @@ public void Prestep(Bodies bodies, ref TwoBodyReferences bodyReferences, int cou
             //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.
-
-            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);
-
-            //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);
+            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 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 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);
-            Matrix2x3Wide.Transform(csi, angularJacobianA, out var angularImpulseA);
-            Matrix2x3Wide.Transform(csi, angularJacobianB, out var angularImpulseB);
-            Symmetric3x3Wide.TransformWithoutOverlap(angularImpulseA, inertiaA.InverseInertiaTensor, out var angularChangeA);
-            Symmetric3x3Wide.TransformWithoutOverlap(angularImpulseB, inertiaB.InverseInertiaTensor, out var angularChangeB);
-            Vector3Wide.Scale(linearImpulseA, inertiaA.InverseMass, out var linearChangeA);
-            Vector3Wide.Scale(linearImpulseA, inertiaB.InverseMass, out var negatedLinearChangeB);
-
-            Vector3Wide.Add(linearChangeA, velocityA.Linear, out velocityA.Linear);
-            Vector3Wide.Add(angularChangeA, velocityA.Angular, out velocityA.Angular);
-            Vector3Wide.Subtract(velocityB.Linear, negatedLinearChangeB, out velocityB.Linear);
-            Vector3Wide.Add(angularChangeB, velocityB.Angular, out velocityB.Angular);
-        }
+            Symmetric2x2Wide.InvertWithoutOverlap(inverseEffectiveMass, out var effectiveMass);
 
-        [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public void WarmStart(ref BodyVelocities velocityA, ref BodyVelocities 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);
-        }
+            SpringSettingsWide.ComputeSpringiness(prestep.SpringSettings, dt, out var positionErrorToVelocity, out var effectiveMassCFMScale, out var softnessImpulseScale);
+            Symmetric2x2Wide.Scale(effectiveMass, effectiveMassCFMScale, out effectiveMass);
 
-        [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public void Solve(ref BodyVelocities velocityA, ref BodyVelocities 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);
+            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);
-            Vector2Wide.Subtract(projection.BiasVelocity, csv, out csv);
-            Symmetric2x2Wide.TransformWithoutOverlap(csv, projection.EffectiveMass, out var csi);
-            Vector2Wide.Scale(accumulatedImpulse, projection.SoftnessImpulseScale, out var softnessContribution);
+            //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(projection.MaximumImpulse, ref accumulatedImpulse, ref csi);
-            ApplyImpulse(ref velocityA, ref velocityB, projection.LinearJacobian, angularA, angularB, projection.InertiaA, projection.InertiaB, ref csi);
+            ServoSettingsWide.ClampImpulse(maximumImpulse, ref accumulatedImpulses, ref csi);
+            ApplyImpulse(ref wsvA, ref wsvB, linearJacobian, angularJA, angularJB, inertiaA, inertiaB, ref csi);
         }
 
+        public static bool RequiresIncrementalSubstepUpdates => false;
+        [MethodImpl(MethodImplOptions.AggressiveInlining)]
+        public static void IncrementallyUpdateForSubstep(in Vector<float> dt, in BodyVelocityWide wsvA, in BodyVelocityWide wsvB, ref PointOnLineServoPrestepData prestepData) { }
     }
 
-    public class PointOnLineServoTypeProcessor : TwoBodyTypeProcessor<PointOnLineServoPrestepData, PointOnLineServoProjection, Vector2Wide, PointOnLineServoFunctions>
+    public class PointOnLineServoTypeProcessor : TwoBodyTypeProcessor<PointOnLineServoPrestepData, Vector2Wide, PointOnLineServoFunctions, AccessAll, AccessAll, AccessAll, AccessAll>
     {
         public const int BatchTypeId = 37;
     }
diff --git a/BepuPhysics/Constraints/ServoSettings.cs b/BepuPhysics/Constraints/ServoSettings.cs
index 36137656a..642b83704 100644
--- a/BepuPhysics/Constraints/ServoSettings.cs
+++ b/BepuPhysics/Constraints/ServoSettings.cs
@@ -1,170 +1,196 @@
 using BepuUtilities;
-using System;
-using System.Collections.Generic;
 using System.Diagnostics;
 using System.Numerics;
 using System.Runtime.CompilerServices;
-using System.Text;
 
-namespace BepuPhysics.Constraints
+namespace BepuPhysics.Constraints;
+
+/// <summary>
+/// Describes how a quickly and strongly a servo constraint should move towards a position target.
+/// </summary>
+/// <remarks>
+/// The constraint will attempt to reach a speed between <see cref="BaseSpeed"/> and <see cref="MaximumSpeed"/> using a force no greater than <see cref="MaximumForce"/>.
+/// The speed that the constraint will attempt to use before clamping is based on its spring settings.
+/// <para>
+/// 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.
+/// </para>
+/// </remarks>
+public struct ServoSettings
 {
-    public struct ServoSettings
-    {
-        public float MaximumSpeed;
-        public float BaseSpeed;
-        public float MaximumForce;
+    /// <summary>
+    /// Maximum speed that the constraint can try to use to move towards the target.
+    /// </summary>
+    public float MaximumSpeed;
+    /// <summary>
+    /// 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.
+    /// </summary>
+    public float BaseSpeed;
+    /// <summary>
+    /// The maximum force that the constraint can apply to move towards the target.
+    /// </summary>
+    /// <remarks>
+    /// 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 <see cref="MaximumForce"/> * dt where dt is the substep duration.
+    /// </remarks>
+    public float MaximumForce;
 
-        /// <summary>
-        /// Gets settings representing a servo with unlimited force, speed, and no base speed.
-        /// </summary>
-        public static ServoSettings Default { get { return new ServoSettings(float.MaxValue, 0, float.MaxValue); } }
+    /// <summary>
+    /// 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.
+    /// </summary>
+    public static ServoSettings Default { get { return new ServoSettings(float.MaxValue, 0, float.MaxValue); } }
 
-        /// <summary>
-        /// Checks servo settings to ensure valid values.
-        /// </summary>
-        /// <param name="settings">Settings to check.</param>
-        /// <returns>True if the settings contain valid values, false otherwise.</returns>
-        public static bool Validate(in ServoSettings settings)
-        {
-            return ConstraintChecker.IsNonnegativeNumber(settings.MaximumSpeed) && ConstraintChecker.IsNonnegativeNumber(settings.BaseSpeed) && ConstraintChecker.IsNonnegativeNumber(settings.MaximumForce);
-        }
+    /// <summary>
+    /// Checks servo settings to ensure valid values.
+    /// </summary>
+    /// <param name="settings">Settings to check.</param>
+    /// <returns>True if the settings contain valid values, false otherwise.</returns>
+    public static bool Validate(in ServoSettings settings)
+    {
+        return ConstraintChecker.IsNonnegativeNumber(settings.MaximumSpeed) && ConstraintChecker.IsNonnegativeNumber(settings.BaseSpeed) && ConstraintChecker.IsNonnegativeNumber(settings.MaximumForce);
+    }
+
+    /// <summary>
+    /// Creates a new servo settings instance with the specified properties.
+    /// </summary>
+    /// <param name="maximumSpeed">Sets the <see cref="MaximumSpeed"/> property.</param>
+    /// <param name="baseSpeed">Sets the <see cref="BaseSpeed"/> property.</param>
+    /// <param name="maximumForce">Sets the <see cref="MaximumForce"/> property.</param>
+    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<float> MaximumSpeed;
+    public Vector<float> BaseSpeed;
+    public Vector<float> 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<float> error, in Vector<float> positionErrorToVelocity, in ServoSettingsWide servoSettings, float dt, float inverseDt, 
+        out Vector<float> clampedBiasVelocity, out Vector<float> maximumImpulse)
+    {
+        //Can't request speed that would cause an overshoot.
+        var baseSpeed = Vector.Min(servoSettings.BaseSpeed, Vector.Abs(error) * new Vector<float>(inverseDt));
+        var biasVelocity = error * positionErrorToVelocity;
+        clampedBiasVelocity = Vector.ConditionalSelect(Vector.LessThan(biasVelocity, Vector<float>.Zero),
+            Vector.Max(-servoSettings.MaximumSpeed, Vector.Min(-baseSpeed, biasVelocity)),
+            Vector.Min(servoSettings.MaximumSpeed, Vector.Max(baseSpeed, biasVelocity)));
+        maximumImpulse = servoSettings.MaximumForce * new Vector<float>(dt);
     }
-    public struct ServoSettingsWide
+    
+    [MethodImpl(MethodImplOptions.AggressiveInlining)]
+    public static void ComputeClampedBiasVelocity(in Vector2Wide errorAxis, in Vector<float> errorLength, in Vector<float> positionErrorToBiasVelocity, in ServoSettingsWide servoSettings,
+        float dt, float inverseDt, out Vector2Wide clampedBiasVelocity, out Vector<float> maximumImpulse)
     {
-        public Vector<float> MaximumSpeed;
-        public Vector<float> BaseSpeed;
-        public Vector<float> MaximumForce;
-
-        [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public static void ComputeClampedBiasVelocity(in Vector<float> error, in Vector<float> positionErrorToVelocity, in ServoSettingsWide servoSettings, float dt, float inverseDt, 
-            out Vector<float> clampedBiasVelocity, out Vector<float> maximumImpulse)
-        {
-            //Can't request speed that would cause an overshoot.
-            var baseSpeed = Vector.Min(servoSettings.BaseSpeed, Vector.Abs(error) * inverseDt);
-            var biasVelocity = error * positionErrorToVelocity;
-            clampedBiasVelocity = Vector.ConditionalSelect(Vector.LessThan(biasVelocity, Vector<float>.Zero),
-                Vector.Max(-servoSettings.MaximumSpeed, Vector.Min(-baseSpeed, biasVelocity)),
-                Vector.Min(servoSettings.MaximumSpeed, Vector.Max(baseSpeed, biasVelocity)));
-            maximumImpulse = servoSettings.MaximumForce * dt;
-        }
-        
-        [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public static void ComputeClampedBiasVelocity(in Vector2Wide errorAxis, in Vector<float> errorLength, in Vector<float> positionErrorToBiasVelocity, in ServoSettingsWide servoSettings,
-            float dt, float inverseDt, out Vector2Wide clampedBiasVelocity, out Vector<float> maximumImpulse)
-        {
-            //Can't request speed that would cause an overshoot.
-            var baseSpeed = Vector.Min(servoSettings.BaseSpeed, errorLength * inverseDt);
-            var unclampedBiasSpeed = errorLength * positionErrorToBiasVelocity;
-            var targetSpeed = Vector.Max(baseSpeed, unclampedBiasSpeed);
-            var scale = Vector.Min(Vector<float>.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<float>(1e-10f));
-            scale = Vector.ConditionalSelect(useFallback, Vector<float>.One, scale);
-            Vector2Wide.Scale(errorAxis, scale * unclampedBiasSpeed, out clampedBiasVelocity);
-            maximumImpulse = servoSettings.MaximumForce * dt;
-        }
-
-        [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public static void ComputeClampedBiasVelocity(in Vector2Wide error, in Vector<float> positionErrorToBiasVelocity, in ServoSettingsWide servoSettings,
-            float dt, float inverseDt, out Vector2Wide clampedBiasVelocity, out Vector<float> maximumImpulse)
-        {
-            Vector2Wide.Length(error, out var errorLength);
-            Vector2Wide.Scale(error, Vector<float>.One / errorLength, out var errorAxis);
-            var useFallback = Vector.LessThan(errorLength, new Vector<float>(1e-10f));
-            errorAxis.X = Vector.ConditionalSelect(useFallback, Vector<float>.Zero, errorAxis.X);
-            errorAxis.Y = Vector.ConditionalSelect(useFallback, Vector<float>.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<float>(inverseDt));
+        var unclampedBiasSpeed = errorLength * positionErrorToBiasVelocity;
+        var targetSpeed = Vector.Max(baseSpeed, unclampedBiasSpeed);
+        var scale = Vector.Min(Vector<float>.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<float>(1e-10f));
+        scale = Vector.ConditionalSelect(useFallback, Vector<float>.One, scale);
+        Vector2Wide.Scale(errorAxis, scale * unclampedBiasSpeed, out clampedBiasVelocity);
+        maximumImpulse = servoSettings.MaximumForce * new Vector<float>(dt);
+    }
 
-        [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public static void ComputeClampedBiasVelocity(in Vector3Wide errorAxis, in Vector<float> errorLength, in Vector<float> positionErrorToBiasVelocity, in ServoSettingsWide servoSettings,
-            float dt, float inverseDt, out Vector3Wide clampedBiasVelocity, out Vector<float> maximumImpulse)
-        {
-            //Can't request speed that would cause an overshoot.
-            var baseSpeed = Vector.Min(servoSettings.BaseSpeed, errorLength * inverseDt);
-            var unclampedBiasSpeed = errorLength * positionErrorToBiasVelocity;
-            var targetSpeed = Vector.Max(baseSpeed, unclampedBiasSpeed);
-            var scale = Vector.Min(Vector<float>.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<float>(1e-10f));
-            scale = Vector.ConditionalSelect(useFallback, Vector<float>.One, scale);
-            Vector3Wide.Scale(errorAxis, scale * unclampedBiasSpeed, out clampedBiasVelocity);
-            maximumImpulse = servoSettings.MaximumForce * dt;
-        }
+    [MethodImpl(MethodImplOptions.AggressiveInlining)]
+    public static void ComputeClampedBiasVelocity(in Vector2Wide error, in Vector<float> positionErrorToBiasVelocity, in ServoSettingsWide servoSettings,
+        float dt, float inverseDt, out Vector2Wide clampedBiasVelocity, out Vector<float> maximumImpulse)
+    {
+        Vector2Wide.Length(error, out var errorLength);
+        Vector2Wide.Scale(error, Vector<float>.One / errorLength, out var errorAxis);
+        var useFallback = Vector.LessThan(errorLength, new Vector<float>(1e-10f));
+        errorAxis.X = Vector.ConditionalSelect(useFallback, Vector<float>.Zero, errorAxis.X);
+        errorAxis.Y = Vector.ConditionalSelect(useFallback, Vector<float>.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<float> positionErrorToBiasVelocity, in ServoSettingsWide servoSettings,
-           float dt, float inverseDt, out Vector3Wide clampedBiasVelocity, out Vector<float> maximumImpulse)
-        {
-            Vector3Wide.Length(error, out var errorLength);
-            Vector3Wide.Scale(error, Vector<float>.One / errorLength, out var errorAxis);
-            var useFallback = Vector.LessThan(errorLength, new Vector<float>(1e-10f));
-            errorAxis.X = Vector.ConditionalSelect(useFallback, Vector<float>.Zero, errorAxis.X);
-            errorAxis.Y = Vector.ConditionalSelect(useFallback, Vector<float>.Zero, errorAxis.Y);
-            errorAxis.Z = Vector.ConditionalSelect(useFallback, Vector<float>.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<float> errorLength, in Vector<float> positionErrorToBiasVelocity, in ServoSettingsWide servoSettings,
+        float dt, float inverseDt, out Vector3Wide clampedBiasVelocity, out Vector<float> maximumImpulse)
+    {
+        //Can't request speed that would cause an overshoot.
+        var baseSpeed = Vector.Min(servoSettings.BaseSpeed, errorLength * new Vector<float>(inverseDt));
+        var unclampedBiasSpeed = errorLength * positionErrorToBiasVelocity;
+        var targetSpeed = Vector.Max(baseSpeed, unclampedBiasSpeed);
+        var scale = Vector.Min(Vector<float>.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<float>(1e-10f));
+        scale = Vector.ConditionalSelect(useFallback, Vector<float>.One, scale);
+        Vector3Wide.Scale(errorAxis, scale * unclampedBiasSpeed, out clampedBiasVelocity);
+        maximumImpulse = servoSettings.MaximumForce * new Vector<float>(dt);
+    }
 
-        [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public static void ClampImpulse(in Vector<float> maximumImpulse, ref Vector<float> accumulatedImpulse, ref Vector<float> 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<float> positionErrorToBiasVelocity, in ServoSettingsWide servoSettings,
+       float dt, float inverseDt, out Vector3Wide clampedBiasVelocity, out Vector<float> maximumImpulse)
+    {
+        Vector3Wide.Length(error, out var errorLength);
+        Vector3Wide.Scale(error, Vector<float>.One / errorLength, out var errorAxis);
+        var useFallback = Vector.LessThan(errorLength, new Vector<float>(1e-10f));
+        errorAxis.X = Vector.ConditionalSelect(useFallback, Vector<float>.Zero, errorAxis.X);
+        errorAxis.Y = Vector.ConditionalSelect(useFallback, Vector<float>.Zero, errorAxis.Y);
+        errorAxis.Z = Vector.ConditionalSelect(useFallback, Vector<float>.Zero, errorAxis.Z);
+        ComputeClampedBiasVelocity(errorAxis, errorLength, positionErrorToBiasVelocity, servoSettings, dt, inverseDt, out clampedBiasVelocity, out maximumImpulse);
+    }
 
-        [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public static void ClampImpulse(in Vector<float> 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<float>(1e-10f)),
-                Vector<float>.One,
-                Vector.Min(maximumImpulse / impulseMagnitude, Vector<float>.One));
-            Vector2Wide.Scale(unclamped, impulseScale, out accumulatedImpulse);
-            Vector2Wide.Subtract(accumulatedImpulse, previousImpulse, out csi);
-        }
+    [MethodImpl(MethodImplOptions.AggressiveInlining)]
+    public static void ClampImpulse(in Vector<float> maximumImpulse, ref Vector<float> accumulatedImpulse, ref Vector<float> 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<float> 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<float>(1e-10f)), 
-                Vector<float>.One, 
-                Vector.Min(maximumImpulse / impulseMagnitude, Vector<float>.One));
-            Vector3Wide.Scale(accumulatedImpulse, impulseScale, out accumulatedImpulse);
-            Vector3Wide.Subtract(accumulatedImpulse, previousAccumulatedImpulse, out csi);
-        }
+    [MethodImpl(MethodImplOptions.AggressiveInlining)]
+    public static void ClampImpulse(in Vector<float> 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<float>(1e-10f)),
+            Vector<float>.One,
+            Vector.Min(maximumImpulse / impulseMagnitude, Vector<float>.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<float> 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<float>(1e-10f)), 
+            Vector<float>.One, 
+            Vector.Min(maximumImpulse / impulseMagnitude, Vector<float>.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];
     }
+
 }
diff --git a/BepuPhysics/Constraints/SwingLimit.cs b/BepuPhysics/Constraints/SwingLimit.cs
index 8f41e4eb7..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;
@@ -35,7 +34,7 @@ public struct SwingLimit : ITwoBodyConstraintDescription<SwingLimit>
         /// </summary>
         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,7 +43,8 @@ public readonly int ConstraintTypeId
             }
         }
 
-        public readonly Type TypeProcessorType => typeof(SwingLimitTypeProcessor);
+        public static Type TypeProcessorType => typeof(SwingLimitTypeProcessor);
+        public static TypeProcessor CreateTypeProcessor() => new SwingLimitTypeProcessor();
 
         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
             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<SwingLimitPrestepData>.Get(ref batch.PrestepData, bundleIndex), innerIndex);
@@ -89,24 +89,40 @@ public struct SwingLimitPrestepData
         public SpringSettingsWide SpringSettings;
     }
 
-    public struct SwingLimitProjection
+    public struct SwingLimitFunctions : ITwoBodyConstraintFunctions<SwingLimitPrestepData, Vector<float>>
     {
-        //JacobianB = -JacobianA, so no need to store it explicitly.
-        public Vector3Wide VelocityToImpulseA;
-        public Vector<float> BiasImpulse;
-        public Vector<float> SoftnessImpulseScale;
-        public Vector3Wide ImpulseToVelocityA;
-        public Vector3Wide NegatedImpulseToVelocityB;
-    }
+        [MethodImpl(MethodImplOptions.AggressiveInlining)]
+        private static void ApplyImpulse(in Vector3Wide impulseToVelocityA, in Vector3Wide negatedImpulseToVelocityB, in Vector<float> 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);
+        }
 
-    public struct SwingLimitFunctions : IConstraintFunctions<SwingLimitPrestepData, SwingLimitProjection, Vector<float>>
-    {
         [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)
+        static void ComputeJacobian(in Vector3Wide axisLocalA, in Vector3Wide axisLocalB, in QuaternionWide orientationA, in QuaternionWide orientationB, out Vector3Wide axisA, out Vector3Wide axisB, out Vector3Wide jacobianA)
         {
-            bodies.GatherOrientation(ref bodyReferences, count, out var orientationA, out var orientationB);
+            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<float>(1e-7f));
+            Vector3Wide.ConditionalSelect(useFallback, fallbackJacobian, jacobianA, out jacobianA);
+        }
+        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<float> 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);
+            Symmetric3x3Wide.TransformWithoutOverlap(jacobianA, inertiaB.InverseInertiaTensor, out var negatedImpulseToVelocityB);
+            ApplyImpulse(impulseToVelocityA, negatedImpulseToVelocityB, accumulatedImpulses, ref wsvA.Angular, ref wsvB.Angular);
+        }
 
+        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<float> 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.)
             //C = dot(axisA, axisB) >= MinimumDot
@@ -118,72 +134,41 @@ public void Prestep(Bodies bodies, ref TwoBodyReferences bodyReferences, int cou
             //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<float>(1e-7f));
-            Vector3Wide.ConditionalSelect(useFallback, fallbackJacobian, jacobianA, out jacobianA);
+            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 projection.ImpulseToVelocityA);
+            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 projection.NegatedImpulseToVelocityB);
-            Vector3Wide.Dot(projection.ImpulseToVelocityA, jacobianA, out var angularA);
-            Vector3Wide.Dot(projection.NegatedImpulseToVelocityB, jacobianA, out var angularB);
+            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 projection.SoftnessImpulseScale);
-            var effectiveMass = effectiveMassCFMScale / (angularA + angularB);
-            Vector3Wide.Scale(jacobianA, effectiveMass, out projection.VelocityToImpulseA);
+            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. TODO: arguably, should bake the negation into positionErrorToVelocity, given its name.
+            //Note the negation: we want to oppose the separation.
             var biasVelocity = -Vector.Min(error * new Vector<float>(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<float> 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 BodyVelocities velocityA, ref BodyVelocities velocityB, ref SwingLimitProjection projection, ref Vector<float> 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<float> 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);
+            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);
-            csi = projection.BiasImpulse - accumulatedImpulse * projection.SoftnessImpulseScale - csi;
+            var csi = effectiveMass * (biasVelocity - csv) - accumulatedImpulses * softnessImpulseScale;
 
-            var previousAccumulatedImpulse = accumulatedImpulse;
-            accumulatedImpulse = Vector.Max(Vector<float>.Zero, accumulatedImpulse + csi);
-            csi = accumulatedImpulse - previousAccumulatedImpulse;
-            ApplyImpulse(ref velocityA.Angular, ref velocityB.Angular, ref projection, ref csi);
+            InequalityHelpers.ClampPositive(ref accumulatedImpulses, ref csi);
+            ApplyImpulse(impulseToVelocityA, negatedImpulseToVelocityB, csi, ref wsvA.Angular, ref wsvB.Angular);
         }
 
+        public static bool RequiresIncrementalSubstepUpdates => false;
+        [MethodImpl(MethodImplOptions.AggressiveInlining)]
+        public static void IncrementallyUpdateForSubstep(in Vector<float> dt, in BodyVelocityWide wsvA, in BodyVelocityWide wsvB, ref SwingLimitPrestepData prestepData) { }
     }
 
-    public class SwingLimitTypeProcessor : TwoBodyTypeProcessor<SwingLimitPrestepData, SwingLimitProjection, Vector<float>, SwingLimitFunctions>
+    public class SwingLimitTypeProcessor : TwoBodyTypeProcessor<SwingLimitPrestepData, Vector<float>, SwingLimitFunctions, AccessOnlyAngular, AccessOnlyAngular, AccessOnlyAngular, AccessOnlyAngular>
     {
         public const int BatchTypeId = 25;
     }
diff --git a/BepuPhysics/Constraints/SwivelHinge.cs b/BepuPhysics/Constraints/SwivelHinge.cs
index 60ff8052f..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;
@@ -34,7 +33,7 @@ public struct SwivelHinge : ITwoBodyConstraintDescription<SwivelHinge>
         /// </summary>
         public SpringSettings SpringSettings;
 
-        public readonly int ConstraintTypeId
+        public static int ConstraintTypeId
         {
             [MethodImpl(MethodImplOptions.AggressiveInlining)]
             get
@@ -43,7 +42,8 @@ public readonly int ConstraintTypeId
             }
         }
 
-        public readonly Type TypeProcessorType => typeof(SwivelHingeTypeProcessor);
+        public static Type TypeProcessorType => typeof(SwivelHingeTypeProcessor);
+        public static TypeProcessor CreateTypeProcessor() => new SwivelHingeTypeProcessor();
 
         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 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<SwivelHingePrestepData>.Get(ref batch.PrestepData, bundleIndex), innerIndex);
@@ -80,141 +80,140 @@ public struct SwivelHingePrestepData
         public SpringSettingsWide SpringSettings;
     }
 
-    public struct SwivelHingeProjection
+    public struct SwivelHingeFunctions : ITwoBodyConstraintFunctions<SwivelHingePrestepData, Vector4Wide>
     {
-        public Vector3Wide OffsetA;
-        public Vector3Wide OffsetB;
-        public Vector3Wide SwivelHingeJacobian;
-        public Vector4Wide BiasVelocity;
-        public Symmetric4x4Wide EffectiveMass;
-        public Vector<float> SoftnessImpulseScale;
-        public BodyInertias InertiaA;
-        public BodyInertias InertiaB;
-    }
+        [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<Vector<float>, 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);
+        }
 
-    public struct SwivelHingeFunctions : IConstraintFunctions<SwivelHingePrestepData, SwivelHingeProjection, Vector4Wide>
-    {
         [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)
+        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)
         {
-            bodies.GatherPose(ref bodyReferences, count, out var ab, out var orientationA, out var orientationB);
-            projection.InertiaA = inertiaA;
-            projection.InertiaB = inertiaB;
+            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);
+            //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<float>(1e-3f));
+            //This causes a discontinuity, but a discontinuity is better than a NaNsplode.
+            swivelHingeJacobian = Vector3Wide.ConditionalSelect(useFallbackJacobian, hingeAxis, swivelHingeJacobian);
+        }
 
+        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 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)    ]
             //[ 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);
+            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(projection.OffsetA, inertiaA.InverseInertiaTensor, out var ballSocketContributionAngularA);
-            Symmetric3x3Wide.SkewSandwichWithoutOverlap(projection.OffsetB, inertiaB.InverseInertiaTensor, out var ballSocketContributionAngularB);
+            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 = projection.InertiaA.InverseMass + projection.InertiaB.InverseMass;
+            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(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);
+            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, projection.OffsetA, out var offDiagonalContributionA);
-            Vector3Wide.CrossWithoutOverlap(swivelHingeInertiaB, projection.OffsetB, out var offDiagonalContributionB);
+            Vector3Wide.CrossWithoutOverlap(swivelHingeInertiaA, offsetA, out var offDiagonalContributionA);
+            Vector3Wide.CrossWithoutOverlap(swivelHingeInertiaB, 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);
+            //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(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.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.
-            projection.BiasVelocity.W = positionErrorToVelocity * -error;
-        }
-
-        [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        private static void ApplyImpulse(ref BodyVelocities velocityA, ref BodyVelocities 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<Vector<float>, 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 BodyVelocities velocityA, ref BodyVelocities 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)
-        {
+            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(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.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(velocityA.Linear, velocityB.Linear, out var ballSocketLinearCSV);
+            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(projection.BiasVelocity, csv, out csv);
+            Vector4Wide.Subtract(biasVelocity, csv, out csv);
 
-            Symmetric4x4Wide.TransformWithoutOverlap(csv, projection.EffectiveMass, out var csi);
-            Vector4Wide.Scale(accumulatedImpulse, projection.SoftnessImpulseScale, out var softnessContribution);
+            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(ref velocityA, ref velocityB, ref projection, ref csi);
+            accumulatedImpulses += csi;
+
+            ApplyImpulse(offsetA, offsetB, swivelHingeJacobian, inertiaA, inertiaB, ref csi, ref wsvA, ref wsvB);
         }
 
+        public static bool RequiresIncrementalSubstepUpdates => false;
+        [MethodImpl(MethodImplOptions.AggressiveInlining)]
+        public static void IncrementallyUpdateForSubstep(in Vector<float> dt, in BodyVelocityWide wsvA, in BodyVelocityWide wsvB, ref SwivelHingePrestepData prestepData) { }
     }
 
-    public class SwivelHingeTypeProcessor : TwoBodyTypeProcessor<SwivelHingePrestepData, SwivelHingeProjection, Vector4Wide, SwivelHingeFunctions>
+    public class SwivelHingeTypeProcessor : TwoBodyTypeProcessor<SwivelHingePrestepData, Vector4Wide, SwivelHingeFunctions, AccessNoPosition, AccessNoPosition, AccessAll, AccessAll>
     {
         public const int BatchTypeId = 46;
     }
diff --git a/BepuPhysics/Constraints/ThreeBodyTypeProcessor.cs b/BepuPhysics/Constraints/ThreeBodyTypeProcessor.cs
index 833ad367a..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;
 
@@ -23,38 +21,47 @@ public struct ThreeBodyReferences
     /// </summary>
     /// <typeparam name="TPrestepData">Type of the prestep data used by the constraint.</typeparam>
     /// <typeparam name="TAccumulatedImpulse">Type of the accumulated impulses used by the constraint.</typeparam>
-    /// <typeparam name="TProjection">Type of the projection to input.</typeparam>
-    public interface IThreeBodyConstraintFunctions<TPrestepData, TProjection, TAccumulatedImpulse>
+    public interface IThreeBodyConstraintFunctions<TPrestepData, TAccumulatedImpulse>
     {
-        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 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);
+        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);
+        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,
+            ref TPrestepData prestep, ref TAccumulatedImpulse accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB, ref BodyVelocityWide wsvC);
+
+        /// <summary>
+        /// Gets whether this constraint type requires incremental updates for each substep taken beyond the first.
+        /// </summary>
+        static abstract bool RequiresIncrementalSubstepUpdates { get; }
+        static abstract void IncrementallyUpdateForSubstep(in Vector<float> dt, in BodyVelocityWide wsvA, in BodyVelocityWide wsvB, in BodyVelocityWide wsvC, ref TPrestepData prestepData);
     }
 
     /// <summary>
     /// Shared implementation across all three body constraints.
     /// </summary>
-    public abstract class ThreeBodyTypeProcessor<TPrestepData, TProjection, TAccumulatedImpulse, TConstraintFunctions>
-        : TypeProcessor<ThreeBodyReferences, TPrestepData, TProjection, TAccumulatedImpulse>
-        where TPrestepData : unmanaged where TProjection : unmanaged where TAccumulatedImpulse : unmanaged
-        where TConstraintFunctions : unmanaged, IThreeBodyConstraintFunctions<TPrestepData, TProjection, TAccumulatedImpulse>
+    public abstract class ThreeBodyTypeProcessor<TPrestepData, TAccumulatedImpulse, TConstraintFunctions,
+        TWarmStartAccessFilterA, TWarmStartAccessFilterB, TWarmStartAccessFilterC, TSolveAccessFilterA, TSolveAccessFilterB, TSolveAccessFilterC>
+        : TypeProcessor<ThreeBodyReferences, TPrestepData, TAccumulatedImpulse>
+        where TPrestepData : unmanaged where TAccumulatedImpulse : unmanaged
+        where TConstraintFunctions : unmanaged, IThreeBodyConstraintFunctions<TPrestepData, TAccumulatedImpulse>
+        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;
 
-        public sealed unsafe override void EnumerateConnectedBodyIndices<TEnumerator>(ref TypeBatch typeBatch, int indexInTypeBatch, ref TEnumerator enumerator)
-        {
-            BundleIndexing.GetBundleIndices(indexInTypeBatch, out var constraintBundleIndex, out var constraintInnerIndex);
-            ref var indices = ref GatherScatter.GetOffsetInstance(ref Buffer<ThreeBodyReferences>.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<ThreeBodyReferences>
         {
             [MethodImpl(MethodImplOptions.AggressiveInlining)]
-            public int GetSortKey(int constraintIndex, ref Buffer<ThreeBodyReferences> bodyReferences)
+            public static int GetSortKey(int constraintIndex, ref Buffer<ThreeBodyReferences> bodyReferences)
             {
                 BundleIndexing.GetBundleIndices(constraintIndex, out var bundleIndex, out var innerIndex);
                 ref var bundleReferences = ref bodyReferences[bundleIndex];
@@ -76,7 +83,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<byte> bodyReferencesCache)
         {
             GenerateSortKeysAndCopyReferences<ThreeBodySortKeyGenerator>(
                 ref typeBatch,
@@ -89,129 +96,86 @@ internal sealed override void VerifySortRegion(ref TypeBatch typeBatch, int bund
         {
             VerifySortRegion<ThreeBodySortKeyGenerator>(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)
+       
+        public override void WarmStart<TIntegratorCallbacks, TBatchIntegrationMode, TAllowPoseIntegration>(
+            ref TypeBatch typeBatch, ref Buffer<IndexSet> integrationFlags, Bodies bodies, ref TIntegratorCallbacks integratorCallbacks,
+            float dt, float inverseDt, int startBundle, int exclusiveEndBundle, int workerIndex)
         {
-            ref var prestepBase = ref Unsafe.AsRef<TPrestepData>(typeBatch.PrestepData.Memory);
-            ref var bodyReferencesBase = ref Unsafe.AsRef<ThreeBodyReferences>(typeBatch.BodyReferences.Memory);
-            ref var projectionBase = ref Unsafe.AsRef<TProjection>(typeBatch.Projection.Memory);
-            var function = default(TConstraintFunctions);
+            var prestepBundles = typeBatch.PrestepData.As<TPrestepData>();
+            var bodyReferencesBundles = typeBatch.BodyReferences.As<ThreeBodyReferences>();
+            var accumulatedImpulsesBundles = typeBatch.AccumulatedImpulses.As<TAccumulatedImpulse>();
             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.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);
-            }
-        }
+                ref var prestep = ref prestepBundles[i];
+                ref var accumulatedImpulses = ref accumulatedImpulsesBundles[i];
+                ref var references = ref bodyReferencesBundles[i];
+                GatherAndIntegrate<TIntegratorCallbacks, TBatchIntegrationMode, TWarmStartAccessFilterA, TAllowPoseIntegration>(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<TIntegratorCallbacks, TBatchIntegrationMode, TWarmStartAccessFilterB, TAllowPoseIntegration>(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<TIntegratorCallbacks, TBatchIntegrationMode, TWarmStartAccessFilterC, TAllowPoseIntegration>(bodies, ref integratorCallbacks, ref integrationFlags, 2, dt, workerIndex, i, ref references.IndexC,
+                    out var positionC, out var orientationC, out var wsvC, out var inertiaC);
 
-        public unsafe override void WarmStart(ref TypeBatch typeBatch, ref Buffer<BodyVelocity> bodyVelocities, int startBundle, int exclusiveEndBundle)
-        {
-            ref var bodyReferencesBase = ref Unsafe.AsRef<ThreeBodyReferences>(typeBatch.BodyReferences.Memory);
-            ref var accumulatedImpulsesBase = ref Unsafe.AsRef<TAccumulatedImpulse>(typeBatch.AccumulatedImpulses.Memory);
-            ref var projectionBase = ref Unsafe.AsRef<TProjection>(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);
-                int count = GetCountInBundle(ref typeBatch, i);
-                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);
-                Bodies.ScatterVelocities(ref wsvA, ref wsvB, ref wsvC, ref bodyVelocities, ref bodyReferences, count);
+                //if (typeof(TAllowPoseIntegration) == typeof(AllowPoseIntegration))
+                //    function.UpdateForNewPose(positionA, orientationA, inertiaA, wsvA, positionB, orientationB, inertiaB, wsvB, positionC, orientationC, inertiaC, wsvC, new Vector<float>(dt), accumulatedImpulses, ref prestep);
 
-            }
-        }
+                TConstraintFunctions.WarmStart(positionA, orientationA, inertiaA, positionB, orientationB, inertiaB, positionC, orientationC, inertiaC, ref prestep, ref accumulatedImpulses, ref wsvA, ref wsvB, ref wsvC);
 
-        public unsafe override void SolveIteration(ref TypeBatch typeBatch, ref Buffer<BodyVelocity> bodyVelocities, int startBundle, int exclusiveEndBundle)
-        {
-            ref var bodyReferencesBase = ref Unsafe.AsRef<ThreeBodyReferences>(typeBatch.BodyReferences.Memory);
-            ref var accumulatedImpulsesBase = ref Unsafe.AsRef<TAccumulatedImpulse>(typeBatch.AccumulatedImpulses.Memory);
-            ref var projectionBase = ref Unsafe.AsRef<TProjection>(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);
-                int count = GetCountInBundle(ref typeBatch, i);
-                Bodies.GatherVelocities(ref bodyVelocities, ref bodyReferences, count, out var wsvA, out var wsvB, out var wsvC);
-                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);
-            }
-        }
+                if (typeof(TBatchIntegrationMode) == typeof(BatchShouldNeverIntegrate))
+                {
+                    bodies.ScatterVelocities<TWarmStartAccessFilterA>(ref wsvA, ref references.IndexA);
+                    bodies.ScatterVelocities<TWarmStartAccessFilterB>(ref wsvB, ref references.IndexB);
+                    bodies.ScatterVelocities<TWarmStartAccessFilterC>(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<AccessAll>(ref wsvA, ref references.IndexA);
+                    bodies.ScatterVelocities<AccessAll>(ref wsvB, ref references.IndexB);
+                    bodies.ScatterVelocities<AccessAll>(ref wsvC, ref references.IndexC);
+                }
 
-        public unsafe override void JacobiPrestep(ref TypeBatch typeBatch, Bodies bodies, ref FallbackBatch jacobiBatch, float dt, float inverseDt, int startBundle, int exclusiveEndBundle)
-        {
-            ref var prestepBase = ref Unsafe.AsRef<TPrestepData>(typeBatch.PrestepData.Memory);
-            ref var bodyReferencesBase = ref Unsafe.AsRef<ThreeBodyReferences>(typeBatch.BodyReferences.Memory);
-            ref var projectionBase = ref Unsafe.AsRef<TProjection>(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.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 projection);
             }
         }
-        public unsafe override void JacobiWarmStart(ref TypeBatch typeBatch, ref Buffer<BodyVelocity> bodyVelocities, ref FallbackTypeBatchResults jacobiResults, int startBundle, int exclusiveEndBundle)
+
+        public override void Solve(ref TypeBatch typeBatch, Bodies bodies, float dt, float inverseDt, int startBundle, int exclusiveEndBundle)
         {
-            ref var bodyReferencesBase = ref Unsafe.AsRef<ThreeBodyReferences>(typeBatch.BodyReferences.Memory);
-            ref var accumulatedImpulsesBase = ref Unsafe.AsRef<TAccumulatedImpulse>(typeBatch.AccumulatedImpulses.Memory);
-            ref var projectionBase = ref Unsafe.AsRef<TProjection>(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);
+            var prestepBundles = typeBatch.PrestepData.As<TPrestepData>();
+            var bodyReferencesBundles = typeBatch.BodyReferences.As<ThreeBodyReferences>();
+            var accumulatedImpulsesBundles = typeBatch.AccumulatedImpulses.As<TAccumulatedImpulse>();
             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.GatherVelocities(ref bodyVelocities, ref bodyReferences, count, out wsvA, out wsvB, out wsvC);
-                function.WarmStart(ref wsvA, ref wsvB, ref wsvC, ref projection, ref accumulatedImpulses);
+                ref var prestep = ref prestepBundles[i];
+                ref var accumulatedImpulses = ref accumulatedImpulsesBundles[i];
+                ref var references = ref bodyReferencesBundles[i];
+                bodies.GatherState<TSolveAccessFilterA>(references.IndexA, true, out var positionA, out var orientationA, out var wsvA, out var inertiaA);
+                bodies.GatherState<TSolveAccessFilterB>(references.IndexB, true, out var positionB, out var orientationB, out var wsvB, out var inertiaB);
+                bodies.GatherState<TSolveAccessFilterC>(references.IndexC, true, out var positionC, out var orientationC, out var wsvC, out var inertiaC);
+
+                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<TSolveAccessFilterA>(ref wsvA, ref references.IndexA);
+                bodies.ScatterVelocities<TSolveAccessFilterB>(ref wsvB, ref references.IndexB);
+                bodies.ScatterVelocities<TSolveAccessFilterC>(ref wsvC, ref references.IndexC);
             }
         }
-        public unsafe override void JacobiSolveIteration(ref TypeBatch typeBatch, ref Buffer<BodyVelocity> bodyVelocities, ref FallbackTypeBatchResults jacobiResults, 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)
         {
-            ref var bodyReferencesBase = ref Unsafe.AsRef<ThreeBodyReferences>(typeBatch.BodyReferences.Memory);
-            ref var accumulatedImpulsesBase = ref Unsafe.AsRef<TAccumulatedImpulse>(typeBatch.AccumulatedImpulses.Memory);
-            ref var projectionBase = ref Unsafe.AsRef<TProjection>(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);
+            var prestepBundles = typeBatch.PrestepData.As<TPrestepData>();
+            var bodyReferencesBundles = typeBatch.BodyReferences.As<ThreeBodyReferences>();
+            var dtWide = new Vector<float>(dt);
             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.GatherVelocities(ref bodyVelocities, ref bodyReferences, count, out wsvA, out wsvB, out wsvC);
-                function.Solve(ref wsvA, ref wsvB, ref wsvC, ref projection, ref accumulatedImpulses);
+                ref var prestep = ref prestepBundles[i];
+                ref var references = ref bodyReferencesBundles[i];
+                bodies.GatherState<AccessOnlyVelocity>(references.IndexA, true, out _, out _, out var wsvA, out _);
+                bodies.GatherState<AccessOnlyVelocity>(references.IndexB, true, out _, out _, out var wsvB, out _);
+                bodies.GatherState<AccessOnlyVelocity>(references.IndexC, true, out _, out _, out var wsvC, out _);
+                TConstraintFunctions.IncrementallyUpdateForSubstep(dtWide, wsvA, wsvB, wsvC, ref prestep);
             }
         }
-
     }
 }
diff --git a/BepuPhysics/Constraints/TwistLimit.cs b/BepuPhysics/Constraints/TwistLimit.cs
index d977f4140..23468c4b8 100644
--- a/BepuPhysics/Constraints/TwistLimit.cs
+++ b/BepuPhysics/Constraints/TwistLimit.cs
@@ -36,7 +36,7 @@ public struct TwistLimit : ITwoBodyConstraintDescription<TwistLimit>
         /// </summary>
         public SpringSettings SpringSettings;
 
-        public readonly int ConstraintTypeId
+        public static int ConstraintTypeId
         {
             [MethodImpl(MethodImplOptions.AggressiveInlining)]
             get
@@ -45,7 +45,8 @@ public readonly int ConstraintTypeId
             }
         }
 
-        public readonly Type TypeProcessorType => typeof(TwistLimitTypeProcessor);
+        public static Type TypeProcessorType => typeof(TwistLimitTypeProcessor);
+        public static TypeProcessor CreateTypeProcessor() => new TwistLimitTypeProcessor();
 
         public readonly void ApplyDescription(ref TypeBatch batch, int bundleIndex, int innerIndex)
         {
@@ -61,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<TwistLimitPrestepData>.Get(ref batch.PrestepData, bundleIndex), innerIndex);
@@ -82,72 +83,60 @@ public struct TwistLimitPrestepData
         public SpringSettingsWide SpringSettings;
     }
 
-    public struct TwistLimitProjection
-    {
-        public Vector3Wide VelocityToImpulseA;
-        public Vector<float> BiasImpulse;
-        public Vector<float> SoftnessImpulseScale;
-        public Vector3Wide ImpulseToVelocityA;
-        public Vector3Wide NegatedImpulseToVelocityB;
-    }
-
-
-    public struct TwistLimitFunctions : IConstraintFunctions<TwistLimitPrestepData, TwistLimitProjection, Vector<float>>
+    public struct TwistLimitFunctions : ITwoBodyConstraintFunctions<TwistLimitPrestepData, Vector<float>>
     {
         [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)
+        static void ComputeJacobian(in QuaternionWide orientationA, in QuaternionWide orientationB,
+            in QuaternionWide localBasisA, in QuaternionWide localBasisB, in Vector<float> minimumAngle, in Vector<float> maximumAngle, out Vector<float> error, out Vector3Wide jacobianA)
         {
-            Unsafe.SkipInit(out projection);
-            TwistServoFunctions.ComputeJacobian(bodies, bodyReferences, count, prestep.LocalBasisA, prestep.LocalBasisB,
-                out var basisBX, out var basisBZ, out var basisA, out var 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(prestep.MinimumAngle, angle, out var minError);
-            MathHelper.GetSignedAngleDifference(prestep.MaximumAngle, angle, out var maxError);
+            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.
-            var error = Vector.ConditionalSelect(useMin, -minError, maxError);
+            error = Vector.ConditionalSelect(useMin, -minError, maxError);
             Vector3Wide.Negate(jacobianA, out var negatedJacobianA);
             Vector3Wide.ConditionalSelect(useMin, negatedJacobianA, jacobianA, out jacobianA);
+        }
+        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<float> 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);
+            Symmetric3x3Wide.TransformWithoutOverlap(jacobianA, inertiaB.InverseInertiaTensor, out var negatedImpulseToVelocityB);
+            TwistServoFunctions.ApplyImpulse(ref wsvA.Angular, ref wsvB.Angular, impulseToVelocityA, negatedImpulseToVelocityB, accumulatedImpulses);
+        }
+
+        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<float> accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB)
+        {
+            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,
-                ref projection.ImpulseToVelocityA, ref projection.NegatedImpulseToVelocityB,
-                out var positionErrorToVelocity, out projection.SoftnessImpulseScale, out var effectiveMass, out projection.VelocityToImpulseA);
+                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<float>.Zero), error * inverseDt, error * positionErrorToVelocity);
-            projection.BiasImpulse = biasVelocity * effectiveMass;
-        }
-
-
-        [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public void WarmStart(ref BodyVelocities velocityA, ref BodyVelocities velocityB, ref TwistLimitProjection projection, ref Vector<float> accumulatedImpulse)
-        {
-            TwistServoFunctions.ApplyImpulse(ref velocityA.Angular, ref velocityB.Angular, projection.ImpulseToVelocityA, projection.NegatedImpulseToVelocityB, accumulatedImpulse);
-        }
+            var biasImpulse = biasVelocity * effectiveMass;
 
-        [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public void Solve(ref BodyVelocities velocityA, ref BodyVelocities velocityB, ref TwistLimitProjection projection, ref Vector<float> accumulatedImpulse)
-        {
-            Vector3Wide.Subtract(velocityA.Angular, velocityB.Angular, out var netVelocity);
-            Vector3Wide.Dot(netVelocity, projection.VelocityToImpulseA, out var csiVelocityComponent);
+            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 = projection.BiasImpulse - accumulatedImpulse * projection.SoftnessImpulseScale - csiVelocityComponent;
-            var previousAccumulatedImpulse = accumulatedImpulse;
-            accumulatedImpulse = Vector.Max(Vector<float>.Zero, accumulatedImpulse + csi);
-            csi = accumulatedImpulse - previousAccumulatedImpulse;
+            var csi = biasImpulse - accumulatedImpulses * softnessImpulseScale - csiVelocityComponent;
+            InequalityHelpers.ClampPositive(ref accumulatedImpulses, ref csi);
 
-            TwistServoFunctions.ApplyImpulse(ref velocityA.Angular, ref velocityB.Angular, projection.ImpulseToVelocityA, projection.NegatedImpulseToVelocityB, csi);
+            TwistServoFunctions.ApplyImpulse(ref wsvA.Angular, ref wsvB.Angular, impulseToVelocityA, negatedImpulseToVelocityB, csi);
         }
 
+        public static bool RequiresIncrementalSubstepUpdates => false;
+        [MethodImpl(MethodImplOptions.AggressiveInlining)]
+        public static void IncrementallyUpdateForSubstep(in Vector<float> dt, in BodyVelocityWide wsvA, in BodyVelocityWide wsvB, ref TwistLimitPrestepData prestepData) { }
     }
 
-    public class TwistLimitTypeProcessor : TwoBodyTypeProcessor<TwistLimitPrestepData, TwistLimitProjection, Vector<float>, TwistLimitFunctions>
+    public class TwistLimitTypeProcessor : TwoBodyTypeProcessor<TwistLimitPrestepData, Vector<float>, TwistLimitFunctions, AccessOnlyAngular, AccessOnlyAngular, AccessOnlyAngular, AccessOnlyAngular>
     {
         public const int BatchTypeId = 27;
     }
diff --git a/BepuPhysics/Constraints/TwistMotor.cs b/BepuPhysics/Constraints/TwistMotor.cs
index 206bfb5c3..3ae2ab81d 100644
--- a/BepuPhysics/Constraints/TwistMotor.cs
+++ b/BepuPhysics/Constraints/TwistMotor.cs
@@ -30,7 +30,7 @@ public struct TwistMotor : ITwoBodyConstraintDescription<TwistMotor>
         /// </summary>
         public MotorSettings Settings;
 
-        public readonly int ConstraintTypeId
+        public static int ConstraintTypeId
         {
             [MethodImpl(MethodImplOptions.AggressiveInlining)]
             get
@@ -39,7 +39,8 @@ public readonly int ConstraintTypeId
             }
         }
 
-        public readonly Type TypeProcessorType => typeof(TwistMotorTypeProcessor);
+        public static Type TypeProcessorType => typeof(TwistMotorTypeProcessor);
+        public static TypeProcessor CreateTypeProcessor() => new TwistMotorTypeProcessor();
 
         public readonly void ApplyDescription(ref TypeBatch batch, int bundleIndex, int innerIndex)
         {
@@ -54,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<TwistMotorPrestepData>.Get(ref batch.PrestepData, bundleIndex), innerIndex);
@@ -73,67 +74,58 @@ public struct TwistMotorPrestepData
         public MotorSettingsWide Settings;
     }
 
-    public struct TwistMotorProjection
-    {
-        public Vector3Wide VelocityToImpulseA;
-        public Vector<float> SoftnessImpulseScale;
-        public Vector<float> BiasImpulse;
-        public Vector<float> MaximumImpulse;
-        public Vector3Wide ImpulseToVelocityA;
-        public Vector3Wide NegatedImpulseToVelocityB;
-    }
-
-
-    public struct TwistMotorFunctions : IConstraintFunctions<TwistMotorPrestepData, TwistMotorProjection, Vector<float>>
+    public struct TwistMotorFunctions : ITwoBodyConstraintFunctions<TwistMotorPrestepData, Vector<float>>
     {
         [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 static void ComputeJacobian(in QuaternionWide orientationA, in QuaternionWide orientationB, in Vector3Wide localAxisA, in Vector3Wide localAxisB, out Vector3Wide jacobianA)
         {
-            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);
-            Vector3Wide.Add(axisA, axisB, out var jacobianA);
+            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<float>.One / length, out jacobianA);
             Vector3Wide.ConditionalSelect(Vector.LessThan(length, new Vector<float>(1e-10f)), axisA, jacobianA, out jacobianA);
-
-            TwistServoFunctions.ComputeEffectiveMassContributions(inertiaA.InverseInertiaTensor, inertiaB.InverseInertiaTensor, jacobianA,
-                ref projection.ImpulseToVelocityA, ref 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 BodyVelocities velocityA, ref BodyVelocities velocityB, ref TwistMotorProjection projection, ref Vector<float> accumulatedImpulse)
+        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<float> accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB)
         {
-            TwistServoFunctions.ApplyImpulse(ref velocityA.Angular, ref velocityB.Angular, projection.ImpulseToVelocityA, projection.NegatedImpulseToVelocityB, accumulatedImpulse);
+            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);
         }
 
-        [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public void Solve(ref BodyVelocities velocityA, ref BodyVelocities velocityB, ref TwistMotorProjection projection, ref Vector<float> accumulatedImpulse)
+        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<float> accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB)
         {
-            Vector3Wide.Subtract(velocityA.Angular, velocityB.Angular, out var netVelocity);
-            Vector3Wide.Dot(netVelocity, projection.VelocityToImpulseA, out var csiVelocityComponent);
+            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 = projection.BiasImpulse - accumulatedImpulse * projection.SoftnessImpulseScale - csiVelocityComponent;
-            var previousAccumulatedImpulse = accumulatedImpulse;
-            accumulatedImpulse = Vector.Max(Vector.Min(accumulatedImpulse + csi, projection.MaximumImpulse), -projection.MaximumImpulse);
-            csi = accumulatedImpulse - previousAccumulatedImpulse;
+            var csi = biasImpulse - accumulatedImpulses * softnessImpulseScale - csiVelocityComponent;
+            var previousAccumulatedImpulse = accumulatedImpulses;
+            accumulatedImpulses = Vector.Max(Vector.Min(accumulatedImpulses + csi, maximumImpulse), -maximumImpulse);
+            csi = accumulatedImpulses - previousAccumulatedImpulse;
 
-            TwistServoFunctions.ApplyImpulse(ref velocityA.Angular, ref velocityB.Angular, projection.ImpulseToVelocityA, projection.NegatedImpulseToVelocityB, csi);
+            TwistServoFunctions.ApplyImpulse(ref wsvA.Angular, ref wsvB.Angular, impulseToVelocityA, negatedImpulseToVelocityB, csi);
         }
 
+        public static bool RequiresIncrementalSubstepUpdates => false;
+        [MethodImpl(MethodImplOptions.AggressiveInlining)]
+        public static void IncrementallyUpdateForSubstep(in Vector<float> dt, in BodyVelocityWide wsvA, in BodyVelocityWide wsvB, ref TwistMotorPrestepData prestepData) { }
     }
 
-    public class TwistMotorTypeProcessor : TwoBodyTypeProcessor<TwistMotorPrestepData, TwistMotorProjection, Vector<float>, TwistMotorFunctions>
+    public class TwistMotorTypeProcessor : TwoBodyTypeProcessor<TwistMotorPrestepData, Vector<float>, TwistMotorFunctions, AccessOnlyAngular, AccessOnlyAngular, AccessOnlyAngular, AccessOnlyAngular>
     {
         public const int BatchTypeId = 28;
     }
diff --git a/BepuPhysics/Constraints/TwistServo.cs b/BepuPhysics/Constraints/TwistServo.cs
index c60ba3848..08e9a75eb 100644
--- a/BepuPhysics/Constraints/TwistServo.cs
+++ b/BepuPhysics/Constraints/TwistServo.cs
@@ -36,7 +36,7 @@ public struct TwistServo : ITwoBodyConstraintDescription<TwistServo>
         /// </summary>
         public ServoSettings ServoSettings;
 
-        public readonly int ConstraintTypeId
+        public static int ConstraintTypeId
         {
             [MethodImpl(MethodImplOptions.AggressiveInlining)]
             get
@@ -45,7 +45,8 @@ public readonly int ConstraintTypeId
             }
         }
 
-        public readonly Type TypeProcessorType => typeof(TwistServoTypeProcessor);
+        public static Type TypeProcessorType => typeof(TwistServoTypeProcessor);
+        public static TypeProcessor CreateTypeProcessor() => new TwistServoTypeProcessor();
 
         public readonly void ApplyDescription(ref TypeBatch batch, int bundleIndex, int innerIndex)
         {
@@ -61,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<TwistServoPrestepData>.Get(ref batch.PrestepData, bundleIndex), innerIndex);
@@ -82,26 +83,12 @@ public struct TwistServoPrestepData
         public ServoSettingsWide ServoSettings;
     }
 
-    public struct TwistServoProjection
-    {
-        public Vector3Wide VelocityToImpulseA;
-        public Vector<float> BiasImpulse;
-        public Vector<float> SoftnessImpulseScale;
-        public Vector<float> MaximumImpulse;
-        public Vector3Wide ImpulseToVelocityA;
-        public Vector3Wide NegatedImpulseToVelocityB;
-    }
-
-
-    public struct TwistServoFunctions : IConstraintFunctions<TwistServoPrestepData, TwistServoProjection, Vector<float>>
+    public struct TwistServoFunctions : ITwoBodyConstraintFunctions<TwistServoPrestepData, Vector<float>>
     {
         [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.
@@ -138,14 +125,14 @@ 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<float>.Zero), -absAngle, absAngle);
         }
 
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
         public static void ComputeEffectiveMassContributions(
            in Symmetric3x3Wide inverseInertiaA, in Symmetric3x3Wide inverseInertiaB, in Vector3Wide jacobianA,
-           ref Vector3Wide impulseToVelocityA, ref Vector3Wide negatedImpulseToVelocityB, out Vector<float> unsoftenedInverseEffectiveMass)
+           out Vector3Wide impulseToVelocityA, out Vector3Wide negatedImpulseToVelocityB, out Vector<float> 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.
@@ -159,35 +146,16 @@ 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<float> positionErrorToVelocity, out Vector<float> softnessImpulseScale,
+            out Vector3Wide impulseToVelocityA, out Vector3Wide negatedImpulseToVelocityB, out Vector<float> positionErrorToVelocity, out Vector<float> softnessImpulseScale,
             out Vector<float> 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;
             Vector3Wide.Scale(jacobianA, effectiveMass, out velocityToImpulseA);
         }
 
-        [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)
-        {
-            Unsafe.SkipInit(out projection);
-            ComputeJacobian(bodies, bodyReferences, count, 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,
-                ref projection.ImpulseToVelocityA, ref 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<float> csi)
@@ -199,28 +167,61 @@ 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<float> accumulatedImpulse)
+        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<float>.One / length, out jacobianA);
+            Vector3Wide.ConditionalSelect(Vector.LessThan(length, new Vector<float>(1e-10f)), basisAZ, jacobianA, out jacobianA);
+        }
+
+        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<float> accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB)
         {
-            ApplyImpulse(ref velocityA.Angular, ref velocityB.Angular, projection.ImpulseToVelocityA, projection.NegatedImpulseToVelocityB, accumulatedImpulse);
+            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);
         }
 
-        [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public void Solve(ref BodyVelocities velocityA, ref BodyVelocities velocityB, ref TwistServoProjection projection, ref Vector<float> accumulatedImpulse)
+        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<float> accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB)
         {
-            Vector3Wide.Subtract(velocityA.Angular, velocityB.Angular, out var netVelocity);
-            Vector3Wide.Dot(netVelocity, projection.VelocityToImpulseA, out var csiVelocityComponent);
+            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 = projection.BiasImpulse - accumulatedImpulse * projection.SoftnessImpulseScale - csiVelocityComponent;
-            var previousAccumulatedImpulse = accumulatedImpulse;
-            accumulatedImpulse = Vector.Min(Vector.Max(accumulatedImpulse + csi, -projection.MaximumImpulse), projection.MaximumImpulse);
-            csi = accumulatedImpulse - previousAccumulatedImpulse;
+            var csi = biasImpulse - accumulatedImpulses * softnessImpulseScale - csiVelocityComponent;
+            var previousAccumulatedImpulse = accumulatedImpulses;
+            accumulatedImpulses = Vector.Min(Vector.Max(accumulatedImpulses + csi, -maximumImpulse), maximumImpulse);
+            csi = accumulatedImpulses - previousAccumulatedImpulse;
 
-            ApplyImpulse(ref velocityA.Angular, ref velocityB.Angular, projection.ImpulseToVelocityA, projection.NegatedImpulseToVelocityB, csi);
+            ApplyImpulse(ref wsvA.Angular, ref wsvB.Angular, impulseToVelocityA, negatedImpulseToVelocityB, csi);
         }
 
+        public static bool RequiresIncrementalSubstepUpdates => false;
+        [MethodImpl(MethodImplOptions.AggressiveInlining)]
+        public static void IncrementallyUpdateForSubstep(in Vector<float> dt, in BodyVelocityWide wsvA, in BodyVelocityWide wsvB, ref TwistServoPrestepData prestepData) { }
     }
 
-    public class TwistServoTypeProcessor : TwoBodyTypeProcessor<TwistServoPrestepData, TwistServoProjection, Vector<float>, TwistServoFunctions>
+    public class TwistServoTypeProcessor : TwoBodyTypeProcessor<TwistServoPrestepData, Vector<float>, TwistServoFunctions, AccessOnlyAngular, AccessOnlyAngular, AccessOnlyAngular, AccessOnlyAngular>
     {
         public const int BatchTypeId = 26;
     }
diff --git a/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs b/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs
index 7a65e7720..a6ff48093 100644
--- a/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs
+++ b/BepuPhysics/Constraints/TwoBodyTypeProcessor.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;
 
@@ -23,23 +21,18 @@ public struct TwoBodyReferences
     /// </summary>
     /// <typeparam name="TPrestepData">Type of the prestep data used by the constraint.</typeparam>
     /// <typeparam name="TAccumulatedImpulse">Type of the accumulated impulses used by the constraint.</typeparam>
-    /// <typeparam name="TProjection">Type of the projection to input.</typeparam>
-    public interface IConstraintFunctions<TPrestepData, TProjection, TAccumulatedImpulse>
+    public interface ITwoBodyConstraintFunctions<TPrestepData, TAccumulatedImpulse>
     {
-        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 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);
-    }
-
-    /// <summary>
-    /// Prestep, warm start, solve iteration, and incremental contact update functions for a two body contact constraint type.
-    /// </summary>
-    /// <typeparam name="TPrestepData">Type of the prestep data used by the constraint.</typeparam>
-    /// <typeparam name="TAccumulatedImpulse">Type of the accumulated impulses used by the constraint.</typeparam>
-    /// <typeparam name="TProjection">Type of the projection to input.</typeparam>
-    public interface IContactConstraintFunctions<TPrestepData, TProjection, TAccumulatedImpulse> : IConstraintFunctions<TPrestepData, TProjection, TAccumulatedImpulse>
-    {
-        void IncrementallyUpdateContactData(in Vector<float> dt, in BodyVelocities velocityA, in BodyVelocities velocityB, ref TPrestepData prestepData);
+        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);
+        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);
+
+        /// <summary>
+        /// Gets whether this constraint type requires incremental updates for each substep taken beyond the first.
+        /// </summary>
+        static abstract bool RequiresIncrementalSubstepUpdates { get; }
+        static abstract void IncrementallyUpdateForSubstep(in Vector<float> 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.
@@ -47,27 +40,22 @@ public interface IContactConstraintFunctions<TPrestepData, TProjection, TAccumul
     /// <summary>
     /// Shared implementation across all two body constraints.
     /// </summary>
-    public abstract class TwoBodyTypeProcessor<TPrestepData, TProjection, TAccumulatedImpulse, TConstraintFunctions>
-        : TypeProcessor<TwoBodyReferences, TPrestepData, TProjection, TAccumulatedImpulse>
-        where TPrestepData : unmanaged where TProjection : unmanaged where TAccumulatedImpulse : unmanaged
-        where TConstraintFunctions : unmanaged, IConstraintFunctions<TPrestepData, TProjection, TAccumulatedImpulse>
+    public abstract class TwoBodyTypeProcessor<TPrestepData, TAccumulatedImpulse, TConstraintFunctions,
+        TWarmStartAccessFilterA, TWarmStartAccessFilterB, TSolveAccessFilterA, TSolveAccessFilterB>
+        : TypeProcessor<TwoBodyReferences, TPrestepData, TAccumulatedImpulse>
+        where TPrestepData : unmanaged where TAccumulatedImpulse : unmanaged
+        where TConstraintFunctions : unmanaged, ITwoBodyConstraintFunctions<TPrestepData, TAccumulatedImpulse>
+        where TWarmStartAccessFilterA : unmanaged, IBodyAccessFilter
+        where TWarmStartAccessFilterB : unmanaged, IBodyAccessFilter
+        where TSolveAccessFilterA : unmanaged, IBodyAccessFilter
+        where TSolveAccessFilterB : unmanaged, IBodyAccessFilter
     {
         protected sealed override int InternalBodiesPerConstraint => 2;
 
-        public sealed override void EnumerateConnectedBodyIndices<TEnumerator>(ref TypeBatch typeBatch, int indexInTypeBatch, ref TEnumerator enumerator)
-        {
-            BundleIndexing.GetBundleIndices(indexInTypeBatch, out var constraintBundleIndex, out var constraintInnerIndex);
-            ref var indexA = ref GatherScatter.Get(ref Buffer<TwoBodyReferences>.Get(ref typeBatch.BodyReferences, constraintBundleIndex).IndexA, constraintInnerIndex);
-            ref var indexB = ref Unsafe.Add(ref indexA, Vector<int>.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);
-        }
         struct TwoBodySortKeyGenerator : ISortKeyGenerator<TwoBodyReferences>
         {
             [MethodImpl(MethodImplOptions.AggressiveInlining)]
-            public int GetSortKey(int constraintIndex, ref Buffer<TwoBodyReferences> bodyReferences)
+            public static int GetSortKey(int constraintIndex, ref Buffer<TwoBodyReferences> bodyReferences)
             {
                 BundleIndexing.GetBundleIndices(constraintIndex, out var bundleIndex, out var innerIndex);
                 ref var bundleReferences = ref bodyReferences[bundleIndex];
@@ -102,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<byte> bodyReferencesCache)
         {
             GenerateSortKeysAndCopyReferences<TwoBodySortKeyGenerator>(
                 ref typeBatch,
@@ -116,6 +104,58 @@ internal sealed override void VerifySortRegion(ref TypeBatch typeBatch, int bund
             VerifySortRegion<TwoBodySortKeyGenerator>(ref typeBatch, bundleStartIndex, constraintCount, ref sortedKeys, ref sortedSourceIndices);
         }
 
+        //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<TwoBodyReferences> references, ref Buffer<SolverState> 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<float>.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<TwoBodyReferences> references, ref Buffer<SolverState> states, int bundleIndex, int exclusiveEndBundleIndex)
+        //{
+        //    var targetIndex = bundleIndex + prefetchDistance;
+        //    if (targetIndex < exclusiveEndBundleIndex)
+        //    {
+        //        PrefetchBundle(states.Memory, ref references[targetIndex], GetCountInBundle(ref typeBatch, targetIndex));
+        //    }
+        //}
+
 
 
         //The following covers the common loop logic for all two body constraints. Each iteration invokes the warm start function type.
@@ -125,146 +165,86 @@ 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 Prestep(ref TypeBatch typeBatch, Bodies bodies, float dt, float inverseDt, int startBundle, int exclusiveEndBundle)
+        public override void WarmStart<TIntegratorCallbacks, TBatchIntegrationMode, TAllowPoseIntegration>(
+            ref TypeBatch typeBatch, ref Buffer<IndexSet> integrationFlags, Bodies bodies, ref TIntegratorCallbacks integratorCallbacks,
+            float dt, float inverseDt, int startBundle, int exclusiveEndBundle, int workerIndex)
         {
-            ref var prestepBase = ref Unsafe.AsRef<TPrestepData>(typeBatch.PrestepData.Memory);
-            ref var bodyReferencesBase = ref Unsafe.AsRef<TwoBodyReferences>(typeBatch.BodyReferences.Memory);
-            ref var projectionBase = ref Unsafe.AsRef<TProjection>(typeBatch.Projection.Memory);
-            var function = default(TConstraintFunctions);
+            var prestepBundles = typeBatch.PrestepData.As<TPrestepData>();
+            var bodyReferencesBundles = typeBatch.BodyReferences.As<TwoBodyReferences>();
+            var accumulatedImpulsesBundles = typeBatch.AccumulatedImpulses.As<TAccumulatedImpulse>();
+            //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 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);
+                ref var prestep = ref prestepBundles[i];
+                ref var accumulatedImpulses = ref accumulatedImpulsesBundles[i];
+                ref var references = ref bodyReferencesBundles[i];
+                //Prefetch(WarmStartPrefetchDistance, ref typeBatch, ref bodyReferencesBundles, ref states, i, exclusiveEndBundle);
+                GatherAndIntegrate<TIntegratorCallbacks, TBatchIntegrationMode, TWarmStartAccessFilterA, TAllowPoseIntegration>(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<TIntegratorCallbacks, TBatchIntegrationMode, TWarmStartAccessFilterB, TAllowPoseIntegration>(bodies, ref integratorCallbacks, ref integrationFlags, 1, dt, workerIndex, i, ref references.IndexB,
+                    out var positionB, out var orientationB, out var wsvB, out var inertiaB);
+
+                TConstraintFunctions.WarmStart(positionA, orientationA, inertiaA, positionB, orientationB, inertiaB, ref prestep, ref accumulatedImpulses, ref wsvA, ref wsvB);
+
+                if (typeof(TBatchIntegrationMode) == typeof(BatchShouldNeverIntegrate))
+                {
+                    bodies.ScatterVelocities<TWarmStartAccessFilterA>(ref wsvA, ref references.IndexA);
+                    bodies.ScatterVelocities<TWarmStartAccessFilterB>(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<AccessAll>(ref wsvA, ref references.IndexA);
+                    bodies.ScatterVelocities<AccessAll>(ref wsvB, ref references.IndexB);
+                }
+
             }
         }
 
-        public unsafe override void WarmStart(ref TypeBatch typeBatch, ref Buffer<BodyVelocity> bodyVelocities, int startBundle, int exclusiveEndBundle)
+        public override void Solve(ref TypeBatch typeBatch, Bodies bodies, float dt, float inverseDt, int startBundle, int exclusiveEndBundle)
         {
-            ref var bodyReferencesBase = ref Unsafe.AsRef<TwoBodyReferences>(typeBatch.BodyReferences.Memory);
-            ref var accumulatedImpulsesBase = ref Unsafe.AsRef<TAccumulatedImpulse>(typeBatch.AccumulatedImpulses.Memory);
-            ref var projectionBase = ref Unsafe.AsRef<TProjection>(typeBatch.Projection.Memory);
-            var function = default(TConstraintFunctions);
+            var prestepBundles = typeBatch.PrestepData.As<TPrestepData>();
+            var bodyReferencesBundles = typeBatch.BodyReferences.As<TwoBodyReferences>();
+            var accumulatedImpulsesBundles = typeBatch.AccumulatedImpulses.As<TAccumulatedImpulse>();
+            //EarlyPrefetch(SolvePrefetchDistance, ref typeBatch, ref bodyReferencesBundles, ref motionStates, startBundle, exclusiveEndBundle);
             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);
-                Bodies.GatherVelocities(ref bodyVelocities, ref bodyReferences, count, out var wsvA, out var wsvB); 
-                function.WarmStart(ref wsvA, ref wsvB, ref projection, ref accumulatedImpulses);
-                Bodies.ScatterVelocities(ref wsvA, ref wsvB, ref bodyVelocities, ref bodyReferences, count);
+                ref var prestep = ref prestepBundles[i];
+                ref var accumulatedImpulses = ref accumulatedImpulsesBundles[i];
+                ref var references = ref bodyReferencesBundles[i];
+                //Prefetch(SolvePrefetchDistance, ref typeBatch, ref bodyReferencesBundles, ref motionStates, i, exclusiveEndBundle);
+                bodies.GatherState<TSolveAccessFilterA>(references.IndexA, true, out var positionA, out var orientationA, out var wsvA, out var inertiaA);
+                bodies.GatherState<TSolveAccessFilterB>(references.IndexB, true, out var positionB, out var orientationB, out var wsvB, out var inertiaB);
 
-            }
-        }
+                TConstraintFunctions.Solve(positionA, orientationA, inertiaA, positionB, orientationB, inertiaB, dt, inverseDt, ref prestep, ref accumulatedImpulses, ref wsvA, ref wsvB);
 
-        public unsafe override void SolveIteration(ref TypeBatch typeBatch, ref Buffer<BodyVelocity> bodyVelocities, int startBundle, int exclusiveEndBundle)
-        {
-            ref var bodyReferencesBase = ref Unsafe.AsRef<TwoBodyReferences>(typeBatch.BodyReferences.Memory);
-            ref var accumulatedImpulsesBase = ref Unsafe.AsRef<TAccumulatedImpulse>(typeBatch.AccumulatedImpulses.Memory);
-            ref var projectionBase = ref Unsafe.AsRef<TProjection>(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);
-                int count = GetCountInBundle(ref typeBatch, i);
-                Bodies.GatherVelocities(ref bodyVelocities, ref bodyReferences, count, out var wsvA, out var wsvB);
-                function.Solve(ref wsvA, ref wsvB, ref projection, ref accumulatedImpulses); 
-                Bodies.ScatterVelocities(ref wsvA, ref wsvB, ref bodyVelocities, ref bodyReferences, count);
+                bodies.ScatterVelocities<TSolveAccessFilterA>(ref wsvA, ref references.IndexA);
+                bodies.ScatterVelocities<TSolveAccessFilterB>(ref wsvB, ref references.IndexB);
             }
         }
 
-        public unsafe override void JacobiPrestep(ref TypeBatch typeBatch, Bodies bodies, ref FallbackBatch jacobiBatch, 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)
         {
-            ref var prestepBase = ref Unsafe.AsRef<TPrestepData>(typeBatch.PrestepData.Memory);
-            ref var bodyReferencesBase = ref Unsafe.AsRef<TwoBodyReferences>(typeBatch.BodyReferences.Memory);
-            ref var projectionBase = ref Unsafe.AsRef<TProjection>(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.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 projection);
-            }
-        }
-        public unsafe override void JacobiWarmStart(ref TypeBatch typeBatch, ref Buffer<BodyVelocity> bodyVelocities, ref FallbackTypeBatchResults jacobiResults, int startBundle, int exclusiveEndBundle)
-        {
-            ref var bodyReferencesBase = ref Unsafe.AsRef<TwoBodyReferences>(typeBatch.BodyReferences.Memory);
-            ref var accumulatedImpulsesBase = ref Unsafe.AsRef<TAccumulatedImpulse>(typeBatch.AccumulatedImpulses.Memory);
-            ref var projectionBase = ref Unsafe.AsRef<TProjection>(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.GatherVelocities(ref bodyVelocities, ref bodyReferences, count, out wsvA, out wsvB);
-                function.WarmStart(ref wsvA, ref wsvB, ref projection, ref accumulatedImpulses);
-            }
-        }
-        public unsafe override void JacobiSolveIteration(ref TypeBatch typeBatch, ref Buffer<BodyVelocity> bodyVelocities, ref FallbackTypeBatchResults jacobiResults, int startBundle, int exclusiveEndBundle)
-        {
-            ref var bodyReferencesBase = ref Unsafe.AsRef<TwoBodyReferences>(typeBatch.BodyReferences.Memory);
-            ref var accumulatedImpulsesBase = ref Unsafe.AsRef<TAccumulatedImpulse>(typeBatch.AccumulatedImpulses.Memory);
-            ref var projectionBase = ref Unsafe.AsRef<TProjection>(typeBatch.Projection.Memory);
-            var function = default(TConstraintFunctions);
-            ref var jacobiResultsBundlesA = ref jacobiResults.GetVelocitiesForBody(0);
-            ref var jacobiResultsBundlesB = ref jacobiResults.GetVelocitiesForBody(1);
+            var prestepBundles = typeBatch.PrestepData.As<TPrestepData>();
+            var bodyReferencesBundles = typeBatch.BodyReferences.As<TwoBodyReferences>();
+            var dtWide = new Vector<float>(dt);
             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.GatherVelocities(ref bodyVelocities, ref bodyReferences, count, out wsvA, out wsvB);
-                function.Solve(ref wsvA, ref wsvB, ref projection, ref accumulatedImpulses);
+                ref var prestep = ref prestepBundles[i];
+                ref var references = ref bodyReferencesBundles[i];
+                bodies.GatherState<AccessOnlyVelocity>(references.IndexA, true, out _, out _, out var wsvA, out _);
+                bodies.GatherState<AccessOnlyVelocity>(references.IndexB, true, out _, out _, out var wsvB, out _);
+                TConstraintFunctions.IncrementallyUpdateForSubstep(dtWide, wsvA, wsvB, ref prestep);
             }
         }
-
     }
 
-    public abstract class TwoBodyContactTypeProcessor<TPrestepData, TProjection, TAccumulatedImpulse, TConstraintFunctions>
-        : TwoBodyTypeProcessor<TPrestepData, TProjection, TAccumulatedImpulse, TConstraintFunctions>
-        where TPrestepData : unmanaged where TProjection : unmanaged where TAccumulatedImpulse : unmanaged
-        where TConstraintFunctions : unmanaged, IContactConstraintFunctions<TPrestepData, TProjection, TAccumulatedImpulse>
+    public abstract class TwoBodyContactTypeProcessor<TPrestepData, TAccumulatedImpulse, TConstraintFunctions>
+        : TwoBodyTypeProcessor<TPrestepData, TAccumulatedImpulse, TConstraintFunctions, AccessNoPose, AccessNoPose, AccessNoPose, AccessNoPose>
+        where TPrestepData : unmanaged where TAccumulatedImpulse : unmanaged
+        where TConstraintFunctions : unmanaged, ITwoBodyConstraintFunctions<TPrestepData, TAccumulatedImpulse>
     {
-        public unsafe override void IncrementallyUpdateContactData(ref TypeBatch typeBatch, Bodies bodies, float dt, float inverseDt, int startBundle, int exclusiveEndBundle)
-        {
-            ref var prestepBase = ref Unsafe.AsRef<TPrestepData>(typeBatch.PrestepData.Memory);
-            ref var bodyReferencesBase = ref Unsafe.AsRef<TwoBodyReferences>(typeBatch.BodyReferences.Memory);
-            ref var bodyVelocities = ref bodies.ActiveSet.Velocities;
-            var function = default(TConstraintFunctions);
-            var dtWide = new Vector<float>(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);
-                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);
-            }
-        }
     }
 }
diff --git a/BepuPhysics/Constraints/TypeBatch.cs b/BepuPhysics/Constraints/TypeBatch.cs
index 9b1944fc1..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
 {
@@ -14,12 +10,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;
-        //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<byte> BodyReferences;
+        public Buffer<byte> PrestepData;
+        public Buffer<byte> AccumulatedImpulses;
         public Buffer<ConstraintHandle> IndexToHandle;
         public int ConstraintCount;
         public int TypeId;
@@ -35,7 +28,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 e80e20550..44e90cb28 100644
--- a/BepuPhysics/Constraints/TypeProcessor.cs
+++ b/BepuPhysics/Constraints/TypeProcessor.cs
@@ -5,6 +5,7 @@
 using System.Runtime.CompilerServices;
 using BepuUtilities.Collections;
 using BepuUtilities;
+using System.Runtime.Intrinsics;
 
 namespace BepuPhysics.Constraints
 {
@@ -47,27 +48,90 @@ public void Initialize(int typeId)
         }
 
         /// <summary>
-        /// Allocates a slot in the batch.
+        /// Allocates a slot in the batch, assuming the batch is not a fallback batch.
         /// </summary>
         /// <param name="typeBatch">Type batch to allocate in.</param>
         /// <param name="handle">Handle of the constraint to allocate. Establishes a link from the allocated constraint to its handle.</param>
-        /// <param name="bodyIndices">Pointer to a list of body indices (not handles!) with count equal to the type batch's expected number of involved bodies.</param>
+        /// <param name="encodedBodyIndices">List of body indices (not handles!) with count equal to the type batch's expected number of involved bodies.</param>
         /// <param name="pool">Allocation provider to use if the type batch has to be resized.</param>
         /// <returns>Index of the slot in the batch.</returns>
-        public unsafe abstract int Allocate(ref TypeBatch typeBatch, ConstraintHandle handle, int* bodyIndices, BufferPool pool);
-        public abstract void Remove(ref TypeBatch typeBatch, int index, ref Buffer<ConstraintLocation> handlesToConstraints);
+        public abstract int AllocateInTypeBatch(ref TypeBatch typeBatch, ConstraintHandle handle, Span<int> encodedBodyIndices, BufferPool pool);
+
+        /// <summary>
+        /// Allocates a slot in the batch, assuming the batch is a fallback batch.
+        /// </summary>
+        /// <param name="typeBatch">Type batch to allocate in.</param>
+        /// <param name="handle">Handle of the constraint to allocate. Establishes a link from the allocated constraint to its handle.</param>
+        /// <param name="encodedBodyIndices">List of body indices (not handles!) with count equal to the type batch's expected number of involved bodies.</param>
+        /// <param name="pool">Allocation provider to use if the type batch has to be resized.</param>
+        /// <returns>Index of the slot in the batch.</returns>
+        public abstract int AllocateInTypeBatchForFallback(ref TypeBatch typeBatch, ConstraintHandle handle, Span<int> encodedBodyIndices, BufferPool pool);
+        public abstract void Remove(ref TypeBatch typeBatch, int index, ref Buffer<ConstraintLocation> handlesToConstraints, bool isFallback);
+
+
+        /// <summary>
+        /// Collects body references from active constraints and converts them into properly flagged constraint kinematic body handles.
+        /// </summary>
+        unsafe struct ActiveKinematicFlaggedBodyHandleCollector : IForEach<int>
+        {
+            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;
+            }
+        }
+        /// <summary>
+        /// Moves a constraint from one ConstraintBatch's TypeBatch to another ConstraintBatch's TypeBatch of the same type.
+        /// </summary>
+        /// <param name="sourceTypeBatch">Source type batch to transfer the constraint out of.</param>
+        /// <param name="sourceBatchIndex">Index of the batch that owns the type batch that is the source of the constraint transfer.</param>
+        /// <param name="indexInTypeBatch">Index of the constraint to move in the current type batch.</param>
+        /// <param name="solver">Solver that owns the batches.</param>
+        /// <param name="bodies">Bodies set that owns all the constraint's bodies.</param>
+        /// <param name="targetBatchIndex">Index of the ConstraintBatch in the solver to copy the constraint into.</param>
+        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<BodyHandle>(dynamicBodyHandles, bodyHandleCollector.DynamicCount), new Span<int>(encodedBodyIndices, bodiesPerConstraint));
+        }
 
         /// <summary>
         /// Moves a constraint from one ConstraintBatch's TypeBatch to another ConstraintBatch's TypeBatch of the same type.
         /// </summary>
+        /// <param name="sourceTypeBatch">Source type batch to transfer the constraint out of.</param>
         /// <param name="sourceBatchIndex">Index of the batch that owns the type batch that is the source of the constraint transfer.</param>
         /// <param name="indexInTypeBatch">Index of the constraint to move in the current type batch.</param>
         /// <param name="solver">Solver that owns the batches.</param>
         /// <param name="bodies">Bodies set that owns all the constraint's bodies.</param>
         /// <param name="targetBatchIndex">Index of the ConstraintBatch in the solver to copy the constraint into.</param>
-        public unsafe abstract void TransferConstraint(ref TypeBatch typeBatch, int sourceBatchIndex, int indexInTypeBatch, Solver solver, Bodies bodies, int targetBatchIndex);
+        /// <param name="dynamicBodyHandles">Set of body handles in the constraint referring to dynamic bodies.</param>
+        /// <param name="encodedBodyIndices">Set of encoded body indices to use in the new constraint allocation.</param>
+        public abstract void TransferConstraint(ref TypeBatch sourceTypeBatch, int sourceBatchIndex, int indexInTypeBatch, Solver solver, Bodies bodies, int targetBatchIndex, Span<BodyHandle> dynamicBodyHandles, Span<int> encodedBodyIndices);
 
-        public abstract void EnumerateConnectedBodyIndices<TEnumerator>(ref TypeBatch typeBatch, int indexInTypeBatch, ref TEnumerator enumerator) where TEnumerator : IForEach<int>;
         [Conditional("DEBUG")]
         protected abstract void ValidateAccumulatedImpulsesSizeInBytes(int sizeInBytes);
         public unsafe void EnumerateAccumulatedImpulses<TEnumerator>(ref TypeBatch typeBatch, int indexInTypeBatch, ref TEnumerator enumerator) where TEnumerator : IForEach<float>
@@ -84,7 +148,16 @@ public unsafe void EnumerateAccumulatedImpulses<TEnumerator>(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);
+
+        /// <summary>
+        /// Updates a type batch's body index references for the movement of a body in memory.
+        /// </summary>
+        /// <param name="typeBatch">Type batch containing a constraint that references the body.</param>
+        /// <param name="indexInTypeBatch">Index of the constraint in the type batch.</param>
+        /// <param name="bodyIndexInConstraint">Index within the constraint of the body.</param>
+        /// <param name="newBodyLocation">New index of the body in the bodies active set.</param>
+        /// <returns>True if the body being moved was kinematic according to the constraint's reference.</returns>
+        public abstract bool UpdateForBodyMemoryMove(ref TypeBatch typeBatch, int indexInTypeBatch, int bodyIndexInConstraint, int newBodyLocation);
 
         public abstract void Scramble(ref TypeBatch typeBatch, Random random, ref Buffer<ConstraintLocation> handlesToConstraints);
 
@@ -94,28 +167,31 @@ 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<byte> bodyReferencesCache);
 
         internal abstract void CopyToCache(
             ref TypeBatch typeBatch,
             int bundleStart, int localBundleStart, int bundleCount,
             int constraintStart, int localConstraintStart, int constraintCount,
-            ref Buffer<ConstraintHandle> indexToHandleCache, ref RawBuffer prestepCache, ref RawBuffer accumulatedImpulsesCache);
+            ref Buffer<ConstraintHandle> indexToHandleCache, ref Buffer<byte> prestepCache, ref Buffer<byte> accumulatedImpulsesCache);
 
         internal abstract void Regather(
             ref TypeBatch typeBatch,
             int constraintStart, int constraintCount, ref int firstSourceIndex,
-            ref Buffer<ConstraintHandle> indexToHandleCache, ref RawBuffer bodyReferencesCache, ref RawBuffer prestepCache, ref RawBuffer accumulatedImpulsesCache,
+            ref Buffer<ConstraintHandle> indexToHandleCache, ref Buffer<byte> bodyReferencesCache, ref Buffer<byte> prestepCache, ref Buffer<byte> accumulatedImpulsesCache,
             ref Buffer<ConstraintLocation> handlesToConstraints);
 
-        internal unsafe abstract void GatherActiveConstraints(Bodies bodies, Solver solver, ref QuickList<ConstraintHandle> sourceHandles, int startIndex, int endIndex, ref TypeBatch targetTypeBatch);
+        internal abstract void GatherActiveConstraints(Bodies bodies, Solver solver, ref QuickList<ConstraintHandle> sourceHandles, int startIndex, int endIndex, ref TypeBatch targetTypeBatch);
 
-        internal unsafe abstract void CopySleepingToActive(
-            int sourceSet, int sourceBatchIndex, int sourceTypeBatchIndex, int targetBatchIndex, int targetTypeBatchIndex,
+        internal abstract void AddSleepingToActiveForFallback(
+            int sourceSet, int sourceTypeBatchIndex, int targetTypeBatchIndex, Bodies bodies, Solver solver);
+
+        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<int> sortedKeys, ref Buffer<int> sortedSourceIndices);
@@ -124,18 +200,30 @@ 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, ref Buffer<BodyVelocity> bodyVelocities, int startBundle, int exclusiveEndBundle);
-        public abstract void SolveIteration(ref TypeBatch typeBatch, ref Buffer<BodyVelocity> bodyVelocities, int startBundle, int exclusiveEndBundle);
+        public abstract void WarmStart<TIntegratorCallbacks, TBatchIntegrationMode, TAllowPoseIntegration>(ref TypeBatch typeBatch, ref Buffer<IndexSet> 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 Solve(ref TypeBatch typeBatch, Bodies bodies, float dt, float inverseDt, 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<BodyVelocity> bodyVelocities, ref FallbackTypeBatchResults jacobiResults, int startBundle, int exclusiveEndBundle);
-        public abstract void JacobiSolveIteration(ref TypeBatch typeBatch, ref Buffer<BodyVelocity> bodyVelocities, ref FallbackTypeBatchResults jacobiResults, int startBundle, int exclusiveEndBundle);
+        /// <summary>
+        /// Gets whether this type requires incremental updates for each substep in a frame beyond the first.
+        /// </summary>
+        public abstract bool RequiresIncrementalSubstepUpdates { get; }
+        public virtual void IncrementallyUpdateForSubstep(ref TypeBatch typeBatch, Bodies bodies, float dt, float inverseDt, int startBundle, int end)
+        {
+            Debug.Fail("An incremental update was scheduled for a type batch that does not have a contact data update implementation.");
+        }
 
-        public virtual void IncrementallyUpdateContactData(ref TypeBatch typeBatch, Bodies bodies, float dt, float inverseDt, int startBundle, int end)
+        [MethodImpl(MethodImplOptions.AggressiveInlining)]
+        internal static int GetCountInBundle(ref TypeBatch typeBatch, int bundleStartIndex)
         {
-            Debug.Fail("A contact data update was scheduled for a type batch that does not have a contact data update implementation.");
+            //TODO: May want to check codegen on this. Min vs explicit branch. Theoretically, it could do this branchlessly...
+            return Math.Min(Vector<float>.Count, typeBatch.ConstraintCount - (bundleStartIndex << BundleIndexing.VectorShift));
         }
+
     }
 
     /// <summary>
@@ -143,13 +231,13 @@ public virtual void IncrementallyUpdateContactData(ref TypeBatch typeBatch, Bodi
     /// </summary>
     public interface ISortKeyGenerator<TBodyReferences> where TBodyReferences : unmanaged
     {
-        int GetSortKey(int constraintIndex, ref Buffer<TBodyReferences> bodyReferences);
+        static abstract int GetSortKey(int constraintIndex, ref Buffer<TBodyReferences> 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
     //cuts down on the syntax noise a little bit. 
     //Really, you could use a bunch of composed static generic helpers.
-    public abstract class TypeProcessor<TBodyReferences, TPrestepData, TProjection, TAccumulatedImpulse> : TypeProcessor where TBodyReferences : unmanaged where TPrestepData : unmanaged where TProjection : unmanaged where TAccumulatedImpulse : unmanaged
+    public abstract class TypeProcessor<TBodyReferences, TPrestepData, TAccumulatedImpulse> : TypeProcessor where TBodyReferences : unmanaged where TPrestepData : unmanaged where TAccumulatedImpulse : unmanaged
     {
         protected override int InternalConstrainedDegreesOfFreedom
         {
@@ -171,39 +259,59 @@ public override unsafe void ScaleAccumulatedImpulses(ref TypeBatch typeBatch, fl
             var dofCount = Unsafe.SizeOf<TAccumulatedImpulse>() / Unsafe.SizeOf<Vector<float>>();
             var broadcastedScale = new Vector<float>(scale);
             ref var impulsesBase = ref Unsafe.AsRef<Vector<float>>(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;
             }
         }
 
-
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public unsafe static void AddBodyReferencesLane(ref TBodyReferences bundle, int innerIndex, int* bodyIndices)
+        public static void SetBodyReferencesLane(ref TBodyReferences bundle, int innerIndex, Span<int> bodyIndices)
         {
-            //The jit should be able to fold almost all of the size-related calculations and address fiddling.
-            ref var start = ref Unsafe.As<TBodyReferences, int>(ref bundle);
-            ref var targetLane = ref Unsafe.Add(ref start, innerIndex);
-            var stride = Vector<int>.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<TBodyReferences>() / (stride * sizeof(int));
-            targetLane = *bodyIndices;
-            for (int i = 1; i < bodyCount; ++i)
+            ref var start = ref Unsafe.As<TBodyReferences, int>(ref bundle);
+            ref var targetLane = ref Unsafe.Add(ref start, innerIndex);
+            targetLane = bodyIndices[0];
+            for (int i = 0; i < bodyIndices.Length; ++i)
             {
-                Unsafe.Add(ref targetLane, i * stride) = bodyIndices[i];
+                Unsafe.Add(ref targetLane, i * Vector<int>.Count) = bodyIndices[i];
             }
         }
 
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        protected static int GetCountInBundle(ref TypeBatch typeBatch, int bundleStartIndex)
+        public static void AddBodyReferencesLane(ref TBodyReferences bundle, int innerIndex, Span<int> bodyIndices)
         {
-            //TODO: May want to check codegen on this. Min vs explicit branch. Theoretically, it could do this branchlessly...
-            return Math.Min(Vector<float>.Count, typeBatch.ConstraintCount - (bundleStartIndex << BundleIndexing.VectorShift));
+            //The jit should be able to fold almost all of the size-related calculations and address fiddling.
+            var bodyCount = Unsafe.SizeOf<TBodyReferences>() / (Vector<int>.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'.
+                var negativeOne = new Vector<int>(-1);
+                ref var bodyReferenceBundle = ref Unsafe.As<TBodyReferences, Vector<int>>(ref bundle);
+                for (int i = 0; i < bodyCount; ++i)
+                {
+                    Unsafe.Add(ref bodyReferenceBundle, i) = negativeOne;
+                }
+            }
+            SetBodyReferencesLane(ref bundle, innerIndex, bodyIndices);
+        }
+
+        [MethodImpl(MethodImplOptions.AggressiveInlining)]
+        public static void RemoveBodyReferencesLane(ref TBodyReferences bundle, int innerIndex)
+        {
+            var bodyCount = Unsafe.SizeOf<TBodyReferences>() / (Vector<int>.Count * sizeof(int));
+            ref var start = ref Unsafe.As<TBodyReferences, int>(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<int>.Count) = -1;
+            }
         }
 
 
-        public unsafe sealed override int Allocate(ref TypeBatch typeBatch, ConstraintHandle handle, int* bodyIndices, BufferPool pool)
+        public sealed override int AllocateInTypeBatch(ref TypeBatch typeBatch, ConstraintHandle handle, Span<int> bodyIndices, BufferPool pool)
         {
             Debug.Assert(typeBatch.BodyReferences.Allocated, "Should initialize the batch before allocating anything from it.");
             if (typeBatch.ConstraintCount == typeBatch.IndexToHandle.Length)
@@ -219,23 +327,250 @@ public unsafe sealed override int Allocate(ref TypeBatch typeBatch, ConstraintHa
             GatherScatter.ClearLane<TAccumulatedImpulse, float>(ref Buffer<TAccumulatedImpulse>.Get(ref typeBatch.AccumulatedImpulses, bundleIndex), innerIndex);
             var bundleCount = typeBatch.BundleCount;
             Debug.Assert(typeBatch.PrestepData.Length >= bundleCount * Unsafe.SizeOf<TPrestepData>());
-            Debug.Assert(typeBatch.Projection.Length >= bundleCount * Unsafe.SizeOf<TProjection>());
             Debug.Assert(typeBatch.BodyReferences.Length >= bundleCount * Unsafe.SizeOf<TBodyReferences>());
             Debug.Assert(typeBatch.AccumulatedImpulses.Length >= bundleCount * Unsafe.SizeOf<TAccumulatedImpulse>());
             Debug.Assert(typeBatch.IndexToHandle.Length >= typeBatch.ConstraintCount);
             return index;
         }
 
+
         [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)
+        private unsafe static bool AllowFallbackBundleAllocation(ref TBodyReferences bundle, Vector<int>* broadcastedBodyIndices)
         {
-            //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);
+            //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<int>*)Unsafe.AsPointer(ref bundle);
+            var bodiesPerConstraint = Unsafe.SizeOf<TBodyReferences>() / Unsafe.SizeOf<Vector<int>>(); //redundant, but folds.
+            for (int broadcastedBundleBodyInConstraint = 0; broadcastedBundleBodyInConstraint < bodiesPerConstraint; ++broadcastedBundleBodyInConstraint)
+            {
+                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;
+                    }
+                }
+            }
+            //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<int>.Zero);
+        }
+
+        [MethodImpl(MethodImplOptions.AggressiveInlining)]
+        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<TBodyReferences, Vector<int>>(ref bundle);
+            //Choose the first empty slot as the allocation target. This requires picking the lowest index lane that contains -1.
+            return BundleIndexing.GetFirstSetLaneIndex(Vector.LessThan(bundleBodyIndices, Vector<int>.Zero));
+        }
+
+        [MethodImpl(MethodImplOptions.AggressiveInlining)]
+        private unsafe static bool ProbeBundleForFallback(Buffer<TBodyReferences> typeBatchBodyIndices, Vector<int>* broadcastedBodyIndices, Span<int> encodedBodyIndices, 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, encodedBodyIndices);
+                return true;
+            }
+            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<TBodyReferences, Vector<int>>(ref typeBatch.BodyReferences.As<TBodyReferences>()[bundleIndex])[innerIndex] == -1;
+                Debug.Assert((typeBatch.IndexToHandle[i].Value == -1) == laneIsEmpty);
+            }
+        }
+
+        [Conditional("DEBUG")]
+        void ValidateFallbackAccessSafety(ref TypeBatch typeBatch, int bodiesPerConstraint)
+        {
+            var bodyReferencesBundleSize = Unsafe.SizeOf<Vector<int>>() * bodiesPerConstraint;
+            for (int bundleIndex = 0; bundleIndex < typeBatch.BundleCount; ++bundleIndex)
+            {
+                ref var bodyReferenceForFirstBody = ref Unsafe.As<byte, Vector<int>>(ref typeBatch.BodyReferences[bundleIndex * bodyReferencesBundleSize]);
+                for (int sourceBodyIndexInConstraint = 0; sourceBodyIndexInConstraint < bodiesPerConstraint; ++sourceBodyIndexInConstraint)
+                {
+                    var occupiedLaneMask = Vector.GreaterThanOrEqual(bodyReferenceForFirstBody, Vector<int>.Zero);
+                    var occupiedLaneCountInBundle = 0;
+                    for (int i = 0; i < Vector<int>.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<int>.Count; ++innerIndex)
+                    {
+                        var index = bodyReferencesForSource[innerIndex];
+                        if (index >= 0)
+                        {
+                            var broadcasted = new Vector<int>(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<int>.One);
+                                matchesTotal += matchesInLane;
+                            }
+                            Debug.Assert(matchesTotal == 1, "A body reference should occur no more than once in any constraint bundle.");
+                        }
+                    }
+                }
+            }
+        }
+
+        [Conditional("DEBUG")]
+        void ValidateAccumulatedImpulses(ref TypeBatch typeBatch)
+        {
+            var dofCount = Unsafe.SizeOf<TAccumulatedImpulse>() / Unsafe.SizeOf<Vector<float>>();
+            for (int i = 0; i < typeBatch.BundleCount; ++i)
+            {
+                var impulseBundle = typeBatch.AccumulatedImpulses.As<TAccumulatedImpulse>()[i];
+                ref var impulses = ref Unsafe.As<TAccumulatedImpulse, Vector<float>>(ref impulseBundle);
+                var mask = Vector.GreaterThanOrEqual(Unsafe.As<TBodyReferences, Vector<int>>(ref typeBatch.BodyReferences.As<TBodyReferences>()[i]), Vector<int>.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, Span<int> encodedBodyIndices, BufferPool pool)
+        {
+            //This folds.
+            var bodiesPerConstraint = Unsafe.SizeOf<TBodyReferences>() / Unsafe.SizeOf<Vector<int>>();
+            //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);
+            }
+            //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 = 16;
+            //Note that this only ever executes for the active set, so body references are indices.
+            var typeBatchBodyIndices = typeBatch.BodyReferences.As<TBodyReferences>();
+            int targetBundleIndex = -1;
+            int targetInnerIndex = -1;
+            var broadcastedBodyIndices = stackalloc Vector<int>[bodiesPerConstraint];
+            for (int bodyIndexInConstraint = 0; bodyIndexInConstraint < encodedBodyIndices.Length; ++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<int>(encodedBodyIndices[bodyIndexInConstraint] & Bodies.BodyReferenceMask);
+            }
+            var bundleCount = typeBatch.BundleCount;
+            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)
+                {
+                    if (ProbeBundleForFallback(typeBatchBodyIndices, broadcastedBodyIndices, encodedBodyIndices, 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, 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;
+                    var bundleJump = bundleCount / probeLocationCount;
+                    var remainder = lastBundleIndex - bundleJump * probeLocationCount;
+                    for (int probeIndex = 0; probeIndex < probeLocationCount; ++probeIndex)
+                    {
+                        if (ProbeBundleForFallback(typeBatchBodyIndices, broadcastedBodyIndices, encodedBodyIndices, 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.
+                var oldCount = typeBatch.ConstraintCount;
+
+                var indexInTypeBatch = bundleCount * Vector<int>.Count;
+                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;
+                typeBatch.IndexToHandle[indexInTypeBatch] = handle;
+                ref var bundle = ref Buffer<TBodyReferences>.Get(ref typeBatch.BodyReferences, bundleCount);
+                AddBodyReferencesLane(ref bundle, 0, encodedBodyIndices);
+                //Clear the slot's accumulated impulse. The backing memory could be initialized to any value.
+                GatherScatter.ClearLane<TAccumulatedImpulse, float>(ref Buffer<TAccumulatedImpulse>.Get(ref typeBatch.AccumulatedImpulses, bundleCount), 0);
+                Debug.Assert(typeBatch.PrestepData.Length >= typeBatch.BundleCount * Unsafe.SizeOf<TPrestepData>());
+                Debug.Assert(typeBatch.BodyReferences.Length >= typeBatch.BundleCount * Unsafe.SizeOf<TBodyReferences>());
+                Debug.Assert(typeBatch.AccumulatedImpulses.Length >= typeBatch.BundleCount * Unsafe.SizeOf<TAccumulatedImpulse>());
+                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);
+                for (int i = oldCount; i < indexInTypeBatch; ++i)
+                {
+                    typeBatch.IndexToHandle[i].Value = -1;
+                }
+                //ValidateEmptyFallbackSlots(ref typeBatch);
+                //ValidateFallbackAccessSafety(ref typeBatch, bodiesPerConstraint);
+                //ValidateAccumulatedImpulses(ref typeBatch);
+                return indexInTypeBatch;
+            }
+            else
+            {
+                //Clear the slot's accumulated impulse. The backing memory could be initialized to any value.
+                GatherScatter.ClearLane<TAccumulatedImpulse, float>(ref Buffer<TAccumulatedImpulse>.Get(ref typeBatch.AccumulatedImpulses, targetBundleIndex), targetInnerIndex);
+                var indexInTypeBatch = targetBundleIndex * Vector<int>.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<TPrestepData>());
+                Debug.Assert(typeBatch.BodyReferences.Length >= bundleCount * Unsafe.SizeOf<TBodyReferences>());
+                Debug.Assert(typeBatch.AccumulatedImpulses.Length >= bundleCount * Unsafe.SizeOf<TAccumulatedImpulse>());
+                //ValidateEmptyFallbackSlots(ref typeBatch);
+                //ValidateFallbackAccessSafety(ref typeBatch, bodiesPerConstraint);
+                //ValidateAccumulatedImpulses(ref typeBatch);
+                return indexInTypeBatch;
+            }
+
         }
+
         /// <summary>
         /// Overwrites all the data in the target constraint slot with source data.
         /// </summary>
@@ -246,16 +581,17 @@ protected static void Move(
             ref TBodyReferences targetReferencesBundle, ref TPrestepData targetPrestepBundle, ref TAccumulatedImpulse targetAccumulatedBundle, ref ConstraintHandle targetIndexToHandle,
             int targetInner, int targetIndex, ref Buffer<ConstraintLocation> 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;
         }
 
 
 
-        public sealed override unsafe void Scramble(ref TypeBatch typeBatch, Random random, ref Buffer<ConstraintLocation> handlesToConstraints)
+        public sealed override void Scramble(ref TypeBatch typeBatch, Random random, ref Buffer<ConstraintLocation> handlesToConstraints)
         {
             //This is a pure debug function used to compare cache optimization strategies. Performance doesn't matter. 
             TPrestepData aPrestep = default;
@@ -291,83 +627,140 @@ public sealed override unsafe void Scramble(ref TypeBatch typeBatch, Random rand
         /// <summary>
         /// Removes a constraint from the batch.
         /// </summary>
+        /// <param name="typeBatch">Type batch to remove a constraint from.</param>
         /// <param name="index">Index of the constraint to remove.</param>
         /// <param name="handlesToConstraints">The handle to constraint mapping used by the solver that could be modified by a swap on removal.</param>
-        public override unsafe void Remove(ref TypeBatch typeBatch, int index, ref Buffer<ConstraintLocation> handlesToConstraints)
+        /// <param name="isFallback">True if the type batch being removed from belongs to the fallback batch, false otherwise.</param>
+        public override unsafe void Remove(ref TypeBatch typeBatch, int index, ref Buffer<ConstraintLocation> 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);
+            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.
+                //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<TBodyReferences>();
+                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<TBodyReferences, Vector<int>>(ref removedBundleSlot);
+                if (Vector.LessThanAll(firstBodyReferences, Vector<int>.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<TPrestepData>();
+                        var accumulatedImpulses = typeBatch.AccumulatedImpulses.As<TAccumulatedImpulse>();
+                        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<int>.Count;
+                        var bundleStartIndexInConstraints = lastBundleIndex * Vector<int>.Count;
+                        for (int i = 0; i < Vector<int>.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[constraintIndex].Value = -1; //Removed handles should be set to -1.
+                                typeBatch.IndexToHandle[newConstraintIndex] = handle;
+                                handlesToConstraints[handle.Value].IndexInTypeBatch = newConstraintIndex;
+                            }
+                        }
+                        //Removed the last bundle index, so drop back by one.
+                        --lastBundleIndex;
+                    }
+                    //Calculate the new constraint count by getting the highest index in the new last bundle.
+                    var innerLaneCount = BundleIndexing.GetLastSetLaneCount(Vector.GreaterThanOrEqual(Unsafe.As<TBodyReferences, Vector<int>>(ref bodyReferences[lastBundleIndex]), Vector<int>.Zero));
+                    typeBatch.ConstraintCount = lastBundleIndex * Vector<int>.Count + innerLaneCount;
 
-            ref var bodyReferences = ref Unsafe.As<byte, TBodyReferences>(ref *typeBatch.BodyReferences.Memory);
-            if (index < lastIndex)
+                    //ValidateEmptyFallbackSlots(ref typeBatch);
+                    //ValidateFallbackAccessSafety(ref typeBatch, bodiesPerConstraint);
+                    //ValidateAccumulatedImpulses(ref typeBatch);
+                }
+            }
+            else
             {
-                //Need to swap.
-                ref var prestepData = ref Unsafe.As<byte, TPrestepData>(ref *typeBatch.PrestepData.Memory);
-                ref var accumulatedImpulses = ref Unsafe.As<byte, TAccumulatedImpulse>(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);
+                var lastIndex = typeBatch.ConstraintCount - 1;
+                typeBatch.ConstraintCount = lastIndex;
+                BundleIndexing.GetBundleIndices(lastIndex, out var sourceBundleIndex, out var sourceInnerIndex);
+
+                ref var bodyReferences = ref Unsafe.As<byte, TBodyReferences>(ref *typeBatch.BodyReferences.Memory);
+                if (index < lastIndex)
+                {
+                    //Need to swap.
+                    ref var prestepData = ref Unsafe.As<byte, TPrestepData>(ref *typeBatch.PrestepData.Memory);
+                    ref var accumulatedImpulses = ref Unsafe.As<byte, TAccumulatedImpulse>(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);
             }
         }
 
+
+
         /// <summary>
         /// Moves a constraint from one ConstraintBatch's TypeBatch to another ConstraintBatch's TypeBatch of the same type.
         /// </summary>
+        /// <param name="sourceTypeBatch">Source type batch to transfer the constraint out of.</param>
         /// <param name="sourceBatchIndex">Index of the batch that owns the type batch that is the source of the constraint transfer.</param>
         /// <param name="indexInTypeBatch">Index of the constraint to move in the current type batch.</param>
         /// <param name="solver">Solver that owns the batches.</param>
         /// <param name="bodies">Bodies set that owns all the constraint's bodies.</param>
         /// <param name="targetBatchIndex">Index of the ConstraintBatch in the solver to copy the constraint into.</param>
-        public unsafe override void TransferConstraint(ref TypeBatch typeBatch, int sourceBatchIndex, int indexInTypeBatch, Solver solver, Bodies bodies, int targetBatchIndex)
+        /// <param name="dynamicBodyHandles">Set of body handles in the constraint referring to dynamic bodies.</param>
+        /// <param name="encodedBodyIndices">Set of encoded body indices to use in the new constraint allocation.</param>
+        public override sealed void TransferConstraint(ref TypeBatch sourceTypeBatch, int sourceBatchIndex, int indexInTypeBatch, Solver solver, Bodies bodies, int targetBatchIndex, Span<BodyHandle> dynamicBodyHandles, Span<int> 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 bodyHandles = stackalloc int[bodiesPerConstraint];
-            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.");
+            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.
-            var constraintHandle = typeBatch.IndexToHandle[indexInTypeBatch];
-            solver.AllocateInBatch(targetBatchIndex, constraintHandle, new Span<BodyHandle>(bodyHandles, bodiesPerConstraint), typeId, out var targetReference);
+            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);
             //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<TBodyReferences>.Get(ref typeBatch.BodyReferences, sourceBundle),
-                ref Buffer<TPrestepData>.Get(ref typeBatch.PrestepData, sourceBundle),
-                ref Buffer<TAccumulatedImpulse>.Get(ref typeBatch.AccumulatedImpulses, sourceBundle),
-                sourceInner,
-                ref Buffer<TBodyReferences>.Get(ref targetReference.TypeBatch.BodyReferences, targetBundle),
-                ref Buffer<TPrestepData>.Get(ref targetReference.TypeBatch.PrestepData, targetBundle),
-                ref Buffer<TAccumulatedImpulse>.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!
-            //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. 
-            //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);
+            //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<TPrestepData>.Get(ref sourceTypeBatch.PrestepData, sourceBundle), sourceInner,
+                ref Buffer<TPrestepData>.Get(ref targetReference.TypeBatch.PrestepData, targetBundle), targetInner);
+            GatherScatter.CopyLane(
+                ref Buffer<TAccumulatedImpulse>.Get(ref sourceTypeBatch.AccumulatedImpulses, sourceBundle), sourceInner,
+                ref Buffer<TAccumulatedImpulse>.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. 
+            //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(sourceBatchIndex, typeId, indexInTypeBatch);
         }
 
         void InternalResize(ref TypeBatch typeBatch, BufferPool pool, int constraintCapacity)
@@ -379,7 +772,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<TProjection>(), 0);
             pool.ResizeToAtLeast(ref typeBatch.BodyReferences, bundleCapacity * Unsafe.SizeOf<TBodyReferences>(), bundleCount * Unsafe.SizeOf<TBodyReferences>());
             pool.ResizeToAtLeast(ref typeBatch.PrestepData, bundleCapacity * Unsafe.SizeOf<TPrestepData>(), bundleCount * Unsafe.SizeOf<TPrestepData>());
             pool.ResizeToAtLeast(ref typeBatch.AccumulatedImpulses, bundleCapacity * Unsafe.SizeOf<TAccumulatedImpulse>(), bundleCount * Unsafe.SizeOf<TAccumulatedImpulse>());
@@ -412,12 +804,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<TBodyReferences, Vector<int>>(ref Buffer<TBodyReferences>.Get(ref typeBatch.BodyReferences, constraintBundleIndex));
-            GatherScatter.Get(ref bundle, constraintInnerIndex + bodyIndexInConstraint * Vector<int>.Count) = newBodyLocation;
+            ref var referenceLocation = ref GatherScatter.Get(ref bundle, constraintInnerIndex + bodyIndexInConstraint * Vector<int>.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.
@@ -427,15 +823,14 @@ protected void GenerateSortKeysAndCopyReferences<TSortKeyGenerator>(
            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<byte> bodyReferencesCache)
             where TSortKeyGenerator : struct, ISortKeyGenerator<TBodyReferences>
         {
-            var sortKeyGenerator = default(TSortKeyGenerator);
             var bodyReferences = typeBatch.BodyReferences.As<TBodyReferences>();
             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<TBodyReferences>();
             bodyReferences.CopyTo(bundleStart, typedBodyReferencesCache, localBundleStart, bundleCount);
@@ -445,7 +840,6 @@ protected void GenerateSortKeysAndCopyReferences<TSortKeyGenerator>(
         protected void VerifySortRegion<TSortKeyGenerator>(ref TypeBatch typeBatch, int bundleStartIndex, int constraintCount, ref Buffer<int> sortedKeys, ref Buffer<int> sortedSourceIndices)
             where TSortKeyGenerator : struct, ISortKeyGenerator<TBodyReferences>
         {
-            var sortKeyGenerator = default(TSortKeyGenerator);
             var previousKey = -1;
             var baseIndex = bundleStartIndex << BundleIndexing.VectorShift;
             var bodyReferences = typeBatch.BodyReferences.As<TBodyReferences>();
@@ -453,7 +847,7 @@ protected void VerifySortRegion<TSortKeyGenerator>(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.");
@@ -466,7 +860,7 @@ internal unsafe sealed override void CopyToCache(
             ref TypeBatch typeBatch,
             int bundleStart, int localBundleStart, int bundleCount,
             int constraintStart, int localConstraintStart, int constraintCount,
-            ref Buffer<ConstraintHandle> indexToHandleCache, ref RawBuffer prestepCache, ref RawBuffer accumulatedImpulsesCache)
+            ref Buffer<ConstraintHandle> indexToHandleCache, ref Buffer<byte> prestepCache, ref Buffer<byte> accumulatedImpulsesCache)
         {
             typeBatch.IndexToHandle.CopyTo(constraintStart, indexToHandleCache, localConstraintStart, constraintCount);
             Unsafe.CopyBlockUnaligned(
@@ -481,7 +875,7 @@ internal unsafe sealed override void CopyToCache(
         internal sealed override void Regather(
             ref TypeBatch typeBatch,
             int constraintStart, int constraintCount, ref int firstSourceIndex,
-            ref Buffer<ConstraintHandle> indexToHandleCache, ref RawBuffer bodyReferencesCache, ref RawBuffer prestepCache, ref RawBuffer accumulatedImpulsesCache,
+            ref Buffer<ConstraintHandle> indexToHandleCache, ref Buffer<byte> bodyReferencesCache, ref Buffer<byte> prestepCache, ref Buffer<byte> accumulatedImpulsesCache,
             ref Buffer<ConstraintLocation> handlesToConstraints)
         {
             var typedBodyReferencesCache = bodyReferencesCache.As<TBodyReferences>();
@@ -510,7 +904,7 @@ internal sealed override void Regather(
             }
         }
 
-        internal unsafe sealed override void GatherActiveConstraints(Bodies bodies, Solver solver, ref QuickList<ConstraintHandle> sourceHandles, int startIndex, int endIndex, ref TypeBatch targetTypeBatch)
+        internal sealed override void GatherActiveConstraints(Bodies bodies, Solver solver, ref QuickList<ConstraintHandle> sourceHandles, int startIndex, int endIndex, ref TypeBatch targetTypeBatch)
         {
             ref var activeConstraintSet = ref solver.ActiveSet;
             ref var activeBodySet = ref bodies.ActiveSet;
@@ -540,39 +934,94 @@ ref Buffer<TAccumulatedImpulse>.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;
+                    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<int>.Count;
                 }
             }
         }
 
-        [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];
+            ref var targetTypeBatch = ref solver.ActiveSet.Batches[batchIndex].TypeBatches[targetTypeBatchIndex];
+            Debug.Assert(sourceTypeBatch.TypeId == targetTypeBatch.TypeId);
+            var bodyCount = Unsafe.SizeOf<TBodyReferences>() / Unsafe.SizeOf<Vector<int>>();
+            //solver.ValidateSetOwnership(ref sourceTypeBatch, sourceSet);
+            //ValidateAccumulatedImpulses(ref targetTypeBatch);
+            //ValidateEmptyFallbackSlots(ref targetTypeBatch);
+            //ValidateFallbackAccessSafety(ref targetTypeBatch, bodyCount);
+            //solver.ValidateConstraintMaps(0, batchIndex, targetTypeBatchIndex);
+            //solver.ValidateConstraintMaps(sourceSet, batchIndex, sourceTypeBatchIndex);
+            Span<int> bodyIndices = stackalloc int[bodyCount];
+            var sourceBundleCount = sourceTypeBatch.BundleCount;
+            var sourceBodyReferences = sourceTypeBatch.BodyReferences.As<TBodyReferences>();
+            var sourcePrestepData = sourceTypeBatch.PrestepData.As<TPrestepData>();
+            var sourceAccumulatedImpulses = sourceTypeBatch.AccumulatedImpulses.As<TAccumulatedImpulse>();
+            var targetPrestepData = targetTypeBatch.PrestepData.As<TPrestepData>();
+            var targetAccumulatedImpulses = targetTypeBatch.AccumulatedImpulses.As<TAccumulatedImpulse>();
+            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<TPrestepData>.Get(ref sourceTypeBatch.PrestepData, sourceBundle), sourceInner,
-                    ref Buffer<TPrestepData>.Get(ref targetTypeBatch.PrestepData, targetBundle), targetInner);
-                GatherScatter.CopyLane(
-                    ref Buffer<TAccumulatedImpulse>.Get(ref sourceTypeBatch.AccumulatedImpulses, sourceBundle), sourceInner,
-                    ref Buffer<TAccumulatedImpulse>.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<int>.Count;
+                var countInBundle = sourceTypeBatch.ConstraintCount - bundleStartConstraintIndex;
+                if (countInBundle > Vector<int>.Count)
+                    countInBundle = Vector<int>.Count;
+                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<TBodyReferences, int>(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 < bodyIndices.Length; ++i)
+                    {
+                        //Bodies have already been moved into the active set, so we can use the mapping.
+                        var encodedBodyHandleValue = Unsafe.Add(ref bodyReferencesLane, Vector<int>.Count * i);
+                        var bodyHandleValue = encodedBodyHandleValue & Bodies.BodyReferenceMask;
+                        Debug.Assert(bodyHandleToLocation[bodyHandleValue].SetIndex == 0);
+                        //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);
+                    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];
+                    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,
+        internal sealed override void CopySleepingToActive(
+            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.");
@@ -598,7 +1047,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<TPrestepData>.Get(ref sourceTypeBatch.PrestepData, sourceBundle), sourceInner,
+                        ref Buffer<TPrestepData>.Get(ref targetTypeBatch.PrestepData, targetBundle), targetInner);
+                    GatherScatter.CopyLane(
+                        ref Buffer<TAccumulatedImpulse>.Get(ref sourceTypeBatch.AccumulatedImpulses, sourceBundle), sourceInner,
+                        ref Buffer<TAccumulatedImpulse>.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)
@@ -613,14 +1076,16 @@ internal unsafe sealed override void CopySleepingToActive(
                 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<int>.Count;
                 }
                 var constraintHandle = sourceTypeBatch.IndexToHandle[sourceIndex];
                 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.
@@ -629,7 +1094,7 @@ internal unsafe sealed override void CopySleepingToActive(
         }
 
 
-        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)
             {
@@ -638,12 +1103,15 @@ 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);
-                    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);
+                    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<int>.Count;
                 }
             }
@@ -669,13 +1137,266 @@ internal override int GetBodyReferenceCount(ref TypeBatch typeBatch, int bodyToF
                     {
                         if (Unsafe.Add(ref bodyVectorBase, innerIndex) == bodyToFind)
                             ++count;
-                        Debug.Assert(count <= 1);
                     }
                 }
             }
             return count;
         }
+
+
+        public enum BundleIntegrationMode
+        {
+            None = 0,
+            Partial = 1,
+            All = 2
+        }
+
+        [MethodImpl(MethodImplOptions.AggressiveInlining)]
+        public static BundleIntegrationMode BundleShouldIntegrate(int bundleIndex, in IndexSet integrationFlags, out Vector<int> integrationMask)
+        {
+            Debug.Assert(Vector<float>.Count <= 32, "Wait, what? The integration mask isn't big enough to handle a vector this big.");
+            var constraintStartIndex = bundleIndex * Vector<float>.Count;
+            var flagBundleIndex = constraintStartIndex >> 6;
+            var flagInnerIndex = constraintStartIndex - (flagBundleIndex << 6);
+            var flagMask = (1 << Vector<float>.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<int>(-1);
+                return BundleIntegrationMode.All;
+            }
+            else if (scalarIntegrationMask > 0)
+            {
+                if (Vector<int>.Count == 4 || Vector<int>.Count == 8)
+                {
+                    Vector<int> selectors;
+                    if (Vector<int>.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<int>(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<int>.Count <= 8, "The vector path assumes that AVX512 is not supported, so this is hitting a fallback path.");
+                    Span<int> mask = stackalloc int[Vector<int>.Count];
+                    for (int i = 0; i < Vector<int>.Count; ++i)
+                    {
+                        mask[i] = (scalarIntegrationMask & (1 << i)) > 0 ? -1 : 0;
+                    }
+                    integrationMask = new Vector<int>(mask);
+                }
+                return BundleIntegrationMode.Partial;
+            }
+            integrationMask = default;
+            return BundleIntegrationMode.None;
+        }
+        [MethodImpl(MethodImplOptions.AggressiveInlining)]
+        public static void IntegratePoseAndVelocity<TIntegratorCallbacks>(
+            ref TIntegratorCallbacks integratorCallbacks, ref Vector<int> bodyIndices, in BodyInertiaWide localInertia, float dt, in Vector<int> 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<float>(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<float>(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<float>(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<float>(0.5f), out newOrientation);
+                QuaternionWide.ConditionalSelect(integrationMask, newOrientation, orientation, out orientation);
+                PoseIntegration.RotateInverseInertia(localInertia.InverseInertiaTensor, orientation, out inertia.InverseInertiaTensor);
+            }
+            integratorCallbacks.IntegrateVelocity(bodyIndices, position, orientation, localInertia, integrationMask, workerIndex, new Vector<float>(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 void IntegrateVelocity<TIntegratorCallbacks, TBatchIntegrationMode>(
+            ref TIntegratorCallbacks integratorCallbacks, ref Vector<int> bodyIndices, in BodyInertiaWide localInertia, float dt, in Vector<int> 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<float>(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<float>(dt));
+            }
+            if (typeof(TBatchIntegrationMode) == typeof(BatchShouldConditionallyIntegrate))
+            {
+                var previousVelocity = velocity;
+                integratorCallbacks.IntegrateVelocity(bodyIndices, position, orientation, localInertia, integrationMask, workerIndex, new Vector<float>(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(bodyIndices, position, orientation, localInertia, integrationMask, workerIndex, new Vector<float>(dt), ref velocity);
+            }
+        }
+
+        /// <summary>
+        /// 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.
+        /// </summary>
+        /// <param name="encodedBodyIndices">Encoded body indices to decode.</param>
+        /// <param name="integrationMask">Mask to apply to the body indices.</param>
+        /// <returns>Body indices suitable for sending to to the IntegrateVelocity callback.</returns>
+        static Vector<int> DecodeBodyIndices(Vector<int> encodedBodyIndices, Vector<int> integrationMask)
+        {
+            return (encodedBodyIndices & new Vector<int>(Bodies.BodyReferenceMask)) | Vector.OnesComplement(integrationMask);
+        }
+
+        //[MethodImpl(MethodImplOptions.AggressiveInlining)]
+        public static void GatherAndIntegrate<TIntegratorCallbacks, TBatchIntegrationMode, TAccessFilter, TShouldIntegratePoses>(
+            Bodies bodies, ref TIntegratorCallbacks integratorCallbacks, ref Buffer<IndexSet> integrationFlags, int bodyIndexInConstraint, float dt, int workerIndex, int bundleIndex,
+            ref Vector<int> 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
+            where TShouldIntegratePoses : unmanaged, IBatchPoseIntegrationAllowed
+        {
+            //These type tests are compile time constants and will be specialized.
+            if (typeof(TShouldIntegratePoses) == typeof(AllowPoseIntegration))
+            {
+                if (typeof(TBatchIntegrationMode) == typeof(BatchShouldAlwaysIntegrate))
+                {
+                    //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(encodedBodyIndices), new Vector<uint>(Bodies.DynamicLimit)));
+                    bodies.GatherState<AccessAll>(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<TAccessFilter>(encodedBodyIndices, 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<AccessAll>(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.
+                        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
+                    {
+                        inertia = gatheredInertia;
+                    }
+                }
+            }
+            else
+            {
+                Debug.Assert(typeof(TShouldIntegratePoses) == typeof(DisallowPoseIntegration));
+                //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))
+                {
+                    //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(encodedBodyIndices), new Vector<uint>(Bodies.DynamicLimit)));
+                    bodies.GatherState<AccessAll>(encodedBodyIndices, false, out position, out orientation, out velocity, out var localInertia);
+                    var decodedBodyIndices = DecodeBodyIndices(encodedBodyIndices, integrationMask);
+                    IntegrateVelocity<TIntegratorCallbacks, TBatchIntegrationMode>(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<TAccessFilter>(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<AccessAll>(encodedBodyIndices, bundleIntegrationMode == BundleIntegrationMode.None, out position, out orientation, out velocity, out var gatheredInertia);
+                    if (bundleIntegrationMode != BundleIntegrationMode.None)
+                    {
+                        var decodedBodyIndices = DecodeBodyIndices(encodedBodyIndices, integrationMask);
+                        IntegrateVelocity<TIntegratorCallbacks, TBatchIntegrationMode>(ref integratorCallbacks, ref decodedBodyIndices, gatheredInertia, dt, integrationMask, position, orientation, ref velocity, workerIndex, out inertia);
+                        bodies.ScatterInertia(ref inertia, encodedBodyIndices, integrationMask);
+                    }
+                    else
+                    {
+                        inertia = gatheredInertia;
+                    }
+                }
+            }
+
+            //var validationMask = Vector.GreaterThanOrEqual(bodyIndices, Vector<int>.Zero);
+            //orientation.Validate(validationMask);
+            //position.Validate(validationMask);
+            //velocity.Linear.Validate(validationMask);
+            //velocity.Angular.Validate(validationMask);
+        }
+
     }
 
 
-}
+}
\ No newline at end of file
diff --git a/BepuPhysics/Constraints/VolumeConstraint.cs b/BepuPhysics/Constraints/VolumeConstraint.cs
index 576fe8381..7e843bb65 100644
--- a/BepuPhysics/Constraints/VolumeConstraint.cs
+++ b/BepuPhysics/Constraints/VolumeConstraint.cs
@@ -1,4 +1,3 @@
-using BepuPhysics.CollisionDetection;
 using BepuUtilities;
 using BepuUtilities.Memory;
 using System;
@@ -9,7 +8,7 @@
 namespace BepuPhysics.Constraints
 {
     /// <summary>
-    /// 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.
     /// </summary>
     public struct VolumeConstraint : IFourBodyConstraintDescription<VolumeConstraint>
@@ -32,13 +31,13 @@ public struct VolumeConstraint : IFourBodyConstraintDescription<VolumeConstraint
         /// <param name="d">Initial position of the fourth body.</param>
         /// <param name="springSettings">Spring settings to apply to the volume constraint.</param>
         [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;
         }
 
-        public readonly int ConstraintTypeId
+        public static int ConstraintTypeId
         {
             [MethodImpl(MethodImplOptions.AggressiveInlining)]
             get
@@ -47,7 +46,8 @@ public readonly int ConstraintTypeId
             }
         }
 
-        public readonly Type TypeProcessorType => typeof(VolumeConstraintTypeProcessor);
+        public static Type TypeProcessorType => typeof(VolumeConstraintTypeProcessor);
+        public static TypeProcessor CreateTypeProcessor() => new VolumeConstraintTypeProcessor();
 
         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
             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<VolumeConstraintPrestepData>.Get(ref batch.PrestepData, bundleIndex), innerIndex);
@@ -73,94 +73,18 @@ public struct VolumeConstraintPrestepData
         public SpringSettingsWide SpringSettings;
     }
 
-    public struct VolumeConstraintProjection
-    {
-        public Vector3Wide JacobianB;
-        public Vector3Wide JacobianC;
-        public Vector3Wide JacobianD;
-        public Vector<float> EffectiveMass;
-        public Vector<float> BiasImpulse;
-        public Vector<float> SoftnessImpulseScale;
-        public Vector<float> InverseMassA;
-        public Vector<float> InverseMassB;
-        public Vector<float> InverseMassC;
-        public Vector<float> InverseMassD;
-    }
-
-    public struct VolumeConstraintFunctions : IFourBodyConstraintFunctions<VolumeConstraintPrestepData, VolumeConstraintProjection, Vector<float>>
+    public struct VolumeConstraintFunctions : IFourBodyConstraintFunctions<VolumeConstraintPrestepData, Vector<float>>
     {
         [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)
+        private static void ApplyImpulse(
+            in Vector<float> inverseMassA, in Vector<float> inverseMassB, in Vector<float> inverseMassC, in Vector<float> inverseMassD,
+            in Vector3Wide negatedJacobianA, in Vector3Wide jacobianB, in Vector3Wide jacobianC, in Vector3Wide jacobianD, in Vector<float> impulse,
+            ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB, ref BodyVelocityWide velocityC, ref BodyVelocityWide velocityD)
         {
-            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:
-            //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<T> 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 BodyVelocities velocityA, ref BodyVelocities velocityB, ref BodyVelocities velocityC, ref BodyVelocities velocityD,
-            ref VolumeConstraintProjection projection, ref Vector3Wide negatedJacobianA, ref Vector<float> 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.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);
@@ -168,50 +92,100 @@ private static void ApplyImpulse(ref BodyVelocities velocityA, ref BodyVelocitie
         }
 
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        private static void GetNegatedJacobianA(in VolumeConstraintProjection projection, out Vector3Wide jacobianA)
+        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 Vector<float> contributionA, out Vector<float> contributionB, out Vector<float> contributionC, out Vector<float> contributionD,
+            out Vector<float> inverseJacobianLength)
         {
-            Vector3Wide.Add(projection.JacobianB, projection.JacobianC, out jacobianA);
-            Vector3Wide.Add(projection.JacobianD, jacobianA, out jacobianA);
+            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);
+            //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<float>(1e-14f), jacobianLengthSquared);
+            inverseJacobianLength = MathHelper.FastReciprocalSquareRoot(jacobianLengthSquared);
         }
 
-        [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public void WarmStart(ref BodyVelocities velocityA, ref BodyVelocities velocityB, ref BodyVelocities velocityC, ref BodyVelocities velocityD, ref VolumeConstraintProjection projection, ref Vector<float> accumulatedImpulse)
+        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<float> accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB, ref BodyVelocityWide wsvC, ref BodyVelocityWide wsvD)
         {
-            //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);
+            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);
         }
 
-        [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public void Solve(ref BodyVelocities velocityA, ref BodyVelocities velocityB, ref BodyVelocities velocityC, ref BodyVelocities velocityD, ref VolumeConstraintProjection projection, ref Vector<float> accumulatedImpulse)
+        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<float> accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB, ref BodyVelocityWide wsvC, ref BodyVelocityWide wsvD)
         {
+            //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<float>(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 volume);
+            var biasVelocity = (prestep.TargetScaledVolume - volume) * inverseJacobianLength * positionErrorToVelocity;
+
             //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);
+            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 = 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, inverseJacobianLength * csi, ref wsvA, ref wsvB, ref wsvC, ref wsvD);
         }
 
+        public static bool RequiresIncrementalSubstepUpdates => false;
+        [MethodImpl(MethodImplOptions.AggressiveInlining)]
+        public static void IncrementallyUpdateForSubstep(in Vector<float> dt, in BodyVelocityWide wsvA, in BodyVelocityWide wsvB, in BodyVelocityWide wsvC, in BodyVelocityWide wsvD, ref VolumeConstraintPrestepData prestepData) { }
     }
 
 
     /// <summary>
     /// Handles the solve iterations of a bunch of volume constraints.
     /// </summary>
-    public class VolumeConstraintTypeProcessor : FourBodyTypeProcessor<VolumeConstraintPrestepData, VolumeConstraintProjection, Vector<float>, VolumeConstraintFunctions>
+    public class VolumeConstraintTypeProcessor : FourBodyTypeProcessor<VolumeConstraintPrestepData, Vector<float>, 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 776f42bcc..e5bec5aba 100644
--- a/BepuPhysics/Constraints/Weld.cs
+++ b/BepuPhysics/Constraints/Weld.cs
@@ -1,11 +1,16 @@
-using BepuPhysics.CollisionDetection;
-using BepuUtilities;
+using BepuUtilities;
 using BepuUtilities.Memory;
 using System;
 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
 {
     /// <summary>
@@ -27,7 +32,7 @@ public struct Weld : ITwoBodyConstraintDescription<Weld>
         /// </summary>
         public SpringSettings SpringSettings;
 
-        public readonly int ConstraintTypeId
+        public static int ConstraintTypeId
         {
             [MethodImpl(MethodImplOptions.AggressiveInlining)]
             get
@@ -36,7 +41,8 @@ public readonly int ConstraintTypeId
             }
         }
 
-        public readonly Type TypeProcessorType => typeof(WeldTypeProcessor);
+        public static Type TypeProcessorType => typeof(WeldTypeProcessor);
+        public static TypeProcessor CreateTypeProcessor() => new WeldTypeProcessor();
 
         public readonly void ApplyDescription(ref TypeBatch batch, int bundleIndex, int innerIndex)
         {
@@ -51,7 +57,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<WeldPrestepData>.Get(ref batch.PrestepData, bundleIndex), innerIndex);
@@ -68,33 +74,55 @@ public struct WeldPrestepData
         public SpringSettingsWide SpringSettings;
     }
 
-    public struct WeldProjection
-    {
-        public Vector3Wide Offset;
-        public Vector3Wide OffsetBiasVelocity;
-        public Vector3Wide OrientationBiasVelocity;
-        public Symmetric6x6Wide EffectiveMass;
-        public Vector<float> SoftnessImpulseScale;
-        public BodyInertias InertiaA;
-        public BodyInertias InertiaB;
-    }
-
     public struct WeldAccumulatedImpulses
     {
         public Vector3Wide Orientation;
         public Vector3Wide Offset;
     }
 
-    public struct WeldFunctions : IConstraintFunctions<WeldPrestepData, WeldProjection, WeldAccumulatedImpulses>
+    public struct WeldFunctions : ITwoBodyConstraintFunctions<WeldPrestepData, WeldAccumulatedImpulses>
     {
-        [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public void Prestep(Bodies bodies, ref TwoBodyReferences bodyReferences, int count, float dt, float inverseDt, ref BodyInertias inertiaA, ref BodyInertias inertiaB,
-            ref WeldPrestepData prestep, out WeldProjection projection)
+        //[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)
         {
-            bodies.GatherPose(ref bodyReferences, count, out var localPositionB, out var orientationA, out var orientationB);
-            projection.InertiaA = inertiaA;
-            projection.InertiaB = inertiaB;
+            //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
+            Scale(offsetCSI, inertiaA.InverseMass, out var linearChangeA);
+            Add(velocityA.Linear, linearChangeA, out velocityA.Linear);
 
+            //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);
+            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
+
+            TransformWithoutOverlap(orientationCSI, inertiaB.InverseInertiaTensor, out var negatedAngularChangeB);
+            Subtract(velocityB.Angular, negatedAngularChangeB, out velocityB.Angular); //note subtraction; the jacobian is -I        
+        }
+
+        //[MethodImpl(MethodImplOptions.NoInlining)]
+        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 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:
             //localOrientation * orientationA = orientationB
             //positionA + localOffset * orientationA = positionB
@@ -107,106 +135,91 @@ public void Prestep(Bodies bodies, ref TwoBodyReferences bodyReferences, int cou
             //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.
+            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.
-            //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 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;
-            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(localPositionB, 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);
-        }
+            var positionError = positionB - positionA - offset;
+            var targetOrientationB = prestep.LocalOrientation * orientationA;
+            //ConcatenateWithoutOverlap(prestep.LocalOrientation, orientationA, out var targetOrientationB);
+            ConcatenateWithoutOverlap(Conjugate(targetOrientationB), orientationB, out var rotationError);
+            GetAxisAngleFromQuaternion(rotationError, out var rotationErrorAxis, out var rotationErrorLength);
 
-        [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        private static void ApplyImpulse(ref BodyVelocities velocityA, ref BodyVelocities 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 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)]
-        public void Solve(ref BodyVelocities velocityA, ref BodyVelocities velocityB, ref WeldProjection projection, ref WeldAccumulatedImpulses accumulatedImpulse)
-        {
+            SpringSettingsWide.ComputeSpringiness(prestep.SpringSettings, dt, out var positionErrorToVelocity, out var effectiveMassCFMScale, out var softnessImpulseScale);
+            var orientationBiasVelocity = rotationErrorAxis * (rotationErrorLength * positionErrorToVelocity);
+            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;
             //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);
+            //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);
+
+            //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;
+            //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;
+            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, offset, orientationCSI, offsetCSI, ref wsvA, ref wsvB);
         }
 
+
+        public static bool RequiresIncrementalSubstepUpdates => false;
+        [MethodImpl(MethodImplOptions.AggressiveInlining)]
+        public static void IncrementallyUpdateForSubstep(in Vector<float> dt, in BodyVelocityWide wsvA, in BodyVelocityWide wsvB, ref WeldPrestepData prestepData) { }
     }
 
 
     /// <summary>
     /// Handles the solve iterations of a bunch of ball socket constraints.
     /// </summary>
-    public class WeldTypeProcessor : TwoBodyTypeProcessor<WeldPrestepData, WeldProjection, WeldAccumulatedImpulses, WeldFunctions>
+    public class WeldTypeProcessor : TwoBodyTypeProcessor<WeldPrestepData, WeldAccumulatedImpulses, WeldFunctions, AccessNoPosition, AccessNoPose, AccessAll, AccessAll>
     {
         public const int BatchTypeId = 31;
     }
diff --git a/BepuPhysics/PositionLastTimestepper.cs b/BepuPhysics/DefaultTimestepper.cs
similarity index 54%
rename from BepuPhysics/PositionLastTimestepper.cs
rename to BepuPhysics/DefaultTimestepper.cs
index e58b6f5cb..456e6974c 100644
--- a/BepuPhysics/PositionLastTimestepper.cs
+++ b/BepuPhysics/DefaultTimestepper.cs
@@ -1,43 +1,36 @@
-using System;
-using System.Collections.Generic;
-using System.Diagnostics;
-using System.Text;
-using BepuUtilities;
+using BepuUtilities;
 
 namespace BepuPhysics
 {
     /// <summary>
-    /// Updates the simulation in the order of: sleeper -> integrate velocities and update body bounding boxes -> collision detection -> solver -> integrate body poses -> data structure optimization.
+    /// 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.
     /// </summary>
-    public class PositionLastTimestepper : ITimestepper
+    public class DefaultTimestepper : ITimestepper
     {
         /// <summary>
         /// Fires after the sleeper completes and before bodies are integrated.
         /// </summary>
         public event TimestepperStageHandler Slept;
         /// <summary>
-        /// Fires after bodies have had their velocities and bounding boxes updated, but before collision detection begins.
+        /// Fires after bodies have their bounding boxes updated for the frame's predicted motion and before collision detection.
         /// </summary>
         public event TimestepperStageHandler BeforeCollisionDetection;
         /// <summary>
-        /// Fires after all collisions have been identified, but before constraints are solved.
+        /// Fires after all collisions have been identified, but before the substep loop begins.
         /// </summary>
         public event TimestepperStageHandler CollisionsDetected;
         /// <summary>
-        /// Fires after the solver executes and before body poses are integrated.
+        /// Fires after the solver executes and before the final integration step.
         /// </summary>
         public event TimestepperStageHandler ConstraintsSolved;
-        /// <summary>
-        /// Fires after bodies have their poses integrated and before data structures are incrementally optimized.
-        /// </summary>
-        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);
+            simulation.PredictBoundingBoxes(dt, threadDispatcher);
             BeforeCollisionDetection?.Invoke(dt, threadDispatcher);
 
             simulation.CollisionDetection(dt, threadDispatcher);
@@ -46,9 +39,6 @@ public void Timestep(Simulation simulation, float dt, IThreadDispatcher threadDi
             simulation.Solve(dt, threadDispatcher);
             ConstraintsSolved?.Invoke(dt, threadDispatcher);
 
-            simulation.IntegratePoses(dt, threadDispatcher);
-            PosesIntegrated?.Invoke(dt, threadDispatcher);
-
             simulation.IncrementallyOptimizeDataStructures(threadDispatcher);
         }
     }
diff --git a/BepuPhysics/DefaultTypes.cs b/BepuPhysics/DefaultTypes.cs
index 6d14b9868..1a7223e04 100644
--- a/BepuPhysics/DefaultTypes.cs
+++ b/BepuPhysics/DefaultTypes.cs
@@ -46,6 +46,7 @@ public static void RegisterDefaults(Solver solver, NarrowPhase narrowPhase)
             solver.Register<BallSocketMotor>();
             solver.Register<BallSocketServo>();
             solver.Register<AngularAxisGearMotor>();
+            solver.Register<CenterDistanceLimit>();
 
             solver.Register<Contact1OneBody>();
             solver.Register<Contact2OneBody>();
@@ -63,22 +64,22 @@ public static void RegisterDefaults(Solver solver, NarrowPhase narrowPhase)
             solver.Register<Contact3Nonconvex>();
             solver.Register<Contact4Nonconvex>();
 
-            narrowPhase.RegisterContactConstraintAccessor(new NonconvexTwoBodyAccessor<Contact4Nonconvex, Contact4NonconvexPrestepData, Contact4NonconvexAccumulatedImpulses, ContactImpulses4, ConstraintCache4>());
-            narrowPhase.RegisterContactConstraintAccessor(new NonconvexTwoBodyAccessor<Contact3Nonconvex, Contact3NonconvexPrestepData, Contact3NonconvexAccumulatedImpulses, ContactImpulses3, ConstraintCache3>());
-            narrowPhase.RegisterContactConstraintAccessor(new NonconvexTwoBodyAccessor<Contact2Nonconvex, Contact2NonconvexPrestepData, Contact2NonconvexAccumulatedImpulses, ContactImpulses2, ConstraintCache2>());
-
-            narrowPhase.RegisterContactConstraintAccessor(new NonconvexOneBodyAccessor<Contact4NonconvexOneBody, Contact4NonconvexOneBodyPrestepData, Contact4NonconvexAccumulatedImpulses, ContactImpulses4, ConstraintCache4>());
-            narrowPhase.RegisterContactConstraintAccessor(new NonconvexOneBodyAccessor<Contact3NonconvexOneBody, Contact3NonconvexOneBodyPrestepData, Contact3NonconvexAccumulatedImpulses, ContactImpulses3, ConstraintCache3>());
-            narrowPhase.RegisterContactConstraintAccessor(new NonconvexOneBodyAccessor<Contact2NonconvexOneBody, Contact2NonconvexOneBodyPrestepData, Contact2NonconvexAccumulatedImpulses, ContactImpulses2, ConstraintCache2>());
-
-            narrowPhase.RegisterContactConstraintAccessor(new ConvexTwoBodyAccessor<Contact4, Contact4PrestepData, Contact4AccumulatedImpulses, ContactImpulses4, ConstraintCache4>());
-            narrowPhase.RegisterContactConstraintAccessor(new ConvexTwoBodyAccessor<Contact3, Contact3PrestepData, Contact3AccumulatedImpulses, ContactImpulses3, ConstraintCache3>());
-            narrowPhase.RegisterContactConstraintAccessor(new ConvexTwoBodyAccessor<Contact2, Contact2PrestepData, Contact2AccumulatedImpulses, ContactImpulses2, ConstraintCache2>());
-            narrowPhase.RegisterContactConstraintAccessor(new ConvexTwoBodyAccessor<Contact1, Contact1PrestepData, Contact1AccumulatedImpulses, ContactImpulses1, ConstraintCache1>());
-            narrowPhase.RegisterContactConstraintAccessor(new ConvexOneBodyAccessor<Contact4OneBody, Contact4OneBodyPrestepData, Contact4AccumulatedImpulses, ContactImpulses4, ConstraintCache4>());
-            narrowPhase.RegisterContactConstraintAccessor(new ConvexOneBodyAccessor<Contact3OneBody, Contact3OneBodyPrestepData, Contact3AccumulatedImpulses, ContactImpulses3, ConstraintCache3>());
-            narrowPhase.RegisterContactConstraintAccessor(new ConvexOneBodyAccessor<Contact2OneBody, Contact2OneBodyPrestepData, Contact2AccumulatedImpulses, ContactImpulses2, ConstraintCache2>());
-            narrowPhase.RegisterContactConstraintAccessor(new ConvexOneBodyAccessor<Contact1OneBody, Contact1OneBodyPrestepData, Contact1AccumulatedImpulses, ContactImpulses1, ConstraintCache1>());
+            narrowPhase.RegisterContactConstraintAccessor(new NonconvexTwoBodyAccessor<Contact4Nonconvex, Contact4NonconvexPrestepData, Contact4NonconvexAccumulatedImpulses, ContactImpulses4>());
+            narrowPhase.RegisterContactConstraintAccessor(new NonconvexTwoBodyAccessor<Contact3Nonconvex, Contact3NonconvexPrestepData, Contact3NonconvexAccumulatedImpulses, ContactImpulses3>());
+            narrowPhase.RegisterContactConstraintAccessor(new NonconvexTwoBodyAccessor<Contact2Nonconvex, Contact2NonconvexPrestepData, Contact2NonconvexAccumulatedImpulses, ContactImpulses2>());
+
+            narrowPhase.RegisterContactConstraintAccessor(new NonconvexOneBodyAccessor<Contact4NonconvexOneBody, Contact4NonconvexOneBodyPrestepData, Contact4NonconvexAccumulatedImpulses, ContactImpulses4>());
+            narrowPhase.RegisterContactConstraintAccessor(new NonconvexOneBodyAccessor<Contact3NonconvexOneBody, Contact3NonconvexOneBodyPrestepData, Contact3NonconvexAccumulatedImpulses, ContactImpulses3>());
+            narrowPhase.RegisterContactConstraintAccessor(new NonconvexOneBodyAccessor<Contact2NonconvexOneBody, Contact2NonconvexOneBodyPrestepData, Contact2NonconvexAccumulatedImpulses, ContactImpulses2>());
+
+            narrowPhase.RegisterContactConstraintAccessor(new ConvexTwoBodyAccessor<Contact4, Contact4PrestepData, Contact4AccumulatedImpulses, ContactImpulses4>());
+            narrowPhase.RegisterContactConstraintAccessor(new ConvexTwoBodyAccessor<Contact3, Contact3PrestepData, Contact3AccumulatedImpulses, ContactImpulses3>());
+            narrowPhase.RegisterContactConstraintAccessor(new ConvexTwoBodyAccessor<Contact2, Contact2PrestepData, Contact2AccumulatedImpulses, ContactImpulses2>());
+            narrowPhase.RegisterContactConstraintAccessor(new ConvexTwoBodyAccessor<Contact1, Contact1PrestepData, Contact1AccumulatedImpulses, ContactImpulses1>());
+            narrowPhase.RegisterContactConstraintAccessor(new ConvexOneBodyAccessor<Contact4OneBody, Contact4OneBodyPrestepData, Contact4AccumulatedImpulses, ContactImpulses4>());
+            narrowPhase.RegisterContactConstraintAccessor(new ConvexOneBodyAccessor<Contact3OneBody, Contact3OneBodyPrestepData, Contact3AccumulatedImpulses, ContactImpulses3>());
+            narrowPhase.RegisterContactConstraintAccessor(new ConvexOneBodyAccessor<Contact2OneBody, Contact2OneBodyPrestepData, Contact2AccumulatedImpulses, ContactImpulses2>());
+            narrowPhase.RegisterContactConstraintAccessor(new ConvexOneBodyAccessor<Contact1OneBody, Contact1OneBodyPrestepData, Contact1AccumulatedImpulses, ContactImpulses1>());
 
         }
 
diff --git a/BepuPhysics/FallbackBatch.cs b/BepuPhysics/FallbackBatch.cs
deleted file mode 100644
index 0b88cefb0..000000000
--- a/BepuPhysics/FallbackBatch.cs
+++ /dev/null
@@ -1,530 +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 FallbackTypeBatchResults
-    {
-        public Buffer<Buffer<BodyVelocities>> BodyVelocities;
-
-        [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public ref Buffer<BodyVelocities> GetVelocitiesForBody(int slotIndex)
-        {
-            return ref BodyVelocities[slotIndex];
-        }
-    }
-
-    /// <summary>
-    /// 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.
-    /// </summary>
-    public struct FallbackBatch
-    {
-        public struct FallbackReference
-        {
-            public ConstraintHandle ConstraintHandle;
-            public int IndexInConstraint;
-            public override string ToString()
-            {
-                return $"{ConstraintHandle}, {IndexInConstraint}";
-            }
-        }
-        /// <summary>
-        /// Gets the number of bodies in the fallback batch.
-        /// </summary>
-        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<int, QuickSet<FallbackReference, FallbackReferenceComparer>, PrimitiveComparer<int>> bodyConstraintReferences;
-
-        internal struct FallbackReferenceComparer : IEqualityComparerRef<FallbackReference>
-        {
-            [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<TBodyReferenceGetter>(ConstraintHandle constraintHandle, Span<BodyHandle> 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<FallbackReference, FallbackReferenceComparer>(minimumReferenceCapacity, pool);
-                    bodyConstraintReferences.Keys[elementIndex] = bodyReference;
-                    bodyConstraintReferences.Table[tableIndex] = elementIndex + 1;
-                    ++bodyConstraintReferences.Count;
-                }
-                var fallbackReference = new FallbackReference { ConstraintHandle = constraintHandle, IndexInConstraint = i };
-                constraintReferences.AddRef(ref fallbackReference, pool);
-            }
-        }
-
-        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<BodyHandle> 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<BodyHandle> constraintBodyHandles, Bodies bodies,
-          int typeId, BufferPool pool, int minimumBodyCapacity = 8, int minimumReferenceCapacity = 8)
-        {
-            Allocate(handle, constraintBodyHandles, bodies, typeId, pool, new InactiveSetGetter(), minimumBodyCapacity, minimumReferenceCapacity);
-        }
-
-
-        internal unsafe void Remove(int bodyReference, ConstraintHandle constraintHandle, ref QuickList<int> 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.FastRemoveRef(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)
-            {
-                //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;
-                }
-            }
-        }
-
-
-        internal unsafe void Remove(Solver solver, BufferPool bufferPool, ref ConstraintBatch batch, ConstraintHandle constraintHandle, 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<int>(allocationIdsToRemoveMemory, maximumAllocationIdsToFree);
-            var allocationIdsToFree = new QuickList<int>(initialSpan);
-            typeProcessor.EnumerateConnectedBodyIndices(ref batch.TypeBatches[batch.TypeIndexToTypeBatchIndex[typeId]], indexInTypeBatch, ref enumerator);
-            for (int i = 0; i < bodyCount; ++i)
-            {
-                Remove(bodyIndices[i], constraintHandle, ref allocationIdsToFree);
-            }
-            for (int i = 0; i < allocationIdsToFree.Count; ++i)
-            {
-                bufferPool.ReturnUnsafely(allocationIdsToFree[i]);
-            }
-        }
-
-        internal unsafe void TryRemove(int bodyReference, ref QuickList<int> 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<FallbackTypeBatchResults> 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<FallbackTypeBatchResults> 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<int> references, int count, out Vector<float> jacobiScale)
-        {
-            ref var start = ref Unsafe.As<Vector<int>, int>(ref references);
-            Unsafe.SkipInit(out Vector<int> counts);
-            ref var countsStart = ref Unsafe.As<Vector<int>, int>(ref counts);
-            for (int i = 0; i < count; ++i)
-            {
-                var index = bodyConstraintReferences.IndexOfRef(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<float> jacobiScaleA, out Vector<float> jacobiScaleB)
-        {
-            ref var startA = ref Unsafe.As<Vector<int>, int>(ref references.IndexA);
-            ref var startB = ref Unsafe.As<Vector<int>, int>(ref references.IndexB);
-            Unsafe.SkipInit(out Vector<int> countsA);
-            Unsafe.SkipInit(out Vector<int> countsB);
-            ref var countsAStart = ref Unsafe.As<Vector<int>, int>(ref countsA);
-            ref var countsBStart = ref Unsafe.As<Vector<int>, 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));
-                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<float> jacobiScaleA, out Vector<float> jacobiScaleB, out Vector<float> jacobiScaleC)
-        {
-            ref var startA = ref Unsafe.As<Vector<int>, int>(ref references.IndexA);
-            ref var startB = ref Unsafe.As<Vector<int>, int>(ref references.IndexB);
-            ref var startC = ref Unsafe.As<Vector<int>, int>(ref references.IndexC);
-            Unsafe.SkipInit(out Vector<int> countsA);
-            Unsafe.SkipInit(out Vector<int> countsB);
-            Unsafe.SkipInit(out Vector<int> countsC);
-            ref var countsAStart = ref Unsafe.As<Vector<int>, int>(ref countsA);
-            ref var countsBStart = ref Unsafe.As<Vector<int>, int>(ref countsB);
-            ref var countsCStart = ref Unsafe.As<Vector<int>, 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));
-                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<float> jacobiScaleA, out Vector<float> jacobiScaleB, out Vector<float> jacobiScaleC, out Vector<float> jacobiScaleD)
-        {
-            ref var startA = ref Unsafe.As<Vector<int>, int>(ref references.IndexA);
-            ref var startB = ref Unsafe.As<Vector<int>, int>(ref references.IndexB);
-            ref var startC = ref Unsafe.As<Vector<int>, int>(ref references.IndexC);
-            ref var startD = ref Unsafe.As<Vector<int>, int>(ref references.IndexD);
-            Unsafe.SkipInit(out Vector<int> countsA);
-            Unsafe.SkipInit(out Vector<int> countsB);
-            Unsafe.SkipInit(out Vector<int> countsC);
-            Unsafe.SkipInit(out Vector<int> countsD);
-            ref var countsAStart = ref Unsafe.As<Vector<int>, int>(ref countsA);
-            ref var countsBStart = ref Unsafe.As<Vector<int>, int>(ref countsB);
-            ref var countsCStart = ref Unsafe.As<Vector<int>, int>(ref countsC);
-            ref var countsDStart = ref Unsafe.As<Vector<int>, 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));
-                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.Fallback.bodyConstraintReferences.Keys.Allocated);
-                ref var bodyConstraintReferences = ref set.Fallback.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<FallbackTypeBatchResults> 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 *= inverseCount;
-                bodyVelocity.Angular *= inverseCount;
-                bodies.ActiveSet.Velocities[bodyIndex] = 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.AddUnsafelyRef(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 FallbackBatch sourceBatch, BufferPool pool, out FallbackBatch 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<int, QuickSet<FallbackReference, FallbackReferenceComparer>, PrimitiveComparer<int>>(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/HandyEnumerators.cs b/BepuPhysics/HandyEnumerators.cs
index 6b05aa334..ee4468469 100644
--- a/BepuPhysics/HandyEnumerators.cs
+++ b/BepuPhysics/HandyEnumerators.cs
@@ -1,41 +1,87 @@
-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
 {
     /// <summary>
     /// Collects body handles associated with an active constraint as integers.
     /// </summary>
-    public unsafe struct ActiveConstraintBodyHandleCollector : IForEach<int> 
+    public unsafe struct ActiveConstraintBodyHandleCollector : IForEach<int>
     {
         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.BodyReferenceMask].Value;
         }
     }
 
-    public unsafe struct ReferenceCollector : IForEach<int>
+    /// <summary>
+    /// Collects body handles associated with an active constraint as integers.
+    /// </summary>
+    public unsafe struct ActiveConstraintDynamicBodyHandleCollector : IForEach<int>
+    {
+        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 (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;
+            }
+        }
+    }
+
+    /// <summary>
+    /// Collects body indices associated with an active constraint. Encoded metadata is stripped.
+    /// </summary>
+    public unsafe struct ActiveConstraintBodyIndexCollector : IForEach<int>
+    {
+        public int* Indices;
+        public int Count;
+
+        public ActiveConstraintBodyIndexCollector(int* indices)
+        {
+            Indices = indices;
+            Count = 0;
+        }
+
+        public void LoopBody(int encodedBodyIndex)
+        {
+            Indices[Count++] = encodedBodyIndex & Bodies.BodyReferenceMask;
+        }
+    }
+
+    /// <summary>
+    /// 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.
+    /// </summary>
+    public unsafe struct PassthroughReferenceCollector : IForEach<int>
     {
         public int* References;
         public int Index;
 
-        public ReferenceCollector(int* references)
+        public PassthroughReferenceCollector(int* references)
         {
             References = references;
             Index = 0;
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/ITimestepper.cs b/BepuPhysics/ITimestepper.cs
index 68bfe375b..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
 {
@@ -12,14 +9,6 @@ namespace BepuPhysics
     /// <param name="threadDispatcher">Thread dispatcher used for this timestep.</param>
     public delegate void TimestepperStageHandler(float dt, IThreadDispatcher threadDispatcher);
 
-    /// <summary>
-    /// Delegate used by ITimesteppers for stage callbacks within substepping loops.
-    /// </summary>
-    /// <param name="substepIndex">Index of the substep executing this stage.</param>
-    /// <param name="dt">Time step duration.</param>
-    /// <param name="threadDispatcher">Thread dispatcher used for this timestep.</param>
-    public delegate void TimestepperSubstepStageHandler(int substepIndex, float dt, IThreadDispatcher threadDispatcher);
-
     /// <summary>
     /// Defines a type capable of updating the simulation state for a given elapsed time.
     /// </summary>
@@ -38,6 +27,7 @@ public interface ITimestepper
         /// <summary>
         /// Performs one timestep of the given length.
         /// </summary>
+        /// <param name="simulation">Simulation to be stepped forward in time.</param>
         /// <param name="dt">Duration of the time step.</param>
         /// <param name="threadDispatcher">Thread dispatcher to use for execution, if any.</param>
         void Timestep(Simulation simulation, float dt, IThreadDispatcher threadDispatcher = null);
diff --git a/BepuPhysics/InvasiveHashDiagnostics.cs b/BepuPhysics/InvasiveHashDiagnostics.cs
new file mode 100644
index 000000000..2fe7961c9
--- /dev/null
+++ b/BepuPhysics/InvasiveHashDiagnostics.cs
@@ -0,0 +1,162 @@
+using BepuUtilities.Collections;
+using System;
+using System.Runtime.CompilerServices;
+
+namespace BepuPhysics
+{
+    /// <summary>
+    /// Hardcoded hash types used by invasive hash diagnostics.
+    /// </summary>
+    public enum HashDiagnosticType
+    {
+        AwakeBodyStates0,
+        AwakeBodyStates1,
+        AwakeBodyStates2,
+        AwakeBodyStates3,
+        AwakeBodyStates4,
+        AwakeBodyStates5,
+        AwakeBodyStates6,
+        AwakeBodyStates7,
+        AwakeBodyStates8,
+        AwakeBodyStates9,
+        AwakeBodyStates10,
+        AwakeBodyCollidableStates0,
+        AwakeBodyCollidableStates1,
+        AwakeBodyCollidableStates2,
+        AwakeBodyCollidableStates3,
+        AwakeBodyCollidableStates4,
+        AwakeBodyCollidableStates5,
+        AwakeBodyCollidableStates6,
+        AwakeBodyCollidableStates7,
+        AwakeBodyCollidableStates8,
+        AwakeBodyCollidableStates9,
+        AwakeBodyCollidableStates10,
+        AddSleepingToActiveForFallback,
+        SolverBodyReferenceBeforeCollisionDetection,
+        SolverBodyReferenceBeforePreflush,
+        SolverBodyReferenceAfterPreflushPhase1,
+        SolverBodyReferenceAfterPreflushPhase2,
+        SolverBodyReferenceAfterPreflushPhase3,
+        SolverBodyReferenceAfterPreflush,
+        SolverBodyReferenceBeforeSolver,
+        SolverBodyReferenceAfterSolver,
+        SolverBodyReferenceAtEnd,
+        DeterministicConstraintAdd,
+        AddToSimulationSpeculative,
+        AddToSimulationSpeculativeFallbackSolverReferences,
+        EnqueueStaleRemoval,
+        RemoveConstraintsFromFallbackBatchReferencedHandles,
+        RemoveConstraintsFromBatchReferencedHandles,
+        RemoveConstraintsFromBodyLists,
+        RemoveConstraintsFromTypeBatch,
+        ReturnConstraintHandles,
+        PreflushJobs,
+        AllocateInTypeBatchForFallback,
+        AllocateInTypeBatchForFallbackProbes,
+        AllocateInBatch,
+        TypeProcessorRemove
+    }
+    /// <summary>
+    /// Helper diagnostics class for monitoring internal state determinism across runs.
+    /// Typically used by inserting tests into engine internals.
+    /// </summary>
+    public class InvasiveHashDiagnostics
+    {
+        /// <summary>
+        /// This is meant as an internal diagnostic utility, so hardcoding some things is totally fine.
+        /// </summary>
+        const int HashTypeCount = 46;
+        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}");
+            bool anyFailed = false;
+            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]}.");
+                        anyFailed = true;
+                    }
+                }
+            }
+            if (anyFailed)
+            {
+                Console.WriteLine("Press enter to continue.");
+                Console.ReadLine();
+            }
+            ++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<T>(ref int hash, T value) where T : unmanaged
+        {
+            var intCount = Unsafe.SizeOf<T>() / 4;
+            ref var intBase = ref Unsafe.As<T, int>(ref value);
+            for (int i = 0; i < intCount; ++i)
+            {
+                ContributeToHash(ref hash, Unsafe.Add(ref intBase, i));
+            }
+            ref var byteBase = ref Unsafe.As<int, byte>(ref Unsafe.Add(ref intBase, intCount));
+            var byteRemainder = Unsafe.SizeOf<T>() - 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<T>(HashDiagnosticType hashType, T value) where T : unmanaged
+        {
+            ContributeToHash(ref GetHashForType(hashType), value);
+        }
+    }
+}
diff --git a/BepuPhysics/IslandAwakener.cs b/BepuPhysics/IslandAwakener.cs
index df1e04cc7..29b84e4f2 100644
--- a/BepuPhysics/IslandAwakener.cs
+++ b/BepuPhysics/IslandAwakener.cs
@@ -3,10 +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.Text;
+using System.Runtime.InteropServices;
 using System.Threading;
 
 namespace BepuPhysics
@@ -14,7 +14,7 @@ namespace BepuPhysics
     /// <summary>
     /// Provides functionality for efficiently waking up sleeping bodies.
     /// </summary>
-    public class IslandAwakener
+    public unsafe class IslandAwakener
     {
         Solver solver;
         Statics statics;
@@ -99,7 +99,7 @@ public void AwakenSets(ref QuickList<int> setIndices, IThreadDispatcher threadDi
             {
                 this.jobIndex = -1;
                 this.jobCount = phaseOneJobCount;
-                threadDispatcher.DispatchWorkers(phaseOneWorkerDelegate);
+                threadDispatcher.DispatchWorkers(phaseOneWorkerDelegate, phaseOneJobCount);
             }
             else
             {
@@ -113,7 +113,7 @@ public void AwakenSets(ref QuickList<int> setIndices, IThreadDispatcher threadDi
             {
                 this.jobIndex = -1;
                 this.jobCount = phaseTwoJobCount;
-                threadDispatcher.DispatchWorkers(phaseTwoWorkerDelegate);
+                threadDispatcher.DispatchWorkers(phaseTwoWorkerDelegate, phaseTwoJobCount);
             }
             else
             {
@@ -178,10 +178,10 @@ enum PhaseTwoJobType
         {
             BroadPhase,
             CopyConstraintRegion,
+            AddFallbackTypeBatchConstraints
         }
-        struct PhaseTwoJob
+        struct CopyConstraintRegionJob
         {
-            public PhaseTwoJobType Type;
             public int SourceStart;
             public int TargetStart;
             public int Count;
@@ -192,11 +192,34 @@ struct PhaseTwoJob
             public int TargetTypeBatch;
         }
 
+        struct FallbackAddSource
+        {
+            public int SourceSet;
+            public int SourceTypeBatchIndex;
+        }
+        struct AddFallbackTypeBatchConstraintsJob
+        {
+            public Buffer<FallbackAddSource> 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<int> uniqueSetIndices;
         QuickList<PhaseOneJob> phaseOneJobs;
         QuickList<PhaseTwoJob> phaseTwoJobs;
-        internal unsafe void ExecutePhaseOneJob(int index)
+        internal void ExecutePhaseOneJob(int index)
         {
             ref var job = ref phaseOneJobs[index];
             switch (job.Type)
@@ -223,7 +246,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)
                         {
@@ -246,21 +268,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]].Fallback;
-                            ref var target = ref solver.ActiveSet.Fallback;
-                            if (source.bodyConstraintReferences.Count > 0)
+                            ref var source = ref solver.Sets[uniqueSetIndices[i]].SequentialFallback;
+                            ref var target = ref solver.ActiveSet.SequentialFallback;
+                            if (source.dynamicBodyConstraintCounts.Count > 0)
                             {
-                                for (int j = 0; j < source.bodyConstraintReferences.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.bodyConstraintReferences.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.bodyConstraintReferences.AddUnsafelyRef(ref bodyLocation.Index, source.bodyConstraintReferences.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.");
                                 }
-                                //We've reused the lists. Set the count to zero so they don't get disposed later.
-                                source.bodyConstraintReferences.Count = 0;
                             }
                         }
                     }
@@ -273,25 +293,19 @@ 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 poses 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.) 
+                        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 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 sourcePose = ref sourceSet.Poses[sourceIndex];
-                            ref var targetPose = ref targetSet.Poses[targetIndex];
-                            targetPose = sourcePose;
-                            targetLocalInertia = sourceLocalInertia;
-                            PoseIntegration.RotateInverseInertia(sourceLocalInertia.InverseInertiaTensor, sourcePose.Orientation, out targetWorldInertia.InverseInertiaTensor);
-                            targetWorldInertia.InverseMass = sourceLocalInertia.InverseMass;
+                            var sourceBodyIndex = i + job.SourceStart;
+                            if (Bodies.IsKinematicUnsafeGCHole(ref sourceSet.DynamicsState[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.Velocities.CopyTo(job.SourceStart, targetSet.Velocities, job.TargetStart, job.Count);
                         sourceSet.Activity.CopyTo(job.SourceStart, targetSet.Activity, job.TargetStart, job.Count);
                         if (resetActivityStates)
                         {
@@ -309,10 +323,10 @@ internal unsafe void ExecutePhaseOneJob(int index)
         }
 
 
-        internal unsafe void ExecutePhaseTwoJob(int index)
+        internal 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:
                     {
@@ -339,13 +353,13 @@ 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))
                                     {
                                         if (movedLeaf.Mobility == Collidables.CollidableMobility.Static)
                                         {
-                                            statics.Collidables[statics.HandleToIndex[movedLeaf.StaticHandle.Value]].BroadPhaseIndex = staticBroadPhaseIndexToRemove;
+                                            statics.GetDirectReference(movedLeaf.StaticHandle).BroadPhaseIndex = staticBroadPhaseIndexToRemove;
                                         }
                                         else
                                         {
@@ -368,12 +382,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 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);
+                        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.Batch, job.TargetTypeBatch,
+                            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:
+                    {
+                        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;
             }
@@ -391,6 +416,13 @@ internal void AccumulateUniqueIndices(ref QuickList<int> candidateSetIndices, re
                     uniqueSetIndices.AllocateUnsafely() = candidateSetIndex;
                 }
             }
+            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<int>();
+                QuickSort.Sort(ref uniqueSetIndices[0], 0, uniqueSetIndices.Count - 1, ref comparer);
+            }
         }
         [Conditional("DEBUG")]
         void ValidateSleepingSetIndex(int setIndex)
@@ -414,41 +446,9 @@ void ValidateUniqueSets(ref QuickList<int> setIndices)
 
         }
 
-        //This is getting into the realm of Fizzbuzz Enterprise. 
-        interface ITypeCount
+        struct TypeAllocationSizes
         {
-            void Add<T>(T other) where T : ITypeCount;
-        }
-        struct ConstraintCount : ITypeCount
-        {
-            public int Count;
-
-            [MethodImpl(MethodImplOptions.AggressiveInlining)]
-            public void Add<T>(T other) where T : ITypeCount
-            {
-                Debug.Assert(typeof(T) == typeof(ConstraintCount));
-                Count += Unsafe.As<T, ConstraintCount>(ref other).Count;
-            }
-        }
-        struct PairCacheCount : ITypeCount
-        {
-            public int ElementSizeInBytes;
-            public int ByteCount;
-
-            [MethodImpl(MethodImplOptions.AggressiveInlining)]
-            public void Add<T>(T other) where T : ITypeCount
-            {
-                Debug.Assert(typeof(T) == typeof(PairCacheCount));
-                ref var pairCacheOther = ref Unsafe.As<T, PairCacheCount>(ref other);
-                Debug.Assert(ElementSizeInBytes == 0 || ElementSizeInBytes == pairCacheOther.ElementSizeInBytes);
-                ElementSizeInBytes = pairCacheOther.ElementSizeInBytes;
-                ByteCount += pairCacheOther.ByteCount;
-            }
-        }
-
-        struct TypeAllocationSizes<T> where T : unmanaged, ITypeCount
-        {
-            public Buffer<T> TypeCounts;
+            public Buffer<int> TypeCounts;
             public int HighestOccupiedTypeIndex;
             public TypeAllocationSizes(BufferPool pool, int maximumTypeCount)
             {
@@ -457,9 +457,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;
             }
@@ -470,7 +470,7 @@ public void Dispose(BufferPool pool)
         }
 
 
-        unsafe internal (int phaseOneJobCount, int phaseTwoJobCount) PrepareJobs(ref QuickList<int> setIndices, bool resetActivityStates, int threadCount)
+        internal (int phaseOneJobCount, int phaseTwoJobCount) PrepareJobs(ref QuickList<int> setIndices, bool resetActivityStates, int threadCount)
         {
             if (setIndices.Count == 0)
                 return (0, 0);
@@ -497,7 +497,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.SequentialFallback.BodyCount;
                 for (int batchIndex = 0; batchIndex < constraintSet.Batches.Count; ++batchIndex)
                 {
                     ref var batch = ref constraintSet.Batches[batchIndex];
@@ -512,25 +512,12 @@ unsafe internal (int phaseOneJobCount, int phaseTwoJobCount) PrepareJobs(ref Qui
             }
             //We accumulated indices above; add one to get the capacity requirement.
             ++highestRequiredTypeCapacity;
-            pool.Take<TypeAllocationSizes<ConstraintCount>>(highestNewBatchCount, out var constraintCountPerTypePerBatch);
+            pool.Take<TypeAllocationSizes>(highestNewBatchCount, out var constraintCountPerTypePerBatch);
             for (int batchIndex = 0; batchIndex < highestNewBatchCount; ++batchIndex)
             {
-                constraintCountPerTypePerBatch[batchIndex] = new TypeAllocationSizes<ConstraintCount>(pool, highestRequiredTypeCapacity);
+                constraintCountPerTypePerBatch[batchIndex] = new TypeAllocationSizes(pool, highestRequiredTypeCapacity);
             }
-            var narrowPhaseConstraintCaches = new TypeAllocationSizes<PairCacheCount>(pool, PairCache.CollisionConstraintTypeCount);
-            var narrowPhaseCollisionCaches = new TypeAllocationSizes<PairCacheCount>(pool, PairCache.CollisionTypeCount);
 
-            void AccumulatePairCacheTypeCounts(ref Buffer<SleepingCache> sourceTypeCaches, ref TypeAllocationSizes<PairCacheCount> 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)
             {
@@ -543,49 +530,46 @@ void AccumulatePairCacheTypeCounts(ref Buffer<SleepingCache> 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.
             //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,
             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);
+                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)
             {
                 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;
+                    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.
+                    if (batchIndex == solver.FallbackBatchThreshold)
+                        countForType *= Vector<float>.Count;
                     if (countForType > 0)
                     {
                         var typeProcessor = solver.TypeProcessors[typeId];
@@ -600,44 +584,27 @@ void AccumulatePairCacheTypeCounts(ref Buffer<SleepingCache> sourceTypeCaches, r
                 constraintCountPerType.Dispose(pool);
             }
             pool.Return(ref constraintCountPerTypePerBatch);
-            //and narrow phase pair caches.
-            ref var targetPairCache = ref pairCache.GetCacheForAwakening();
-            void EnsurePairCacheTypeCapacities(ref TypeAllocationSizes<PairCacheCount> cacheSizes, ref Buffer<UntypedList> 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<PhaseOneJob>(Math.Max(32, highestNewBatchCount + 1), pool);
-            phaseTwoJobs = new QuickList<PhaseTwoJob>(32, pool);
+            phaseOneJobs = new QuickList<PhaseOneJob>(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 };
             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 };
             }
+            phaseTwoJobs = new QuickList<PhaseTwoJob>(32, pool);
             phaseTwoJobs.AllocateUnsafely() = new PhaseTwoJob { Type = PhaseTwoJobType.BroadPhase };
 
             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<int, QuickList<FallbackAddSource>, PrimitiveComparer<int>> targetFallbackTypeBatchesToSources = highestNewBatchCount > solver.FallbackBatchThreshold ? targetFallbackTypeBatchesToSources = new(8, pool) : default;
             for (int i = 0; i < uniqueSetIndices.Count; ++i)
             {
                 var sourceSetIndex = uniqueSetIndices[i];
@@ -678,7 +645,10 @@ void EnsurePairCacheTypeCapacities(ref TypeAllocationSizes<PairCacheCount> cache
                 {
                     const int constraintJobSize = 32;
                     ref var sourceSet = ref solver.Sets[sourceSetIndex];
-                    for (int batchIndex = 0; batchIndex < sourceSet.Batches.Count; ++batchIndex)
+                    var fallbackIndex = solver.FallbackBatchThreshold;
+
+                    int synchronizedBatchCountInSource = sourceSet.Batches.Count > solver.FallbackBatchThreshold ? solver.FallbackBatchThreshold : sourceSet.Batches.Count;
+                    for (int batchIndex = 0; batchIndex < synchronizedBatchCountInSource; ++batchIndex)
                     {
                         ref var sourceBatch = ref sourceSet.Batches[batchIndex];
                         ref var targetBatch = ref activeSolverSet.Batches[batchIndex];
@@ -692,35 +662,81 @@ void EnsurePairCacheTypeCapacities(ref TypeAllocationSizes<PairCacheCount> cache
                             var baseConstraintsPerJob = sourceTypeBatch.ConstraintCount / jobCount;
                             var remainder = sourceTypeBatch.ConstraintCount - baseConstraintsPerJob * jobCount;
                             phaseTwoJobs.EnsureCapacity(phaseTwoJobs.Count + jobCount, pool);
-
                             var previousSourceEnd = 0;
                             for (int jobIndex = 0; jobIndex < jobCount; ++jobIndex)
                             {
                                 ref var job = ref phaseTwoJobs.AllocateUnsafely();
                                 job.Type = PhaseTwoJobType.CopyConstraintRegion;
-                                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;
-                                targetTypeBatch.ConstraintCount += job.Count;
+                                job.CopyConstraintRegion = new CopyConstraintRegionJob
+                                {
+                                    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<int>*)(targetTypeBatch.BodyReferences.Memory + (targetTypeBatch.BundleCount - 1) * vectorCount * Unsafe.SizeOf<Vector<int>>());
+                                    var negativeOne = new Vector<int>(-1);
+                                    for (int vectorIndex = 0; vectorIndex < vectorCount; ++vectorIndex)
+                                    {
+                                        bundleStart[vectorIndex] = negativeOne;
+                                    }
+                                }
                             }
                             Debug.Assert(previousSourceEnd == sourceTypeBatch.ConstraintCount);
                             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<FallbackAddSource>(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);
         }
 
 
         internal void DisposeForCompletedAwakenings(ref QuickList<int> 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];
@@ -740,8 +756,19 @@ internal void DisposeForCompletedAwakenings(ref QuickList<int> setIndices)
                 sleeper.ReturnSetId(setIndex);
 
             }
-            phaseOneJobs.Dispose(pool);
-            phaseTwoJobs.Dispose(pool);
+            if (phaseOneJobs.Span.Allocated)
+            {
+                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/IslandScaffold.cs b/BepuPhysics/IslandScaffold.cs
index e20c16240..a2e2a8ef7 100644
--- a/BepuPhysics/IslandScaffold.cs
+++ b/BepuPhysics/IslandScaffold.cs
@@ -10,10 +10,10 @@ namespace BepuPhysics
     unsafe struct ConstraintHandleEnumerator : IForEach<int>
     {
         public int* BodyIndices;
-        public int IndexInConstraint;
+        public int Count;
         public void LoopBody(int i)
         {
-            BodyIndices[IndexInConstraint++] = i;
+            BodyIndices[Count++] = i;
         }
     }
 
@@ -76,40 +76,29 @@ 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, Span<int> 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.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<int>(enumerator.BodyIndices, bodiesPerConstraint)))
+            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)
                 {
-                    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<BodyHandle> bodyHandles = stackalloc BodyHandle[bodiesPerConstraint];
-                    for (int i = 0; i < bodyHandles.Length; ++i)
+                    Span<BodyHandle> 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(constraintHandle, bodyHandles, solver.bodies, constraintLocation.TypeId, pool);
+                    fallbackBatch.AllocateForInactive(dynamicBodyHandles, solver.bodies, pool);
                 }
                 return true;
             }
@@ -137,7 +126,7 @@ internal struct IslandScaffold
     {
         public QuickList<int> BodyIndices;
         public QuickList<IslandScaffoldConstraintBatch> Protobatches;
-        public FallbackBatch FallbackBatch;
+        public SequentialFallbackBatch FallbackBatch;
 
         public IslandScaffold(ref QuickList<int> bodyIndices, ref QuickList<ConstraintHandle> constraintHandles, Solver solver, BufferPool pool) : this()
         {
@@ -162,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.EnumerateConnectedDynamicBodies(constraintHandle, ref enumerator);
+            var dynamicBodyIndices = new Span<int>(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;
                 }
@@ -176,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)
diff --git a/BepuPhysics/IslandSleeper.cs b/BepuPhysics/IslandSleeper.cs
index 52e6f52cf..6e6f8e94d 100644
--- a/BepuPhysics/IslandSleeper.cs
+++ b/BepuPhysics/IslandSleeper.cs
@@ -1,18 +1,16 @@
 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;
 
 namespace BepuPhysics
 {
-    public class IslandSleeper
+    public unsafe class IslandSleeper
     {
         IdPool setIdPool;
         Bodies bodies;
@@ -150,7 +148,7 @@ bool EnqueueUnvisitedNeighbors<TTraversalPredicate>(int bodyIndex,
                     constraintHandles.Add(entry.ConnectingConstraintHandle, pool);
                     consideredConstraints.AddUnsafely(entry.ConnectingConstraintHandle.Value);
                     bodyEnumerator.ConstraintBodyIndices.Count = 0;
-                    solver.EnumerateConnectedBodies(entry.ConnectingConstraintHandle, ref bodyEnumerator);
+                    solver.EnumerateConnectedBodyReferences(entry.ConnectingConstraintHandle, ref bodyEnumerator);
                     for (int j = 0; j < bodyEnumerator.ConstraintBodyIndices.Count; ++j)
                     {
                         var connectedBodyIndex = bodyEnumerator.ConstraintBodyIndices[j];
@@ -274,8 +272,8 @@ void FindIslands<TPredicate>(int workerIndex, BufferPool threadPool, ref TPredic
             Debug.Assert(workerTraversalResults.Allocated && workerTraversalResults.Length > workerIndex);
             ref var results = ref workerTraversalResults[workerIndex];
             results.Islands = new QuickList<IslandScaffold>(64, threadPool);
-            var bodyIndices = new QuickList<int>(Math.Min(InitialIslandBodyCapacity, bodies.ActiveSet.Count), threadPool);
-            var constraintHandles = new QuickList<ConstraintHandle>(Math.Min(InitialIslandConstraintCapacity, solver.HandlePool.HighestPossiblyClaimedId + 1), threadPool);
+            var bodyIndices = new QuickList<int>(int.Min(InitialIslandBodyCapacity, bodies.ActiveSet.Count), threadPool);
+            var constraintHandles = new QuickList<ConstraintHandle>(int.Max(8, int.Min(InitialIslandConstraintCapacity, solver.HandlePool.HighestPossiblyClaimedId + 1)), threadPool);
 
             TraversalTest<TPredicate> traversalTest;
             traversalTest.Predicate = predicate;
@@ -329,11 +327,12 @@ void FindIslands(int workerIndex, BufferPool threadPool)
         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.GetThreadMemoryPool(workerIndex));
+            FindIslands(workerIndex, threadDispatcher.WorkerPools[workerIndex]);
         }
 
         Action<int> gatherDelegate;
-        unsafe void Gather(int workerIndex)
+
+        void Gather(int workerIndex)
         {
             while (true)
             {
@@ -359,15 +358,13 @@ 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.Poses[targetIndex] = sourceSet.Poses[sourceIndex];
-                        targetSet.Velocities[targetIndex] = sourceSet.Velocities[sourceIndex];
+                        targetSet.DynamicsState[targetIndex] = sourceSet.DynamicsState[sourceIndex];
 
                         if (sourceCollidable.Shape.Exists)
                         {
                             //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;
@@ -448,15 +445,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.TryRemoveBodyFromConstrainedKinematicsAndRemoveAllConstraintsForBodyFromFallbackBatch(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;
                             }
                         }
                     }
@@ -498,7 +496,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);
@@ -533,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)
@@ -550,7 +548,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<int, PrimitiveComparer<int>>(8, pool);
             for (int batchIndex = 0; batchIndex < island.Protobatches.Count; ++batchIndex)
             {
@@ -559,21 +558,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(
+                        solver.EnumerateConnectedRawBodyReferences(
                             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[rawBodyIndices[i] & Bodies.BodyReferenceMask].Value, pool);
                         }
                         Console.Write($"{handle}, ");
                     }
@@ -589,7 +590,7 @@ unsafe void PrintIsland(ref IslandScaffold island)
         }
 
 
-        unsafe void Sleep(ref QuickList<int> traversalStartBodyIndices, IThreadDispatcher threadDispatcher, bool deterministic, int targetSleptBodyCount, int targetTraversedBodyCount, bool forceSleep)
+        void Sleep(ref QuickList<int> 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.
@@ -646,7 +647,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
@@ -780,7 +781,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);
+                            SequentialFallbackBatch.CreateFrom(ref island.FallbackBatch, pool, out constraintSet.SequentialFallback);
                         }
                     }
                 }
@@ -791,7 +792,7 @@ void DisposeWorkerTraversalResults()
             jobIndex = -1;
             if (threadCount > 1)
             {
-                threadDispatcher.DispatchWorkers(gatherDelegate);
+                threadDispatcher.DispatchWorkers(gatherDelegate, gatheringJobs.Count);
             }
             else
             {
@@ -819,7 +820,7 @@ void DisposeWorkerTraversalResults()
             jobIndex = -1;
             if (threadCount > 1)
             {
-                threadDispatcher.DispatchWorkers(executeRemovalWorkDelegate);
+                threadDispatcher.DispatchWorkers(executeRemovalWorkDelegate, removalJobs.Count);
             }
             else
             {
@@ -842,7 +843,8 @@ void DisposeWorkerTraversalResults()
             jobIndex = -1;
             if (threadCount > 1)
             {
-                threadDispatcher.DispatchWorkers(typeBatchConstraintRemovalDelegate);
+                threadDispatcher.DispatchWorkers(typeBatchConstraintRemovalDelegate, typeBatchConstraintRemovalJobCount);
+                //typeBatchConstraintRemovalDelegate(0);
             }
             else
             {
@@ -951,31 +953,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;
 
-            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<int>(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);
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
+{
+    /// <summary>
+    /// Behaves like a framework SpinWait, but never voluntarily relinquishes the timeslice to off-core threads.
+    /// </summary>
+    /// <remarks><para>There are two big reasons for using this over the regular framework SpinWait:</para>
+    /// <para>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.</para>
+    /// <para>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.)</para>
+    /// <para>SpinWait will also fall back to Sleep(1) by default which obliterates performance, but that behavior can be disabled.</para>
+    /// <para>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.</para></remarks>
+    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/PoseIntegrator.cs b/BepuPhysics/PoseIntegrator.cs
index c1e9f5f1c..f73c31331 100644
--- a/BepuPhysics/PoseIntegrator.cs
+++ b/BepuPhysics/PoseIntegrator.cs
@@ -3,20 +3,18 @@
 using BepuPhysics.Collidables;
 using BepuPhysics.CollisionDetection;
 using System;
-using System.Diagnostics;
 using System.Numerics;
 using System.Runtime.CompilerServices;
 using System.Threading;
+using BepuUtilities.Collections;
+using BepuPhysics.Constraints;
 
 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);
     }
 
     /// <summary>
@@ -27,15 +25,15 @@ public enum AngularIntegrationMode
         /// <summary>
         /// Angular velocity is directly integrated and does not change as the body pose changes. Does not conserve angular momentum.
         /// </summary>
-        Nonconserving,
+        Nonconserving = 0,
         /// <summary>
         /// 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.
         /// </summary>
-        ConserveMomentum,
+        ConserveMomentum = 1,
         /// <summary>
         /// 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.
         /// </summary>
-        ConserveMomentumWithGyroscopicTorque,
+        ConserveMomentumWithGyroscopicTorque = 2,
     }
 
     /// <summary>
@@ -48,6 +46,21 @@ public interface IPoseIntegratorCallbacks
         /// </summary>
         AngularIntegrationMode AngularIntegrationMode { get; }
 
+        /// <summary>
+        /// 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 <see cref="Simulation.Timestep"/>. 
+        /// If false, unconstrained bodies will be integrated with the same number of substeps as the constrained bodies in the solver.
+        /// </summary>
+        bool AllowSubstepsForUnconstrainedBodies { get; }
+
+        /// <summary>
+        /// Gets whether the velocity integration callback should be called for kinematic bodies.
+        /// If true, <see cref="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.
+        /// </summary>
+        bool IntegrateVelocityForKinematics { get; }
+
         /// <summary>
         /// Performs any required initialization logic after the Simulation instance has been constructed.
         /// </summary>
@@ -55,20 +68,29 @@ public interface IPoseIntegratorCallbacks
         void Initialize(Simulation simulation);
 
         /// <summary>
-        /// 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 <see cref="IntegrateVelocity"/>.
+        /// 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. 
         /// </summary>
-        /// <param name="dt">Current time step duration.</param>
+        /// <param name="dt">Current integration time step duration.</param>
+        /// <remarks>This is typically used for precomputing anything expensive that will be used across velocity integration.</remarks>
         void PrepareForIntegration(float dt);
 
         /// <summary>
-        /// Callback called for each active body within the simulation during body integration.
+        /// Callback for a bundle of bodies being integrated.
         /// </summary>
-        /// <param name="bodyIndex">Index of the body being visited.</param>
-        /// <param name="pose">Body's current pose.</param>
+        /// <param name="bodyIndices">Indices of the bodies being integrated in this bundle.</param>
+        /// <param name="position">Current body positions.</param>
+        /// <param name="orientation">Current body orientations.</param>
         /// <param name="localInertia">Body's current local inertia.</param>
-        /// <param name="workerIndex">Index of the worker thread processing this body.</param>
-        /// <param name="velocity">Reference to the body's current velocity to integrate.</param>
-        void IntegrateVelocity(int bodyIndex, in RigidPose pose, in BodyInertia localInertia, int workerIndex, ref BodyVelocity velocity);
+        /// <param name="integrationMask">Mask indicating which lanes are active in the bundle. Active lanes will contain 0xFFFFFFFF, inactive lanes will contain 0.</param>
+        /// <param name="workerIndex">Index of the worker thread processing this bundle.</param>
+        /// <param name="dt">Durations to integrate the velocity over. Can vary over lanes.</param>
+        /// <param name="velocity">Velocity of bodies in the bundle. Any changes to lanes which are not active by the integrationMask will be discarded.</param>
+        void IntegrateVelocity(
+            Vector<int> bodyIndices, Vector3Wide position, QuaternionWide orientation, BodyInertiaWide localInertia,
+            Vector<int> integrationMask, int workerIndex, Vector<float> dt, ref BodyVelocityWide velocity);
     }
 
     /// <summary>
@@ -77,7 +99,7 @@ public interface IPoseIntegratorCallbacks
     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 
@@ -88,7 +110,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();
@@ -97,7 +119,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();
@@ -120,31 +142,118 @@ 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<float> halfDt, out QuaternionWide integrated)
         {
-            start.Validate();
-            angularVelocity.Validate();
             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);
-            QuaternionWide.ConcatenateWithoutOverlap(start, q, out var concatenated);
-            QuaternionWide.Normalize(concatenated, out integrated);
+            q.W = MathHelper.Cos(halfAngle);
+            QuaternionWide.ConcatenateWithoutOverlap(start, q, out var end);
+            end = QuaternionWide.Normalize(end);
             var speedValid = Vector.GreaterThan(speed, new Vector<float>(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)]
+        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);
+        }
+
+        /// <summary>
+        /// 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.
+        /// </summary>
+        [MethodImpl(MethodImplOptions.AggressiveInlining)]
+        static void FallbackIfInertiaIncompatible(in Vector3Wide previousAngularVelocity, ref Vector3Wide angularVelocity)
+        {
+            var infinity = new Vector<float>(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.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)]
+        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(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)]
-        public static unsafe void Integrate(in RigidPose pose, in BodyVelocity velocity, float dt, out RigidPose integratedPose)
+        public static void IntegrateAngularVelocityConserveMomentumWithGyroscopicTorque(
+            in QuaternionWide orientation, in Symmetric3x3Wide localInverseInertia, ref Vector3Wide angularVelocity, in Vector<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(localInverseInertia, out var localInertiaTensor);
+
+            Symmetric3x3Wide.TransformWithoutOverlap(localAngularVelocity, localInertiaTensor, out var localAngularMomentum);
+            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, dt, out var change);
+            var jacobian = localInertiaTensor + change;
+
+            Matrix3x3Wide.Invert(jacobian, out var inverseJacobian);
+            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)]
+        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);
@@ -153,15 +262,9 @@ public static unsafe void Integrate(in RigidPose pose, in BodyVelocity velocity,
 
 
     /// <summary>
-    /// 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.
     /// </summary>
-    /// <remarks>
-    /// 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).
-    /// </remarks>
-    public class PoseIntegrator<TCallbacks> : IPoseIntegrator where TCallbacks : IPoseIntegratorCallbacks
+    public unsafe class PoseIntegrator<TCallbacks> : IPoseIntegrator where TCallbacks : IPoseIntegratorCallbacks
     {
         Bodies bodies;
         Shapes shapes;
@@ -169,28 +272,20 @@ public class PoseIntegrator<TCallbacks> : IPoseIntegrator where TCallbacks : IPo
 
         public TCallbacks Callbacks;
 
-        Action<int> integrateBodiesAndUpdateBoundingBoxesWorker;
         Action<int> predictBoundingBoxesWorker;
-        Action<int> integrateVelocitiesBoundsAndInertiasWorker;
-        Action<int> integrateVelocitiesWorker;
-        Action<int> 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;
         }
 
         [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;
@@ -207,263 +302,77 @@ 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)
+        void PredictBoundingBoxes(int startBundleIndex, int endBundleIndex, float dt, ref BoundingBoxBatcher boundingBoxBatcher, int workerIndex)
         {
-            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);
-            }
+            var activities = bodies.ActiveSet.Activity;
+            var collidables = bodies.ActiveSet.Collidables;
 
-            //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)
+            Helpers.FillVectorWithLaneIndices(out var laneIndexOffsets);
+            var dtWide = new Vector<float>(dt);
+            var bodyCount = bodies.ActiveSet.Count;
+            for (int bundleIndex = startBundleIndex; bundleIndex < endBundleIndex; ++bundleIndex)
             {
-                if (!Bodies.HasLockedInertia(localInertia.InverseInertiaTensor))
+                var bundleStartBodyIndex = bundleIndex * Vector<float>.Count;
+                var countInBundle = bodyCount - bundleStartBodyIndex;
+                if (countInBundle > Vector<int>.Count)
+                    countInBundle = Vector<int>.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<int>(bundleStartBodyIndex) + laneIndexOffsets;
+                bodies.GatherState<AccessAll>(laneIndices, false, out var position, out var orientation, out var velocity, out var inertia);
+
+                Vector<int> integrationMask;
+                if (Callbacks.IntegrateVelocityForKinematics)
                 {
-                    IntegrateAngularVelocityConserving(previousOrientation, pose, localInertia, inertia, ref angularVelocity, dt);
+                    integrationMask = BundleIndexing.CreateMaskForCountInBundle(countInBundle);
                 }
-            }
-        }
-
-        [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))
+                else
                 {
-                    PoseIntegration.Integrate(pose.Orientation, angularVelocity, -dt, out var previousOrientation);
-                    IntegrateAngularVelocityConserving(previousOrientation, pose, localInertia, inertia, ref angularVelocity, dt);
+                    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();
 
-        unsafe void IntegrateBodiesAndUpdateBoundingBoxes(int startIndex, int endIndex, float dt, ref BoundingBoxBatcher boundingBoxBatcher, int workerIndex)
-        {
-            ref var basePoses = ref bodies.ActiveSet.Poses[0];
-            ref var baseVelocities = ref bodies.ActiveSet.Velocities[0];
-            ref var baseLocalInertia = ref bodies.ActiveSet.LocalInertias[0];
-            ref var baseInertias = ref bodies.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)
-            {
-                ref var pose = ref Unsafe.Add(ref basePoses, i);
-                ref var velocity = ref Unsafe.Add(ref baseVelocities, i);
-
-                var previousOrientation = pose.Orientation; //This is unused if conservation of angular momentum is disabled... compiler *may* remove it...
-                PoseIntegration.Integrate(pose, velocity, dt, out 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 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.
-                ref var localInertia = ref Unsafe.Add(ref baseLocalInertia, i);
-                ref var inertia = ref Unsafe.Add(ref baseInertias, i);
-                PoseIntegration.RotateInverseInertia(localInertia.InverseInertiaTensor, pose.Orientation, out inertia.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;
-
-                IntegrateAngularVelocity(previousOrientation, pose, localInertia, inertia, ref velocity.Angular, dt);
-                Callbacks.IntegrateVelocity(i, pose, localInertia, workerIndex, ref 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, pose, 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 startIndex, int endIndex, float dt, ref BoundingBoxBatcher boundingBoxBatcher, int workerIndex)
-        {
-            ref var basePoses = ref bodies.ActiveSet.Poses[0];
-            ref var baseVelocities = ref bodies.ActiveSet.Velocities[0];
-            ref var baseLocalInertia = ref bodies.ActiveSet.LocalInertias[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 pose = ref Unsafe.Add(ref basePoses, i);
-                ref var velocity = ref Unsafe.Add(ref baseVelocities, i);
-                pose.Position.Validate();
-                pose.Orientation.ValidateOrientation();
-                velocity.Linear.Validate();
-                velocity.Angular.Validate();
-
-                UpdateSleepCandidacy(ref velocity, ref Unsafe.Add(ref baseActivity, i));
-
-                //Bounding box prediction does not need to update inertia tensors.                
-                var integratedVelocity = velocity;
-                Callbacks.IntegrateVelocity(i, pose, Unsafe.Add(ref baseLocalInertia, i), 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, pose, integratedVelocity, Unsafe.Add(ref baseCollidable, i));
-            }
-        }
-
-        unsafe void IntegrateVelocitiesBoundsAndInertias(int startIndex, int endIndex, float dt, ref BoundingBoxBatcher boundingBoxBatcher, int workerIndex)
-        {
-            ref var basePoses = ref bodies.ActiveSet.Poses[0];
-            ref var baseVelocities = ref bodies.ActiveSet.Velocities[0];
-            ref var baseLocalInertia = ref bodies.ActiveSet.LocalInertias[0];
-            ref var baseInertias = ref bodies.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)
-            {
-                ref var pose = ref Unsafe.Add(ref basePoses, i);
-                ref var velocity = ref Unsafe.Add(ref baseVelocities, i);
-                pose.Position.Validate();
-                pose.Orientation.ValidateOrientation();
-                velocity.Linear.Validate();
-                velocity.Angular.Validate();
-
-                UpdateSleepCandidacy(ref 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, pose.Orientation, out inertia.InverseInertiaTensor);
-                inertia.InverseMass = localInertia.InverseMass;
-
-                IntegrateAngularVelocity(pose, localInertia, inertia, ref velocity.Angular, dt);
-                Callbacks.IntegrateVelocity(i, pose, localInertia, workerIndex, ref velocity);
-
-                boundingBoxBatcher.Add(i, pose, velocity, Unsafe.Add(ref baseCollidable, i));
-            }
-        }
-
-
-        unsafe void IntegrateVelocities(int startIndex, int endIndex, float dt, int workerIndex)
-        {
-            ref var basePoses = ref bodies.ActiveSet.Poses[0];
-            ref var baseVelocities = ref bodies.ActiveSet.Velocities[0];
-            ref var baseLocalInertia = ref bodies.ActiveSet.LocalInertias[0];
-            ref var baseInertias = ref bodies.Inertias[0];
-            for (int i = startIndex; i < endIndex; ++i)
-            {
-                ref var pose = ref Unsafe.Add(ref basePoses, i);
-                ref var velocity = ref Unsafe.Add(ref baseVelocities, i);
-                pose.Position.Validate();
-                pose.Orientation.ValidateOrientation();
-                velocity.Linear.Validate();
-                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, pose.Orientation, out inertia.InverseInertiaTensor);
-                inertia.InverseMass = localInertia.InverseMass;
-
-                IntegrateAngularVelocity(pose, localInertia, inertia, ref velocity.Angular, dt);
-                Callbacks.IntegrateVelocity(i, pose, localInertia, workerIndex, ref velocity);
-            }
-        }
+                //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<int>.Zero))
+                    Callbacks.IntegrateVelocity(laneIndices, position, orientation, inertia, integrationMask, workerIndex, dtWide, ref velocity);
 
-        unsafe void IntegratePoses(int startIndex, int endIndex, float dt, int workerIndex)
-        {
-            ref var basePoses = ref bodies.ActiveSet.Poses[0];
-            ref var baseVelocities = ref bodies.ActiveSet.Velocities[0];
-            for (int i = startIndex; i < endIndex; ++i)
-            {
-                ref var pose = ref Unsafe.Add(ref basePoses, i);
-                ref var velocity = ref Unsafe.Add(ref baseVelocities, i);
-                pose.Position.Validate();
-                pose.Orientation.ValidateOrientation();
-                velocity.Linear.Validate();
-                velocity.Angular.Validate();
-
-                PoseIntegration.Integrate(pose, velocity, dt, out pose);
+                for (int i = 0; i < countInBundle; ++i)
+                {
+                    var bodyIndex = i + bundleStartBodyIndex;
+                    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]);
+                }
             }
         }
 
 
         float cachedDt;
-        int bodiesPerJob;
+        int jobSize;
+        int substepCount;
         IThreadDispatcher threadDispatcher;
 
         //Note that we aren't using a very cache-friendly work distribution here.
@@ -471,7 +380,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)
@@ -480,193 +389,339 @@ 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;
-            Debug.Assert(exclusiveEnd > start, "Jobs that would involve bundles beyond the body count should not be created.");
+            start = jobIndex * jobSize;
+            exclusiveEnd = start + jobSize;
+            if (exclusiveEnd > maximumJobInterval)
+                exclusiveEnd = maximumJobInterval;
             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);
-            var bodyCount = bodies.ActiveSet.Count;
-            while (TryGetJob(bodyCount, out var start, out var exclusiveEnd))
+            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))
             {
                 PredictBoundingBoxes(start, exclusiveEnd, cachedDt, ref boundingBoxUpdater, 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(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;
         }
 
-
-        public void IntegrateBodiesAndUpdateBoundingBoxes(float dt, BufferPool pool, IThreadDispatcher threadDispatcher = null)
+        public void PredictBoundingBoxes(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);
             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(dt, threadDispatcher.ThreadCount);
+                PrepareForMultithreadedExecution(BundleIndexing.GetBundleCount(bodies.ActiveSet.Count), dt, threadDispatcher.ThreadCount);
                 this.threadDispatcher = threadDispatcher;
-                threadDispatcher.DispatchWorkers(integrateBodiesAndUpdateBoundingBoxesWorker);
+                threadDispatcher.DispatchWorkers(predictBoundingBoxesWorker, availableJobCount);
+                //predictBoundingBoxesWorker(0);
                 this.threadDispatcher = null;
             }
             else
             {
                 var boundingBoxUpdater = new BoundingBoxBatcher(bodies, shapes, broadPhase, pool, dt);
-                IntegrateBodiesAndUpdateBoundingBoxes(0, bodies.ActiveSet.Count, dt, ref boundingBoxUpdater, 0);
+                PredictBoundingBoxes(0, BundleIndexing.GetBundleCount(bodies.ActiveSet.Count), dt, ref boundingBoxUpdater, 0);
                 boundingBoxUpdater.Flush();
             }
+
         }
 
-        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);
-            if (threadDispatcher != null)
-            {
-                PrepareForMultithreadedExecution(dt, threadDispatcher.ThreadCount);
-                this.threadDispatcher = threadDispatcher;
-                threadDispatcher.DispatchWorkers(predictBoundingBoxesWorker);
-                this.threadDispatcher = null;
-            }
-            else
+        /// <summary>
+        /// 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.
+        /// </summary>
+        internal void IntegrateKinematicVelocities(Buffer<int> bodyHandles, int bundleStartIndex, int bundleEndIndex, float substepDt, int workerIndex)
+        {
+            var bodyCount = bodyHandles.Length;
+            var bundleCount = BundleIndexing.GetBundleCount(bodyCount);
+            var bundleDt = new Vector<float>(substepDt);
+            var halfDt = bundleDt * new Vector<float>(0.5f);
+
+            int* bodyIndices = stackalloc int[Vector<int>.Count];
+            var bodyIndicesSpan = new Span<int>(bodyIndices, Vector<int>.Count);
+            ref var callbacks = ref Callbacks;
+            var handleToLocation = bodies.HandleToLocation;
+            BodyInertiaWide zeroInertia = default;
+
+            for (int bundleIndex = bundleStartIndex; bundleIndex < bundleEndIndex; ++bundleIndex)
             {
-                var boundingBoxUpdater = new BoundingBoxBatcher(bodies, shapes, broadPhase, pool, dt);
-                PredictBoundingBoxes(0, bodies.ActiveSet.Count, dt, ref boundingBoxUpdater, 0);
-                boundingBoxUpdater.Flush();
-            }
+                var bundleBaseIndex = bundleIndex * Vector<float>.Count;
+                var countInBundle = Math.Min(bodyCount - bundleBaseIndex, Vector<float>.Count);
+                for (int i = 0; i < countInBundle; ++i)
+                {
+                    bodyIndices[i] = handleToLocation[bodyHandles[bundleBaseIndex + i]].Index;
+                }
+
+                var existingMask = BundleIndexing.CreateMaskForCountInBundle(countInBundle);
+                var trailingMask = Vector.OnesComplement(existingMask);
+                var bodyIndicesVector = Vector.BitwiseOr(trailingMask, new Vector<int>(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!)
+                bodies.GatherState<AccessNoInertia>(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<AccessAll>(ref velocity, ref bodyIndicesVector);
+
+            }
         }
 
-        public void IntegrateVelocitiesBoundsAndInertias(float dt, BufferPool pool, IThreadDispatcher threadDispatcher = null)
+        /// <summary>
+        /// 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.
+        /// </summary>
+        internal void IntegrateKinematicPosesAndVelocities(Buffer<int> bodyHandles, int bundleStartIndex, int bundleEndIndex, float substepDt, int workerIndex)
         {
-            bodies.EnsureInertiasCapacity(Math.Max(1, bodies.ActiveSet.Count));
+            var bodyCount = bodyHandles.Length;
+            var bundleCount = BundleIndexing.GetBundleCount(bodyCount);
+            var bundleDt = new Vector<float>(substepDt);
+            var halfDt = bundleDt * new Vector<float>(0.5f);
+
+            int* bodyIndices = stackalloc int[Vector<int>.Count];
+            var bodyIndicesSpan = new Span<int>(bodyIndices, Vector<int>.Count);
+            ref var callbacks = ref Callbacks;
+            var handleToLocation = bodies.HandleToLocation;
+            BodyInertiaWide zeroInertia = default;
+
+            for (int bundleIndex = bundleStartIndex; bundleIndex < bundleEndIndex; ++bundleIndex)
+            {
+                var bundleBaseIndex = bundleIndex * Vector<float>.Count;
+                var countInBundle = Math.Min(bodyCount - bundleBaseIndex, Vector<float>.Count);
+                for (int i = 0; i < countInBundle; ++i)
+                {
+                    bodyIndices[i] = handleToLocation[bodyHandles[bundleBaseIndex + i]].Index;
+                }
 
-            var workerCount = threadDispatcher == null ? 1 : threadDispatcher.ThreadCount;
+                var existingMask = BundleIndexing.CreateMaskForCountInBundle(countInBundle);
+                var trailingMask = Vector.OnesComplement(existingMask);
+                var bodyIndicesVector = new Vector<int>(bodyIndicesSpan);
+                bodyIndicesVector = Vector.BitwiseOr(trailingMask, bodyIndicesVector);
+                bodies.GatherState<AccessNoInertia>(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);
+                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.ScatterVelocities<AccessAll>(ref velocity, ref bodyIndicesVector);
+                }
 
-            Callbacks.PrepareForIntegration(dt);
-            if (threadDispatcher != null)
-            {
-                PrepareForMultithreadedExecution(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)
+        void IntegrateBundlesAfterSubstepping(ref IndexSet mergedConstrainedBodyHandles, int bundleStartIndex, int bundleEndIndex, float dt, float substepDt, int substepCount, int workerIndex)
         {
-            //Isolated velocity integration is used by substeppers that also expect an inertia update.
-            bodies.EnsureInertiasCapacity(Math.Max(1, bodies.ActiveSet.Count));
+            var bodyCount = bodies.ActiveSet.Count;
+            var bundleCount = BundleIndexing.GetBundleCount(bodyCount);
+            var bundleDt = new Vector<float>(dt);
+            var bundleSubstepDt = new Vector<float>(substepDt);
+
+            int* unconstrainedMaskPointer = stackalloc int[Vector<int>.Count];
+            int* bodyIndicesPointer = stackalloc int[Vector<int>.Count];
+            var unconstrainedMaskSpan = new Span<int>(unconstrainedMaskPointer, Vector<int>.Count);
+            var bodyIndicesSpan = new Span<int>(bodyIndicesPointer, Vector<int>.Count);
+            var negativeOne = new Vector<int>(-1);
+            ref var callbacks = ref Callbacks;
+            ref var indexToHandle = ref bodies.ActiveSet.IndexToHandle;
+
+            for (int i = bundleStartIndex; i < bundleEndIndex; ++i)
+            {
+                var bundleBaseIndex = i * Vector<float>.Count;
+                var countInBundle = Math.Min(bodyCount - bundleBaseIndex, Vector<float>.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;
+                    }
+                }
+                var unconstrainedMask = new Vector<int>(unconstrainedMaskSpan);
+                var bodyIndices = new Vector<int>(bodyIndicesSpan);
+                if (countInBundle < Vector<int>.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);
+                }
 
-            var workerCount = threadDispatcher == null ? 1 : threadDispatcher.ThreadCount;
+                Vector<float> bundleEffectiveDt;
+                if (callbacks.AllowSubstepsForUnconstrainedBodies)
+                {
+                    bundleEffectiveDt = bundleSubstepDt;
+                }
+                else
+                {
+                    bundleEffectiveDt = Vector.ConditionalSelect(unconstrainedMask, bundleDt, bundleSubstepDt);
+                }
+                var halfDt = bundleEffectiveDt * new Vector<float>(0.5f);
+                bodies.GatherState<AccessAll>(bodyIndices, false, out var position, out var orientation, out var velocity, out var localInertia);
 
-            Callbacks.PrepareForIntegration(dt);
-            if (threadDispatcher != null)
-            {
-                PrepareForMultithreadedExecution(dt, threadDispatcher.ThreadCount);
-                this.threadDispatcher = threadDispatcher;
-                threadDispatcher.DispatchWorkers(integrateVelocitiesWorker);
-                this.threadDispatcher = null;
+
+                Vector<int> unconstrainedVelocityIntegrationMask;
+                bool anyBodyInBundleNeedsVelocityIntegration;
+                if (callbacks.IntegrateVelocityForKinematics)
+                {
+                    unconstrainedVelocityIntegrationMask = unconstrainedMask;
+                    anyBodyInBundleNeedsVelocityIntegration = anyBodyInBundleIsUnconstrained;
+                }
+                else
+                {
+                    var isKinematic = Bodies.IsKinematic(localInertia);
+                    unconstrainedVelocityIntegrationMask = Vector.AndNot(unconstrainedMask, isKinematic);
+                    anyBodyInBundleNeedsVelocityIntegration = Vector.LessThanAny(unconstrainedVelocityIntegrationMask, Vector<int>.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, 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)
+                {
+                    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;
+
+                        if (anyBodyInBundleNeedsVelocityIntegration)
+                        {
+                            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);
+                        }
+
+                        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, bundleEffectiveDt);
+                        }
+                        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, bodyIndices, integratePoseMask);
+                        if (anyBodyInBundleNeedsVelocityIntegration)
+                        {
+                            bodies.ScatterVelocities<AccessAll>(ref velocity, ref velocityMaskedBodyIndices);
+                        }
+                    }
+                }
+                else
+                {
+                    //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, bodyIndices, integratePoseMask);
+                }
             }
-            else
+        }
+
+        Action<int> 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))
             {
-                IntegrateVelocities(0, bodies.ActiveSet.Count, dt, 0);
+                IntegrateBundlesAfterSubstepping(ref constrainedBodies, start, exclusiveEnd, cachedDt, substepDt, substepCount, workerIndex);
             }
         }
 
-        public void IntegratePoses(float dt, BufferPool pool, IThreadDispatcher threadDispatcher = null)
+        public void IntegrateAfterSubstepping(IndexSet constrainedBodies, float dt, int substepCount, IThreadDispatcher threadDispatcher)
         {
-            //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)
+            //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(dt, threadDispatcher.ThreadCount);
+                PrepareForMultithreadedExecution(BundleIndexing.GetBundleCount(bodies.ActiveSet.Count), dt, threadDispatcher.ThreadCount, substepCount);
+                this.constrainedBodies = constrainedBodies;
                 this.threadDispatcher = threadDispatcher;
-                threadDispatcher.DispatchWorkers(integratePosesWorker);
+                threadDispatcher.DispatchWorkers(integrateAfterSubsteppingWorker, availableJobCount);
                 this.threadDispatcher = null;
+                this.constrainedBodies = default;
             }
             else
             {
-                IntegratePoses(0, bodies.ActiveSet.Count, dt, 0);
+                IntegrateBundlesAfterSubstepping(ref constrainedBodies, 0, BundleIndexing.GetBundleCount(bodies.ActiveSet.Count), dt, substepDt, substepCount, 0);
             }
         }
     }
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
-{
-    /// <summary>
-    /// Updates the simulation in the order of: sleeper -> integrate body poses, velocity and bounding boxes -> collision detection -> solver -> data structure optimization.
-    /// </summary>
-    public class PositionFirstTimestepper : ITimestepper
-    {
-        /// <summary>
-        /// Fires after the sleeper completes and before bodies are integrated.
-        /// </summary>
-        public event TimestepperStageHandler Slept;
-        /// <summary>
-        /// Fires after bodies have had their position, velocity, and bounding boxes updated, but before collision detection begins.
-        /// </summary>
-        public event TimestepperStageHandler BeforeCollisionDetection;
-        /// <summary>
-        /// Fires after all collisions have been identified, but before constraints are solved.
-        /// </summary>
-        public event TimestepperStageHandler CollisionsDetected;
-        /// <summary>
-        /// Fires after the solver executes and before data structures are incrementally optimized.
-        /// </summary>
-        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/SequentialFallbackBatch.cs b/BepuPhysics/SequentialFallbackBatch.cs
new file mode 100644
index 000000000..9bf7df10f
--- /dev/null
+++ b/BepuPhysics/SequentialFallbackBatch.cs
@@ -0,0 +1,280 @@
+using BepuUtilities.Collections;
+using BepuUtilities.Memory;
+using System;
+using System.Diagnostics;
+using System.Runtime.CompilerServices;
+
+namespace BepuPhysics
+{
+    interface IBodyReferenceGetter
+    {
+        static abstract int GetBodyReference(Bodies bodies, BodyHandle handle);
+    }
+
+    struct ActiveSetGetter : IBodyReferenceGetter
+    {
+        [MethodImpl(MethodImplOptions.AggressiveInlining)]
+        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.");
+            return bodyLocation.Index;
+        }
+    }
+    struct InactiveSetGetter : IBodyReferenceGetter
+    {
+        [MethodImpl(MethodImplOptions.AggressiveInlining)]
+        public static int GetBodyReference(Bodies bodies, BodyHandle bodyHandle)
+        {
+            return bodyHandle.Value;
+        }
+    }
+
+    /// <summary>
+    /// 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.
+    /// </summary>
+    public struct SequentialFallbackBatch
+    {
+        /// <summary>
+        /// Gets the number of bodies in the fallback batch.
+        /// </summary>
+        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.
+        //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<int, int, PrimitiveComparer<int>> dynamicBodyConstraintCounts;
+
+        [MethodImpl(MethodImplOptions.AggressiveInlining)]
+        void Allocate<TBodyReferenceGetter>(Span<BodyHandle> dynamicBodyHandles, Bodies bodies,
+            BufferPool pool, TBodyReferenceGetter bodyReferenceGetter, int minimumBodyCapacity)
+            where TBodyReferenceGetter : struct, IBodyReferenceGetter
+        {
+            EnsureCapacity(Math.Max(dynamicBodyConstraintCounts.Count + dynamicBodyHandles.Length, minimumBodyCapacity), pool);
+            for (int i = 0; i < dynamicBodyHandles.Length; ++i)
+            {
+                var bodyReference = TBodyReferenceGetter.GetBodyReference(bodies, dynamicBodyHandles[i]);
+
+                if (dynamicBodyConstraintCounts.FindOrAllocateSlotUnsafely(bodyReference, out var slotIndex))
+                {
+                    ++dynamicBodyConstraintCounts.Values[slotIndex];
+                }
+                else
+                {
+                    dynamicBodyConstraintCounts.Values[slotIndex] = 1;
+                }
+            }
+        }
+
+
+
+        [MethodImpl(MethodImplOptions.AggressiveInlining)]
+        internal void AllocateForActive(Span<BodyHandle> dynamicBodyHandles, Bodies bodies,
+           BufferPool pool, int minimumBodyCapacity = 8)
+        {
+            Allocate(dynamicBodyHandles, bodies, pool, new ActiveSetGetter(), minimumBodyCapacity);
+        }
+
+        [MethodImpl(MethodImplOptions.AggressiveInlining)]
+        internal void AllocateForInactive(Span<BodyHandle> dynamicBodyHandles, Bodies bodies,
+          BufferPool pool, int minimumBodyCapacity = 8)
+        {
+            Allocate(dynamicBodyHandles, bodies, pool, new InactiveSetGetter(), minimumBodyCapacity);
+        }
+
+
+        /// <summary>
+        /// Removes a constraint from a body in the fallback batch.
+        /// </summary>
+        /// <param name="bodyReference">Body associated with a constraint in the fallback batch.</param>
+        /// <param name="allocationIdsToFree">Allocations that should be freed once execution is back in a safe context.</param>
+        /// <returns>True if the body was dynamic and no longer has any constraints associated with it in the fallback batch, false otherwise.</returns>
+        internal bool RemoveOneBodyReferenceFromDynamicsSet(int bodyReference, ref QuickList<int> allocationIdsToFree)
+        {
+            if (!dynamicBodyConstraintCounts.GetTableIndices(ref bodyReference, out var tableIndex, out var bodyReferencesIndex))
+                return false;
+            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;
+                dynamicBodyConstraintCounts.FastRemove(tableIndex, bodyReferencesIndex);
+                if (dynamicBodyConstraintCounts.Count == 0)
+                {
+                    //No constraints remain in the fallback batch. Drop the dictionary.
+                    allocationIdsToFree.AllocateUnsafely() = dynamicBodyConstraintCounts.Keys.Id;
+                    allocationIdsToFree.AllocateUnsafely() = dynamicBodyConstraintCounts.Values.Id;
+                    allocationIdsToFree.AllocateUnsafely() = dynamicBodyConstraintCounts.Table.Id;
+                    dynamicBodyConstraintCounts = default;
+                }
+                return true;
+            }
+            return false;
+        }
+
+        /// <summary>
+        /// Removes a body from the fallback batch's dynamic body constraint counts if it is present.
+        /// </summary>
+        /// <param name="bodyReference">Reference to the body to remove from the fallback batch.</param>
+        /// <param name="allocationIdsToFree">Allocations that should be freed once execution is back in a safe context.</param>
+        /// <returns>True if the body was present in the fallback batch and was removed, false otherwise.</returns>
+        internal bool TryRemoveDynamicBodyFromTracking(int bodyReference, ref QuickList<int> allocationIdsToFree)
+        {
+            if (dynamicBodyConstraintCounts.Keys.Allocated && dynamicBodyConstraintCounts.GetTableIndices(ref bodyReference, out var tableIndex, out var bodyReferencesIndex))
+            {
+                ref var constraintReferences = ref dynamicBodyConstraintCounts.Values[bodyReferencesIndex];
+                //If there are no more constraints associated with this body, get rid of the body list.
+                dynamicBodyConstraintCounts.FastRemove(tableIndex, bodyReferencesIndex);
+                if (dynamicBodyConstraintCounts.Count == 0)
+                {
+                    //No constraints remain in the fallback batch. Drop the dictionary.
+                    allocationIdsToFree.AllocateUnsafely() = dynamicBodyConstraintCounts.Keys.Id;
+                    allocationIdsToFree.AllocateUnsafely() = dynamicBodyConstraintCounts.Values.Id;
+                    allocationIdsToFree.AllocateUnsafely() = dynamicBodyConstraintCounts.Table.Id;
+                    dynamicBodyConstraintCounts = default;
+                }
+                return true;
+            }
+            return false;
+        }
+
+
+        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;
+            var bodyIndices = stackalloc int[bodyCount];
+            var enumerator = new PassthroughReferenceCollector(bodyIndices);
+            var maximumAllocationIdsToFree = 3 + bodyCount * 2;
+            var allocationIdsToRemoveMemory = stackalloc int[maximumAllocationIdsToFree];
+            var initialSpan = new Buffer<int>(allocationIdsToRemoveMemory, maximumAllocationIdsToFree);
+            var allocationIdsToFree = new QuickList<int>(initialSpan);
+            solver.EnumerateConnectedRawBodyReferences(ref batch.TypeBatches[batch.TypeIndexToTypeBatchIndex[typeId]], indexInTypeBatch, ref enumerator);
+            for (int i = 0; i < bodyCount; ++i)
+            {
+                var rawBodyIndex = bodyIndices[i];
+                if (Bodies.IsEncodedDynamicReference(rawBodyIndex))
+                {
+                    var bodyIndex = rawBodyIndex & Bodies.BodyReferenceMask;
+                    if (RemoveOneBodyReferenceFromDynamicsSet(bodyIndex, ref allocationIdsToFree))
+                    {
+                        fallbackBatchHandles.Remove(solver.bodies.ActiveSet.IndexToHandle[bodyIndex].Value);
+                    }
+                }
+            }
+            for (int i = 0; i < allocationIdsToFree.Count; ++i)
+            {
+                bufferPool.ReturnUnsafely(allocationIdsToFree[i]);
+            }
+        }
+
+        [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.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.
+                    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 PassthroughReferenceCollector(connectedBodies);
+                        solver.EnumerateConnectedDynamicBodies(constraintHandle, ref collector);
+                        for (int i = 0; i < bodiesPerConstraint; ++i)
+                        {
+                            var localBodyIndex = bodyConstraintCounts.IndexOf(connectedBodies[i]);
+                            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];
+                        }
+                    }
+                }
+            }
+        }
+        [Conditional("DEBUG")]
+        public static void ValidateReferences(Solver solver)
+        {
+            for (int i = 0; i < solver.Sets.Length; ++i)
+            {
+                if (solver.Sets[i].Allocated)
+                    ValidateSetReferences(solver, i);
+            }
+        }
+        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);
+            var references = dynamicBodyConstraintCounts.Values[elementIndex];
+            dynamicBodyConstraintCounts.FastRemove(tableIndex, elementIndex);
+            dynamicBodyConstraintCounts.AddUnsafely(ref newBodyLocation, references);
+        }
+
+        internal void UpdateForBodyMemorySwap(int a, int 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 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.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 (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. 
+                dynamicBodyConstraintCounts.EnsureCapacity(bodyCapacity, pool);
+            }
+            else
+            {
+                dynamicBodyConstraintCounts = new QuickDictionary<int, int, PrimitiveComparer<int>>(bodyCapacity, pool);
+            }
+
+        }
+
+        public void Compact(BufferPool pool)
+        {
+            if (dynamicBodyConstraintCounts.Keys.Allocated)
+            {
+                dynamicBodyConstraintCounts.Compact(pool);
+            }
+        }
+
+
+        public void Dispose(BufferPool pool)
+        {
+            if (dynamicBodyConstraintCounts.Keys.Allocated)
+            {
+                dynamicBodyConstraintCounts.Dispose(pool);
+            }
+        }
+    }
+}
diff --git a/BepuPhysics/Simulation.cs b/BepuPhysics/Simulation.cs
index cd7d13b81..2cb5b61c3 100644
--- a/BepuPhysics/Simulation.cs
+++ b/BepuPhysics/Simulation.cs
@@ -1,9 +1,7 @@
 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;
@@ -13,437 +11,406 @@
 [module: SkipLocalsInit]
 #endif
 
-namespace BepuPhysics
+namespace BepuPhysics;
+
+/// <summary>
+/// Orchestrates the bookkeeping and execution of a full dynamic simulation.
+/// </summary>
+public partial class Simulation : IDisposable
 {
-    /// <summary>
-    /// Orchestrates the bookkeeping and execution of a full dynamic simulation.
+    /// <summary>
+    /// Gets the system responsible for awakening bodies within the simulation.
     /// </summary>
-    public partial class Simulation : IDisposable
-    {
-        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 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; }
-        public BroadPhase BroadPhase { get; private set; }
-        public CollidableOverlapFinder BroadPhaseOverlapFinder { get; private set; }
-        public NarrowPhase NarrowPhase { get; private set; }
-
-        SimulationProfiler profiler = new(13);
-        /// <summary>
-        /// 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.
-        /// </summary>
-        public SimulationProfiler Profiler { get { return profiler; } }
-
-        //Helpers shared across at least two stages.
-        internal ConstraintRemover constraintRemover;
-
-        /// <summary>
-        /// Gets the main memory pool used to fill persistent structures and main thread ephemeral resources across the engine.
-        /// </summary>
-        public BufferPool BufferPool { get; private set; }
-
-        /// <summary>
-        /// Gets the timestepper used to update the simulation state.
-        /// </summary>
-        public ITimestepper Timestepper { get; private set; }
-
-        /// <summary>
-        /// 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.
-        /// </summary>
-        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;
+    public IslandAwakener Awakener { get; private set; }
+    /// <summary>
+    /// Gets the system responsible for putting bodies to sleep within the simulation.
+    /// </summary>
+    public IslandSleeper Sleeper { get; private set; }
+    /// <summary>
+    /// Gets the collection of bodies in the simulation.
+    /// </summary>
+    public Bodies Bodies { get; private set; }
+    /// <summary>
+    /// Gets the collection of statics within the simulation.
+    /// </summary>
+    public Statics Statics { get; private set; }
+    /// <summary>
+    /// Gets or sets the collection of shapes used by the simulation.
+    /// </summary>
+    /// <remarks>
+    /// While <see cref="BepuPhysics.Collidables.Shapes"/> instances can be shared between multiple <see cref="Simulation"/> instances, there are no guarantees of thread safety.
+    /// Further, setting the <see cref="Shapes"/> 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.
+    /// </remarks>
+    public Shapes Shapes { get; set; }
+    /// <summary>
+    /// Gets the batch compressor used to compact constraints into fewer solver batches.
+    /// </summary>
+    public BatchCompressor SolverBatchCompressor { get; private set; }
+    /// <summary>
+    /// Gets the solver used to solved constraints within the simulation.
+    /// </summary>
+    public Solver Solver { get; private set; }
+    /// <summary>
+    /// Gets the integrator used to update velocities and poses within the simulation.
+    /// </summary>
+    public IPoseIntegrator PoseIntegrator { get; private set; }
+    /// <summary>
+    /// Gets the broad phase used by the simulation. Supports accelerated ray and volume queries.
+    /// </summary>
+    public BroadPhase BroadPhase { get; private set; }
+    /// <summary>
+    /// Gets the system used to find overlapping pairs within the simulation.
+    /// </summary>
+    public CollidableOverlapFinder BroadPhaseOverlapFinder { get; private set; }
+    /// <summary>
+    /// Gets the system used to identify contacts in colliding pairs of shapes and to update contact constraint data.
+    /// </summary>
+    public NarrowPhase NarrowPhase { get; private set; }
 
-        }
+    SimulationProfiler profiler = new(13);
+    /// <summary>
+    /// 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.
+    /// </summary>
+    public SimulationProfiler Profiler { get { return profiler; } }
 
-        /// <summary>
-        /// Constructs a simulation supporting dynamic movement and constraints with the specified narrow phase callbacks.
-        /// </summary>
-        /// <param name="bufferPool">Buffer pool used to fill persistent structures and main thread ephemeral resources across the engine.</param>
-        /// <param name="narrowPhaseCallbacks">Callbacks to use in the narrow phase.</param>
-        /// <param name="poseIntegratorCallbacks">Callbacks to use in the pose integrator.</param>
-        /// <param name="timestepper">Timestepper that defines how the simulation state should be updated.</param>
-        /// <param name="solverIterationCount">Number of iterations the solver should use.</param>
-        /// <param name="solverFallbackBatchThreshold">Number of synchronized batches the solver should maintain before falling back to a lower quality jacobi hybrid solver.</param>
-        /// <param name="initialAllocationSizes">Allocation sizes to initialize the simulation with. If left null, default values are chosen.</param>
-        /// <returns>New simulation.</returns>
-        public static Simulation Create<TNarrowPhaseCallbacks, TPoseIntegratorCallbacks>(
-            BufferPool bufferPool, TNarrowPhaseCallbacks narrowPhaseCallbacks, TPoseIntegratorCallbacks poseIntegratorCallbacks, ITimestepper timestepper,
-            int solverIterationCount = 8, int solverFallbackBatchThreshold = 64, SimulationAllocationSizes? initialAllocationSizes = null)
-            where TNarrowPhaseCallbacks : struct, INarrowPhaseCallbacks
-            where TPoseIntegratorCallbacks : struct, IPoseIntegratorCallbacks
-        {
-            if (initialAllocationSizes == null)
-            {
-                initialAllocationSizes = new SimulationAllocationSizes
-                {
-                    Bodies = 4096,
-                    Statics = 4096,
-                    ShapesPerType = 128,
-                    ConstraintCountPerBodyEstimate = 8,
-                    Constraints = 16384,
-                    ConstraintsPerTypeBatch = 256
-                };
-            }
+    //Helpers shared across at least two stages.
+    internal ConstraintRemover constraintRemover;
 
-            var simulation = new Simulation(bufferPool, initialAllocationSizes.Value, solverIterationCount, solverFallbackBatchThreshold, timestepper);
-            var poseIntegrator = new PoseIntegrator<TPoseIntegratorCallbacks>(simulation.Bodies, simulation.Shapes, simulation.BroadPhase, poseIntegratorCallbacks);
-            simulation.PoseIntegrator = poseIntegrator;
-            var narrowPhase = new NarrowPhase<TNarrowPhaseCallbacks>(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<TNarrowPhaseCallbacks>(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;
-        }
+    /// <summary>
+    /// Gets the main memory pool used to fill persistent structures and main thread ephemeral resources across the engine.
+    /// </summary>
+    public BufferPool BufferPool { get; private set; }
 
+    /// <summary>
+    /// Gets the timestepper used to update the simulation state.
+    /// </summary>
+    public ITimestepper Timestepper { get; private set; }
 
+    /// <summary>
+    /// 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.
+    /// </summary>
+    public bool Deterministic { get; set; }
 
-        private static int ValidateAndCountShapefulBodies(ref BodySet bodySet, ref Tree tree, ref Buffer<CollidableReference> leaves)
+    /// <summary>
+    /// Constructs a simulation supporting dynamic movement and constraints with the specified narrow phase callbacks.
+    /// </summary>
+    /// <param name="bufferPool">Buffer pool used to fill persistent structures and main thread ephemeral resources across the engine.</param>
+    /// <param name="narrowPhaseCallbacks">Callbacks to use in the narrow phase.</param>
+    /// <param name="poseIntegratorCallbacks">Callbacks to use in the pose integrator.</param>
+    /// <param name="timestepper">Timestepper that defines how the simulation state should be updated. If null, <see cref="DefaultTimestepper"/> is used.</param>
+    /// <param name="solveDescription">Describes how the solver should execute, including the number of substeps and the number of velocity iterations per substep.</param>
+    /// <param name="initialAllocationSizes">Allocation sizes to initialize the simulation with. If left null, default values are chosen.</param>
+    /// <param name="shapes">Collection of shapes to use in the simulation, if any. If null, a new <see cref="BepuPhysics.Collidables.Shapes"/> collection will be created for this simulation.</param>
+    /// <returns>New simulation.</returns>
+    public static Simulation Create<TNarrowPhaseCallbacks, TPoseIntegratorCallbacks>(
+        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
+    {
+        if (initialAllocationSizes == null)
         {
-            int shapefulBodyCount = 0;
-            for (int i = 0; i < bodySet.Count; ++i)
+            initialAllocationSizes = new SimulationAllocationSizes
             {
-                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;
-                }
-            }
-            return shapefulBodyCount;
+                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<TPoseIntegratorCallbacks>(simulation.Bodies, simulation.Shapes, simulation.BroadPhase, poseIntegratorCallbacks);
+        simulation.PoseIntegrator = poseIntegrator;
+
+        simulation.Solver = new Solver<TPoseIntegratorCallbacks>(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<TNarrowPhaseCallbacks>(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<TNarrowPhaseCallbacks>(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;
+    }
 
-        [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;
 
-            for (int setIndex = 1; setIndex < Bodies.Sets.Length; ++setIndex)
-            {
-                ref var set = ref Bodies.Sets[setIndex];
-                if (set.Allocated)
-                {
-                    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)
+    private static int ValidateAndCountShapefulBodies(ref BodySet bodySet, ref Tree tree, ref Buffer<CollidableReference> leaves)
+    {
+        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 Statics.Collidables[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 < 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;
+    }
 
-            //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)
-                {
-                    Debug.Assert(BroadPhase.staticLeaves[j].Packed != activeLeaf.Packed);
-                }
-            }
+    [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;
 
-        //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.
-        /// <summary>
-        /// Executes the sleep stage, moving candidate
-        /// </summary>
-        /// <param name="threadDispatcher">Thread dispatcher to use for the sleeper execution, if any.</param>
-        public void Sleep(IThreadDispatcher threadDispatcher = null)
+        for (int setIndex = 1; setIndex < Bodies.Sets.Length; ++setIndex)
         {
-            profiler.Start(Sleeper);
-            Sleeper.Update(threadDispatcher, Deterministic);
-            profiler.End(Sleeper);
+            ref var set = ref Bodies.Sets[setIndex];
+            if (set.Allocated)
+            {
+                inactiveShapefulBodyCount += ValidateAndCountShapefulBodies(ref set, ref BroadPhase.StaticTree, ref BroadPhase.StaticLeaves);
+            }
         }
-
-        /// <summary>
-        /// Updates the position, velocity, world inertia, deactivation candidacy and bounding boxes of active bodies.
-        /// </summary>
-        /// <param name="dt">Duration of the time step.</param>
-        /// <param name="threadDispatcher">Thread dispatcher to use for execution, if any.</param>
-        public void IntegrateBodiesAndUpdateBoundingBoxes(float dt, IThreadDispatcher threadDispatcher = null)
+        Debug.Assert(inactiveShapefulBodyCount + Statics.Count == BroadPhase.StaticTree.LeafCount);
+        for (int i = 0; i < Statics.Count; ++i)
         {
-            profiler.Start(PoseIntegrator);
-            PoseIntegrator.IntegrateBodiesAndUpdateBoundingBoxes(dt, BufferPool, threadDispatcher);
-            profiler.End(PoseIntegrator);
-        }
+            ref var collidable = ref Statics[i];
+            Debug.Assert(collidable.Shape.Exists, "All static collidables must have shapes. That's their only purpose.");
 
-        /// <summary>
-        /// Predicts the bounding boxes of active bodies by speculatively integrating velocity. Does not actually modify body velocities. Updates deactivation candidacy.
-        /// </summary>
-        /// <param name="dt">Duration of the time step.</param>
-        /// <param name="threadDispatcher">Thread dispatcher to use for execution, if any.</param>
-        public void PredictBoundingBoxes(float dt, IThreadDispatcher threadDispatcher = null)
-        {
-            profiler.Start(PoseIntegrator);
-            PoseIntegrator.PredictBoundingBoxes(dt, BufferPool, threadDispatcher);
-            profiler.End(PoseIntegrator);
+            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);
         }
 
-        /// <summary>
-        /// Updates the velocities, world space inertias, bounding boxes, and deactivation candidacy of active bodies.
-        /// </summary>
-        /// <param name="dt">Duration of the time step.</param>
-        /// <param name="threadDispatcher">Thread dispatcher to use for execution, if any.</param>
-        public void IntegrateVelocitiesBoundsAndInertias(float dt, IThreadDispatcher threadDispatcher = null)
+        //Ensure there are no duplicates between the two broad phase trees.
+        for (int i = 0; i < BroadPhase.ActiveTree.LeafCount; ++i)
         {
-            profiler.Start(PoseIntegrator);
-            PoseIntegrator.IntegrateVelocitiesBoundsAndInertias(dt, BufferPool, threadDispatcher);
-            profiler.End(PoseIntegrator);
+            var activeLeaf = BroadPhase.ActiveLeaves[i];
+            for (int j = 0; j < BroadPhase.StaticTree.LeafCount; ++j)
+            {
+                Debug.Assert(BroadPhase.StaticLeaves[j].Packed != activeLeaf.Packed);
+            }
         }
 
-        /// <summary>
-        /// Updates the velocities and world space inertias of active bodies.
-        /// </summary>
-        /// <param name="dt">Duration of the time step.</param>
-        /// <param name="threadDispatcher">Thread dispatcher to use for execution, if any.</param>
-        public void IntegrateVelocitiesAndUpdateInertias(float dt, IThreadDispatcher threadDispatcher = null)
-        {
-            profiler.Start(PoseIntegrator);
-            PoseIntegrator.IntegrateVelocitiesAndUpdateInertias(dt, BufferPool, threadDispatcher);
-            profiler.End(PoseIntegrator);
-        }
+    }
 
-        /// <summary>
-        /// Updates the poses of active bodies.
-        /// </summary>
-        /// <param name="dt">Duration of the time step.</param>
-        /// <param name="threadDispatcher">Thread dispatcher to use for execution, if any.</param>
-        public void IntegratePoses(float dt, IThreadDispatcher threadDispatcher = null)
-        {
-            profiler.Start(PoseIntegrator);
-            PoseIntegrator.IntegratePoses(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.
+    /// <summary>
+    /// Executes the sleep stage, moving candidate
+    /// </summary>
+    /// <param name="threadDispatcher">Thread dispatcher to use for the sleeper execution, if any.</param>
+    public void Sleep(IThreadDispatcher threadDispatcher = null)
+    {
+        profiler.Start(Sleeper);
+        Sleeper.Update(threadDispatcher, Deterministic);
+        profiler.End(Sleeper);
+    }
 
-        /// <summary>
-        /// 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.
-        /// </summary>
-        /// <param name="dt">Duration of the time step.</param>
-        /// <param name="threadDispatcher">Thread dispatcher to use for execution, if any.</param>
-        public void CollisionDetection(float dt, IThreadDispatcher threadDispatcher = null)
-        {
-            profiler.Start(BroadPhase);
-            BroadPhase.Update(threadDispatcher);
-            profiler.End(BroadPhase);
+    /// <summary>
+    /// Predicts the bounding boxes of active bodies by speculatively integrating velocity. Does not actually modify body velocities. Updates deactivation candidacy.
+    /// </summary>
+    /// <param name="dt">Duration of the time step.</param>
+    /// <param name="threadDispatcher">Thread dispatcher to use for execution, if any.</param>
+    public void PredictBoundingBoxes(float dt, IThreadDispatcher threadDispatcher = null)
+    {
+        profiler.Start(PoseIntegrator);
+        PoseIntegrator.PredictBoundingBoxes(dt, BufferPool, threadDispatcher);
+        profiler.End(PoseIntegrator);
+    }
 
-            profiler.Start(BroadPhaseOverlapFinder);
-            BroadPhaseOverlapFinder.DispatchOverlaps(dt, threadDispatcher);
-            profiler.End(BroadPhaseOverlapFinder);
+    /// <summary>
+    /// 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.
+    /// </summary>
+    /// <param name="dt">Duration of the time step.</param>
+    /// <param name="threadDispatcher">Thread dispatcher to use for execution, if any.</param>
+    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(NarrowPhase);
-            NarrowPhase.Flush(threadDispatcher);
-            profiler.End(NarrowPhase);
-        }
+    /// <summary>
+    /// 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.
+    /// </summary>
+    /// <param name="dt">Duration of the time step.</param>
+    /// <param name="threadDispatcher">Thread dispatcher to use for execution, if any.</param>
+    public void Solve(float dt, IThreadDispatcher threadDispatcher = null)
+    {
+        Profiler.Start(Solver);
+        var constrainedBodySet = Solver.PrepareConstraintIntegrationResponsibilities(threadDispatcher);
+        Solver.Solve(dt, threadDispatcher);
+        Profiler.End(Solver);
 
-        /// <summary>
-        /// Uses the current body velocities to incrementally update all active contact constraint penetration depths.
-        /// </summary>
-        /// <param name="dt">Duration of the time step.</param>
-        /// <param name="threadDispatcher">Thread dispatcher to use for execution, if any.</param>
-        public void IncrementallyUpdateContactConstraints(float dt, IThreadDispatcher threadDispatcher = null)
-        {
-            profiler.Start(Solver);
-            Solver.IncrementallyUpdateContactConstraints(dt, threadDispatcher);
-            profiler.End(Solver);
-        }
+        Profiler.Start(PoseIntegrator);
+        PoseIntegrator.IntegrateAfterSubstepping(constrainedBodySet, dt, Solver.SubstepCount, threadDispatcher);
+        Profiler.End(PoseIntegrator);
 
-        /// <summary>
-        /// Solves all active constraints in the simulation.
-        /// </summary>
-        /// <param name="dt">Duration of the time step.</param>
-        /// <param name="threadDispatcher">Thread dispatcher to use for execution, if any.</param>
-        public void Solve(float dt, IThreadDispatcher threadDispatcher = null)
-        {
-            profiler.Start(Solver);
-            Solver.Solve(dt, threadDispatcher);
-            profiler.End(Solver);
-        }
+        Solver.DisposeConstraintIntegrationResponsibilities();
+    }
 
-        /// <summary>
-        /// Incrementally improves body and constraint storage for better performance.
-        /// </summary>
-        /// <param name="threadDispatcher">Thread dispatcher to use for execution, if any.</param>
-        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);
-
-            profiler.Start(SolverBatchCompressor);
-            SolverBatchCompressor.Compress(BufferPool, threadDispatcher, threadDispatcher != null && Deterministic);
-            profiler.End(SolverBatchCompressor);
-        }
+    /// <summary>
+    /// Incrementally improves body and constraint storage for better performance.
+    /// </summary>
+    /// <param name="threadDispatcher">Thread dispatcher to use for execution, if any.</param>
+    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);
+    }
 
-        //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.
-        /// <summary>
-        /// Performs one timestep of the given length.
-        /// </summary>
-        /// <remarks>
-        /// 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.
-        /// </remarks>
-        /// <param name="dt">Duration of the time step.</param>
-        /// <param name="threadDispatcher">Thread dispatcher to use for execution, if any.</param>
-        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);
+    //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.
+    /// <summary>
+    /// Performs one timestep of the given length.
+    /// </summary>
+    /// <remarks>
+    /// 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.
+    /// </remarks>
+    /// <param name="dt">Duration of the time step.</param>
+    /// <param name="threadDispatcher">Thread dispatcher to use for execution, if any.</param>
+    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);
 
-            Timestepper.Timestep(this, dt, threadDispatcher);
+        Timestepper.Timestep(this, dt, threadDispatcher);
 
-            profiler.End(this);
-        }
+        profiler.End(this);
+    }
 
-        /// <summary>
-        /// 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.
-        /// </summary>
-        public void Clear()
-        {
-            Solver.Clear();
-            Bodies.Clear();
-            Statics.Clear();
-            Shapes.Clear();
-            BroadPhase.Clear();
-            NarrowPhase.Clear();
-            Sleeper.Clear();
-        }
+    /// <summary>
+    /// 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.
+    /// </summary>
+    public void Clear()
+    {
+        Solver.Clear();
+        Bodies.Clear();
+        Statics.Clear();
+        Shapes.Clear();
+        BroadPhase.Clear();
+        NarrowPhase.Clear();
+        Sleeper.Clear();
+    }
 
-        /// <summary>
-        /// Increases the allocation size of any buffers too small to hold the allocation target.
-        /// </summary>
-        /// <remarks>
-        /// <para>
-        /// 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.
-        /// </para>
-        /// <para>
-        /// 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.
-        /// </para>
-        /// </remarks>
-        /// <param name="allocationTarget">Allocation sizes to guarantee sufficient size for.</param>
-        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);
-        }
+    /// <summary>
+    /// Increases the allocation size of any buffers too small to hold the allocation target.
+    /// </summary>
+    /// <remarks>
+    /// <para>
+    /// 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.
+    /// </para>
+    /// <para>
+    /// 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.
+    /// </para>
+    /// </remarks>
+    /// <param name="allocationTarget">Allocation sizes to guarantee sufficient size for.</param>
+    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);
+    }
 
 
-        /// <summary>
-        /// 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.
-        /// </summary>
-        /// <remarks>
-        /// <para>
-        /// 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.
-        /// </para>
-        /// <para>
-        /// 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.
-        /// </para>
-        /// </remarks>
-        /// <param name="allocationTarget">Allocation sizes to guarantee sufficient size for.</param>
-        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);
-        }
+    /// <summary>
+    /// 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.
+    /// </summary>
+    /// <remarks>
+    /// <para>
+    /// 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.
+    /// </para>
+    /// <para>
+    /// 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.
+    /// </para>
+    /// </remarks>
+    /// <param name="allocationTarget">Allocation sizes to guarantee sufficient size for.</param>
+    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);
+    }
 
-        /// <summary>
-        /// Clears the simulation of every object and returns all pooled memory to the buffer pool. Leaves the simulation in an unusable state.
-        /// </summary>
-        public void Dispose()
-        {
-            Clear();
-            Sleeper.Dispose();
-            Solver.Dispose();
-            BroadPhase.Dispose();
-            NarrowPhase.Dispose();
-            Bodies.Dispose();
-            Statics.Dispose();
-            Shapes.Dispose();
-        }
+    /// <summary>
+    /// Clears the simulation of every object and returns all pooled memory to the buffer pool. Leaves the simulation in an unusable state.
+    /// </summary>
+    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 add852625..44536eef2 100644
--- a/BepuPhysics/SimulationAllocationSizes.cs
+++ b/BepuPhysics/SimulationAllocationSizes.cs
@@ -1,42 +1,73 @@
-namespace BepuPhysics
-{
-    /// <summary>
-    /// The common set of allocation sizes for a simulation.
-    /// </summary>
-    public struct SimulationAllocationSizes
-    {
-        /// <summary>
-        /// The number of bodies to allocate space for.
-        /// </summary>
-        public int Bodies;
-        /// <summary>
-        /// The number of statics to allocate space for.
-        /// </summary>
-        public int Statics;
-        /// <summary>
-        /// The number of inactive islands to allocate space for.
-        /// </summary>
-        public int Islands;
-        /// <summary>
-        /// Minimum number of shapes to allocate space for in each shape type batch.
-        /// </summary>
-        public int ShapesPerType;
-        /// <summary>
-        /// 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.
-        /// </summary>
-        public int Constraints;
-        /// <summary>
-        /// 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.
-        /// </summary>
-        public int ConstraintsPerTypeBatch;
-        /// <summary>
-        /// 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.
-        /// </summary>
-        public int ConstraintCountPerBodyEstimate;
-
-    }
-}
+using BepuPhysics.Collidables;
+using System.Runtime.InteropServices;
+
+namespace BepuPhysics
+{
+    /// <summary>
+    /// The common set of allocation sizes for a simulation.
+    /// </summary>
+    [StructLayout(LayoutKind.Sequential)]
+    public struct SimulationAllocationSizes
+    {
+        /// <summary>
+        /// The number of bodies to allocate space for.
+        /// </summary>
+        public int Bodies;
+        /// <summary>
+        /// The number of statics to allocate space for.
+        /// </summary>
+        public int Statics;
+        /// <summary>
+        /// The number of inactive islands to allocate space for.
+        /// </summary>
+        public int Islands;
+        /// <summary>
+        /// Minimum number of shapes to allocate space for in each shape type batch.
+        /// </summary>
+        /// <remarks>
+        /// Unused if a <see cref="Shapes"/> instance was directly provided to the <see cref="Simulation"/> constructor.
+        /// </remarks>
+        public int ShapesPerType;
+        /// <summary>
+        /// 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.
+        /// </summary>
+        public int Constraints;
+        /// <summary>
+        /// 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.
+        /// </summary>
+        public int ConstraintsPerTypeBatch;
+        /// <summary>
+        /// 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.
+        /// </summary>
+        public int ConstraintCountPerBodyEstimate;
+
+        /// <summary>
+        /// Constructs a description of simulation allocations.
+        /// </summary>
+        /// <param name="bodies">The number of bodies to allocate space for.</param>
+        /// <param name="statics">The number of statics to allocate space for.</param>
+        /// <param name="islands">The number of inactive islands to allocate space for.</param>
+        /// <param name="shapesPerType">Minimum number of shapes to allocate space for in each shape type batch. Unused if a <see cref="Shapes"/> instance was directly provided to the <see cref="Simulation"/> constructor.</param>
+        /// <param name="constraints">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.</param>
+        /// <param name="constraintsPerTypeBatch">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.</param>
+        /// <param name="constraintCountPerBodyEstimate">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.</param>
+        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/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 e9d82419c..d2d39cdf2 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
 {
@@ -15,7 +13,8 @@ public interface IShapeRayHitHandler
         /// <summary>
         /// 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.
         /// </summary>
-        /// <param name="childIndex">Index of the candidate in the parent collidable.</param>
+        /// <param name="childIndex">Index of the candidate in the parent collidable.
+        /// <para>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.</para></param>
         /// <returns>True if the child should be tested by the ray, false otherwise.</returns>
         bool AllowTest(int childIndex);
         /// <summary>
@@ -25,8 +24,9 @@ public interface IShapeRayHitHandler
         /// <param name="maximumT">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.</param>
         /// <param name="t">Distance along the ray to the impact in units of ray direction length. In other words, hitLocation = ray.Origin + ray.Direction * t.</param>
         /// <param name="normal">Surface normal at the hit location.</param>
-        /// <param name="childIndex">Index of the hit child. For convex shapes or other types that don't have multiple children, this is always zero.</param>
-        void OnRayHit(in RayData ray, ref float maximumT, float t, in Vector3 normal, int childIndex);
+        /// <param name="childIndex">Index of the hit child. 
+        /// <para>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.</para></param>
+        void OnRayHit(in RayData ray, ref float maximumT, float t, Vector3 normal, int childIndex);
     }
 
     /// <summary>
@@ -44,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.
         /// </summary>
         /// <param name="collidable">Parent of the candidate.</param>
-        /// <param name="childIndex">Index of the candidate in the parent collidable.</param>
+        /// <param name="childIndex">Index of the candidate child in its parent collidable.
+        /// <para>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.</para></param>
         /// <returns>True if the child should be tested by the ray, false otherwise.</returns>
         bool AllowTest(CollidableReference collidable, int childIndex);
         /// <summary>
@@ -55,8 +56,9 @@ public interface IRayHitHandler
         /// <param name="t">Distance along the ray to the impact in units of ray direction length. In other words, hitLocation = ray.Origin + ray.Direction * t.</param>
         /// <param name="normal">Surface normal at the hit location.</param>
         /// <param name="collidable">Collidable hit by the ray.</param>
-        /// <param name="childIndex">Index of the hit child. For convex shapes or other types that don't have multiple children, this is always zero.</param>
-        void OnRayHit(in RayData ray, ref float maximumT, float t, in Vector3 normal, CollidableReference collidable, int childIndex);
+        /// <param name="childIndex">Index of the hit child in its parent collidable.
+        /// <para>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.</para></param>
+        void OnRayHit(in RayData ray, ref float maximumT, float t, Vector3 normal, CollidableReference collidable, int childIndex);
     }
 
     /// <summary>
@@ -74,9 +76,10 @@ public interface ISweepHitHandler
         /// Checks whether to run a detailed sweep test against a target collidable's child.
         /// </summary>
         /// <param name="collidable">Collidable to check.</param>
-        /// <param name="child">Index of the child in the collidable to check.</param>
+        /// <param name="childIndex">Index of the child in the collidable to check.
+        /// <para>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.</para></param>
         /// <returns>True if the sweep test should be attempted, false otherwise.</returns>
-        bool AllowTest(CollidableReference collidable, int child);
+        bool AllowTest(CollidableReference collidable, int childIndex);
         /// <summary>
         /// Called when a sweep test detects a hit with nonzero T value.
         /// </summary>
@@ -85,7 +88,7 @@ public interface ISweepHitHandler
         /// <param name="hitLocation">Location of the first hit detected by the sweep.</param>
         /// <param name="hitNormal">Surface normal at the hit location.</param>
         /// <param name="collidable">Collidable hit by the traversal.</param>
-        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);
         /// <summary>
         /// Called when a sweep test detects a hit at T = 0, meaning that no location or normal can be computed.
         /// </summary>
@@ -109,7 +112,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);
             }
@@ -121,14 +124,16 @@ 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
             {
                 ref var location = ref Bodies.HandleToLocation[reference.BodyHandle.Value];
                 ref var set = ref Bodies.Sets[location.SetIndex];
-                pose = set.Poses.Memory + location.Index;
+                pose = &(set.DynamicsState.Memory + location.Index)->Motion.Pose;
                 shape = set.Collidables[location.Index].Shape;
             }
         }
@@ -138,13 +143,13 @@ struct RayHitDispatcher<TRayHitHandler> : IBroadPhaseRayTester where TRayHitHand
             public ShapeRayHitHandler<TRayHitHandler> 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);
                 }
             }
         }
@@ -156,15 +161,16 @@ public unsafe void RayTest(CollidableReference collidable, RayData* rayData, flo
         /// <param name="origin">Origin of the ray to cast.</param>
         /// <param name="direction">Direction of the ray to cast.</param>
         /// <param name="maximumT">Maximum length of the ray traversal in units of the direction's length.</param>
+        /// <param name="pool">Pool used for temporary allocations required by the test, if any.</param>
         /// <param name="hitHandler">callbacks to execute on ray-object intersections.</param>
         /// <param name="id">User specified id of the ray.</param>
-        public unsafe void RayCast<THitHandler>(in Vector3 origin, in Vector3 direction, float maximumT, ref THitHandler hitHandler, int id = 0) where THitHandler : IRayHitHandler
+        public void RayCast<THitHandler>(Vector3 origin, Vector3 direction, float maximumT, BufferPool pool, ref THitHandler hitHandler, int id = 0) where THitHandler : IRayHitHandler
         {
             RayHitDispatcher<THitHandler> 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;
         }
@@ -191,7 +197,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))
                 {
@@ -265,7 +271,7 @@ public unsafe void Sweep<TShape, TSweepHitHandler>(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 = new Vector3(BoundingBoxHelpers.GetAngularBoundsExpansion(velocity.Angular.Length(), maximumT, maximumRadius, maximumAngularExpansion));
             min = min - angularExpansion + pose.Position;
             max = max + angularExpansion + pose.Position;
             var direction = velocity.Linear;
@@ -275,14 +281,14 @@ public unsafe void Sweep<TShape, TSweepHitHandler>(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;
             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;
         }
@@ -299,7 +305,7 @@ public unsafe void Sweep<TShape, TSweepHitHandler>(TShape shape, in RigidPose po
         /// <param name="pool">Pool to allocate any temporary resources in during execution.</param>
         /// <param name="hitHandler">Callbacks executed when a sweep impacts an object in the scene.</param>
         /// <remarks>Simulation objects are treated as stationary during the sweep.</remarks>
-        public unsafe void Sweep<TShape, TSweepHitHandler>(in TShape shape, in RigidPose pose, in BodyVelocity velocity, float maximumT, BufferPool pool, ref TSweepHitHandler hitHandler)
+        public void Sweep<TShape, TSweepHitHandler>(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/SolveDescription.cs b/BepuPhysics/SolveDescription.cs
new file mode 100644
index 000000000..bf6661d99
--- /dev/null
+++ b/BepuPhysics/SolveDescription.cs
@@ -0,0 +1,137 @@
+using BepuUtilities.Memory;
+using System;
+
+namespace BepuPhysics
+{
+    /// <summary>
+    /// Callback executed to determine how many velocity iterations should be used for a given substep.
+    /// </summary>
+    /// <param name="substepIndex">Index of the substep to schedule velocity iterations for.</param>
+    /// <returns>Number of velocity iterations to use for the substep. If nonpositive, <see cref="SolveDescription.VelocityIterationCount"/> will be used for the substep instead.</returns>
+    public delegate int SubstepVelocityIterationScheduler(int substepIndex);
+
+    /// <summary>
+    /// Describes how the solver should schedule substeps and velocity iterations.
+    /// </summary>
+    public struct SolveDescription
+    {
+        /// <summary>
+        /// Number of velocity iterations to use in the solver if there is no <see cref="VelocityIterationScheduler"/> or if it returns a non-positive value for a substep.
+        /// </summary>
+        public int VelocityIterationCount;
+        /// <summary>
+        /// Number of substeps to execute each time the solver runs.
+        /// </summary>
+        public int SubstepCount;
+        /// <summary>
+        /// Number of synchronzed constraint batches to use before using a fallback approach.
+        /// </summary>
+        public int FallbackBatchThreshold;
+        /// <summary>
+        /// 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 <see cref="VelocityIterationCount"/> will be used instead.
+        /// </summary>
+        public SubstepVelocityIterationScheduler VelocityIterationScheduler;
+
+        /// <summary>
+        /// Default number of synchronized constraint batches to use before falling back to an alternative solving method.
+        /// </summary>
+        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.");
+        }
+        /// <summary>
+        /// Creates a solve description.
+        /// </summary>
+        /// <param name="velocityIterationCount">Number of velocity iterations per substep.</param>
+        /// <param name="substepCount">Number of substeps in the solve.</param>
+        /// <param name="fallbackBatchThreshold">Number of synchronzed constraint batches to use before using a fallback approach.</param>
+        public SolveDescription(int velocityIterationCount, int substepCount, int fallbackBatchThreshold = DefaultFallbackBatchThreshold)
+        {
+            SubstepCount = substepCount;
+            VelocityIterationCount = velocityIterationCount;
+            FallbackBatchThreshold = fallbackBatchThreshold;
+            VelocityIterationScheduler = null;
+            ValidateDescription();
+        }
+
+        /// <summary>
+        /// Creates a solve description.
+        /// </summary>
+        /// <param name="substepCount">Number of substeps in the solve.</param>
+        /// <param name="velocityIterationScheduler"></param>
+        /// <param name="fallbackVelocityIterationCount">Number of velocity iterations per substep for any substep that is not given a positive number of velocity iterations by the scheduler.</param>
+        /// <param name="fallbackBatchThreshold">Number of synchronzed constraint batches to use before using a fallback approach.</param>
+        public SolveDescription(int substepCount, SubstepVelocityIterationScheduler velocityIterationScheduler, int fallbackVelocityIterationCount = 1, int fallbackBatchThreshold = DefaultFallbackBatchThreshold)
+        {
+            SubstepCount = substepCount;
+            VelocityIterationCount = fallbackVelocityIterationCount;
+            FallbackBatchThreshold = fallbackBatchThreshold;
+            VelocityIterationScheduler = velocityIterationScheduler;
+            ValidateDescription();
+        }
+
+        /// <summary>
+        /// Creates a solve description.
+        /// </summary>
+        /// <param name="substepVelocityIterations">Number of velocity iterations to use in each substep. Number of substeps will be determined by the length of the span.</param>
+        /// <param name="fallbackVelocityIterationCount">Number of velocity iterations per substep for any substep that is not given a positive number of velocity iterations by the scheduler.</param>
+        /// <param name="fallbackBatchThreshold">Number of synchronzed constraint batches to use before using a fallback approach.</param>
+        public SolveDescription(ReadOnlySpan<int> substepVelocityIterations, int fallbackVelocityIterationCount = 1, int fallbackBatchThreshold = DefaultFallbackBatchThreshold)
+        {
+            SubstepCount = substepVelocityIterations.Length;
+            VelocityIterationCount = fallbackVelocityIterationCount;
+            FallbackBatchThreshold = fallbackBatchThreshold;
+            var copy = substepVelocityIterations.ToArray();
+            VelocityIterationScheduler = substepIndex => copy[substepIndex];
+            ValidateDescription();
+        }
+
+        /// <summary>
+        /// Creates a solve description with the given number of velocity iterations and a single substep, with a fallback threshold of <see cref="DefaultFallbackBatchThreshold"/>.
+        /// </summary>
+        /// <param name="velocityIterationCount">Number of velocity iterations per substep.</param>
+        public static implicit operator SolveDescription(int velocityIterationCount)
+        {
+            return new SolveDescription(velocityIterationCount, 1);
+        }
+        /// <summary>
+        /// Creates a solve description with the given number of substeps and velocity iterations per substep and a fallback threshold of <see cref="DefaultFallbackBatchThreshold"/>.
+        /// </summary>
+        /// <param name="schedule">Number of substeps and iterations per solve.</param>
+        public static implicit operator SolveDescription((int iterationsPerSubstep, int substepCount) schedule)
+        {
+            return new SolveDescription(schedule.substepCount, schedule.iterationsPerSubstep);
+        }
+        /// <summary>
+        /// Creates a solve description with the given number of substeps and velocity iterations per substep and a fallback threshold of <see cref="DefaultFallbackBatchThreshold"/>.
+        /// </summary>
+        /// <param name="substepVelocityIterations">Number of velocity iterations to use in each substep. Number of substeps will be determined by the length of the span.</param>
+        public static implicit operator SolveDescription(ReadOnlySpan<int> substepVelocityIterations)
+        {
+            return new SolveDescription(substepVelocityIterations);
+        }
+        /// <summary>
+        /// Creates a solve description with the given number of substeps and velocity iterations per substep and a fallback threshold of <see cref="DefaultFallbackBatchThreshold"/>.
+        /// </summary>
+        /// <param name="substepVelocityIterations">Number of velocity iterations to use in each substep. Number of substeps will be determined by the length of the span.</param>
+        public static implicit operator SolveDescription(int[] substepVelocityIterations)
+        {
+            return new SolveDescription(substepVelocityIterations);
+        }
+        /// <summary>
+        /// Creates a solve description with the given number of substeps and velocity iterations per substep and a fallback threshold of <see cref="DefaultFallbackBatchThreshold"/>.
+        /// </summary>
+        /// <param name="substepVelocityIterations">Number of velocity iterations to use in each substep. Number of substeps will be determined by the length of the span.</param>
+        public static implicit operator SolveDescription(Buffer<int> substepVelocityIterations)
+        {
+            return new SolveDescription(substepVelocityIterations);
+        }
+    }
+}
diff --git a/BepuPhysics/Solver.cs b/BepuPhysics/Solver.cs
index 0d7d73763..15d8f2993 100644
--- a/BepuPhysics/Solver.cs
+++ b/BepuPhysics/Solver.cs
@@ -7,46 +7,17 @@
 using BepuPhysics.CollisionDetection;
 using BepuUtilities;
 using System.Runtime.InteropServices;
+using System.Numerics;
+using System.Threading;
 
 namespace BepuPhysics
 {
-    public unsafe struct ConstraintReference
-    {
-        internal TypeBatch* typeBatchPointer;
-        public ref TypeBatch TypeBatch
-        {
-            [MethodImpl(MethodImplOptions.AggressiveInlining)]
-            get
-            {
-                return ref *typeBatchPointer;
-            }
-        }
-        public readonly int IndexInTypeBatch;
 
-        [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public ConstraintReference(TypeBatch* typeBatchPointer, int indexInTypeBatch)
-        {
-            this.typeBatchPointer = typeBatchPointer;
-            IndexInTypeBatch = indexInTypeBatch;
-        }
-    }
-
-    public struct ConstraintLocation
+    /// <summary>
+    /// Holds and solves constraints between bodies in a simulation.
+    /// </summary>
+    public abstract partial class Solver
     {
-        //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.)
-        public int SetIndex;
-        public int BatchIndex;
-        public int TypeId;
-        public int IndexInTypeBatch;
-    }
-
-    public partial class Solver
-    {
-
         /// <summary>
         /// 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.
         /// </summary>
@@ -61,6 +32,9 @@ public partial class Solver
         //inactive islands do not store the referenced handles since no new constraints are ever added.
         internal QuickList<IndexSet> batchReferencedHandles;
 
+        /// <summary>
+        /// Set of processors applied to batches of constraints of particular types, indexed by the constraint type id.
+        /// </summary>
         public TypeProcessor[] TypeProcessors;
 
         internal Bodies bodies;
@@ -72,6 +46,9 @@ public partial class Solver
         /// </summary>
         public IdPool HandlePool;
         internal BufferPool pool;
+        /// <summary>
+        /// Mapping from constraint handle (via its internal integer value) to the location of a constraint in memory.
+        /// </summary>
         public Buffer<ConstraintLocation> HandleToConstraint;
 
         /// <summary>
@@ -81,24 +58,52 @@ public partial class Solver
         /// </summary>
         public int FallbackBatchThreshold { get; private set; }
 
+        /// <summary>
+        /// Lock used to add to the constrained kinematic handles from multiple threads, if necessary.
+        /// </summary>
+        internal SpinLock constrainedKinematicLock;
+        /// <summary>
+        /// Set of body handles associated with constrained kinematic bodies. These will be integrated during substepping.
+        /// </summary>
+        public QuickSet<int, PrimitiveComparer<int>> ConstrainedKinematicHandles;
+
+        protected int substepCount;
+        /// <summary>
+        /// Gets or sets the number of substeps the solver will simulate per call to Solve.
+        /// </summary>
+        public int SubstepCount
+        {
+            get { return substepCount; }
+            set
+            {
+                if (value < 1)
+                    throw new ArgumentException("Substep count must be positive.");
+                substepCount = value;
+            }
+        }
 
-        int iterationCount;
+        int velocityIterationCount;
         /// <summary>
-        /// 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.
         /// </summary>
-        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;
             }
         }
 
+        /// <summary>
+        /// 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 <see cref="VelocityIterationCount"/> will be used instead.
+        /// </summary>
+        public SubstepVelocityIterationScheduler VelocityIterationScheduler { get; set; }
+
         int minimumCapacityPerTypeBatch;
         /// <summary>
         /// Gets or sets the minimum amount of space, in constraints, initially allocated in any new type batch.
@@ -113,6 +118,33 @@ public int MinimumCapacityPerTypeBatch
         }
         int[] minimumInitialCapacityPerTypeBatch = new int[0];
 
+        /// <summary>
+        /// Delegate type of solver substep begin/end events.
+        /// </summary>
+        /// <param name="substepIndex">Index of the substep that the event is about.</param>
+        public delegate void SubstepEvent(int substepIndex);
+        /// <summary>
+        /// 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.
+        /// </summary>
+        /// <remarks>Take care when attempting to dispatch multithreaded operations from within this event.
+        /// If using the same <see cref="IThreadDispatcher"/> instance as the solver, the dispatcher implementation must be reentrant. The demos implementation is not.</remarks>
+        public event SubstepEvent SubstepStarted;
+        /// <summary>
+        /// 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.
+        /// </summary>
+        /// <remarks>Take care when attempting to dispatch multithreaded operations from within this event.
+        /// If using the same <see cref="IThreadDispatcher"/> instance as the solver, the dispatcher implementation must be reentrant. The demos implementation is not.</remarks>
+        public event SubstepEvent SubstepEnded;
+
+        protected void OnSubstepStarted(int substepIndex)
+        {
+            SubstepStarted?.Invoke(substepIndex);
+        }
+        protected void OnSubstepEnded(int substepIndex)
+        {
+            SubstepEnded?.Invoke(substepIndex);
+        }
+
         /// <summary>
         /// Sets the minimum capacity initially allocated to a new type batch of the given type.
         /// </summary>
@@ -122,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);
@@ -152,6 +184,43 @@ public void ResetPerTypeInitialCapacities()
             Array.Clear(minimumInitialCapacityPerTypeBatch, 0, minimumInitialCapacityPerTypeBatch.Length);
         }
 
+        /// <summary>
+        /// Counts the number of constraints in a particular type batch.
+        /// </summary>
+        /// <param name="setIndex">Index of the set containing the type batch.</param>
+        /// <param name="batchIndex">Index of the batch containing the type batch.</param>
+        /// <param name="typeBatchIndex">Index of the type batch within the batch.</param>
+        /// <returns>Number of constraints in the type batch.</returns>
+        /// <remarks>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 <see cref="TypeBatch.ConstraintCount"/> value is an upper bound and there may be gaps.</remarks>
+        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;
+            }
+        }
+
         /// <summary>
         /// Gets the total number of constraints across all sets, batches, and types. Requires enumerating
         /// all type batches; this can be expensive.
@@ -164,7 +233,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)
@@ -172,54 +243,75 @@ 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;
         }
 
-
-        Action<int> solveWorker;
-        Action<int> 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.iterationCount = 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<IndexSet>(fallbackBatchThreshold + 1, pool);
+            ActiveSet = new ConstraintSet(pool, FallbackBatchThreshold + 1);
+            batchReferencedHandles = new QuickList<IndexSet>(FallbackBatchThreshold + 1, pool);
             ResizeHandleCapacity(initialCapacity);
-            solveWorker = SolveWorker;
-            incrementalContactUpdateWorker = IncrementalContactUpdateWorker;
+            ConstrainedKinematicHandles = new QuickSet<int, PrimitiveComparer<int>>(bodies.HandleToLocation.Length, pool);
         }
 
+        /// <summary>
+        /// 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.
+        /// </summary>
+        /// <typeparam name="TDescription">Type of the constraint to register with the solver.</typeparam>
+        /// <exception cref="ArgumentException">Fired when another constraint type of the same id has already been registered.</exception>
+        /// <remarks><see cref="DefaultTypes.RegisterDefaults(Solver, NarrowPhase)"/> is called during simuation creation and registers all the built in types. Calling <see cref="Register"/> manually is only necessary if custom types are used.</remarks>
         public void Register<TDescription>() where TDescription : unmanaged, IConstraintDescription<TDescription>
         {
-            var description = default(TDescription);
-            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 = (TypeProcessor)Activator.CreateInstance(description.TypeProcessorType);
-                TypeProcessors[description.ConstraintTypeId] = processor;
-                processor.Initialize(description.ConstraintTypeId);
+                var processor = TDescription.CreateTypeProcessor();
+                TypeProcessors[TDescription.ConstraintTypeId] = processor;
+                processor.Initialize(TDescription.ConstraintTypeId);
             }
-            else if (TypeProcessors[description.ConstraintTypeId].GetType() != description.TypeProcessorType)
+            else
             {
-                throw new ArgumentException(
-                    $"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.");
             }
         }
@@ -234,23 +326,91 @@ public void Register<TDescription>() 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;
         }
 
         /// <summary>
         /// 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.
         /// </summary>
-        /// <typeparam name="T">Type of the type batch being referred to.</typeparam>
         /// <param name="handle">Handle index of the constraint.</param>
-        /// <param name="reference">Temporary direct reference to the type batch and index in the type batch associated with the constraint handle.
-        /// May be invalidated by constraint removals.</param>
+        /// <returns>Temporary direct reference to the type batch and index in the type batch associated with the constraint handle.
+        /// May be invalidated by constraint removals.</returns>
         [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")]
+        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)
+            {
+                ref var set = ref Sets[setIndex];
+                if (set.Allocated)
+                {
+                    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;
+                            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<Vector<int>>()];
+                                for (int bodyIndexInConstraint = 0; bodyIndexInConstraint < bodiesPerConstraint; ++bodyIndexInConstraint)
+                                {
+                                    var referencesForBodyIndexInConstraint = Unsafe.Add(ref Unsafe.As<byte, Vector<int>>(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.DynamicsState[encodedBodyReference & Bodies.BodyReferenceMask].Inertia.Local);
+                                        }
+                                        else
+                                        {
+                                            //Sleeping set references are handles.
+                                            kinematicByInertia = bodies[new BodyHandle { Value = encodedBodyReference & Bodies.BodyReferenceMask }].Kinematic;
+                                        }
+                                        Debug.Assert(kinematicByEncodedReference == kinematicByInertia, "Constraint reference encoded kinematicity must match actual kinematicity by inertia.");
+                                    }
+                                }
+                            }
+                        }
+                    }
+                }
+            }
+        }
+
+        [Conditional("DEBUG")]
+        internal void ValidateConstrainedKinematicsSet()
+        {
+            ref var set = ref bodies.ActiveSet;
+            for (int i = 0; i < set.Count; ++i)
+            {
+                if (Bodies.IsKinematicUnsafeGCHole(ref set.DynamicsState[i].Inertia.Local) && set.Constraints[i].Count > 0)
+                {
+                    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)
+            {
+                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.");
+            }
         }
 
         [Conditional("DEBUG")]
@@ -267,6 +427,292 @@ private void ValidateBodyReference(int body, int expectedCount, ref ConstraintBa
             Debug.Assert(referencesToBody == expectedCount);
         }
 
+        [Conditional("DEBUG")]
+        internal 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<int>.Count - typeBatch.ConstraintCount;
+                    var firstEmptySlotIndex = Vector<int>.Count - expectedEmptyLanesInLastBundle;
+                    ref var lastBodyReferencesBundle = ref typeBatch.BodyReferences[(typeBatch.BundleCount - 1) * bodiesPerConstraint * Unsafe.SizeOf<Vector<int>>()];
+                    for (int bodyIndexInConstraint = 0; bodyIndexInConstraint < bodiesPerConstraint; ++bodyIndexInConstraint)
+                    {
+                        ref var bodyBundleInConstraint = ref Unsafe.Add(ref Unsafe.As<byte, Vector<int>>(ref lastBodyReferencesBundle), bodyIndexInConstraint);
+                        ref var bodiesInBundle = ref Unsafe.As<Vector<int>, int>(ref bodyBundleInConstraint);
+                        for (int i = firstEmptySlotIndex; i < Vector<int>.Count; ++i)
+                        {
+                            Debug.Assert(Unsafe.Add(ref bodiesInBundle, i) == -1, "Any awake incomplete bundle should have its trailing values initialized to -1.");
+                        }
+                    }
+                }
+            }
+        }
+        [Conditional("DEBUG")]
+        internal 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<Vector<int>>() * 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<byte, int>(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")]
+        internal 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.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)
+                {
+                    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;
+                        }
+                    }
+                    var bodyIsInFallbackDynamicsSet = ActiveSet.SequentialFallback.dynamicBodyConstraintCounts.TryGetValue(i, out var countForBody);
+                    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.");
+                    }
+                    else
+                    {
+                        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.");
+                    }
+                }
+            }
+        }
+
+        [Conditional("DEBUG")]
+        internal void ValidateFallbackBatchAccessSafety()
+        {
+            ref var set = ref ActiveSet;
+            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<Vector<int>>() * bodiesPerConstraint;
+                    for (int bundleIndex = 0; bundleIndex < typeBatch.BundleCount; ++bundleIndex)
+                    {
+                        ref var bodyReferenceForFirstBody = ref Unsafe.As<byte, Vector<int>>(ref typeBatch.BodyReferences[bundleIndex * bodyReferencesBundleSize]);
+                        var occupiedLaneMask = Vector.GreaterThanOrEqual(bodyReferenceForFirstBody, Vector<int>.Zero);
+                        var occupiedLaneCountInBundle = 0;
+                        for (int i = 0; i < Vector<int>.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);
+                            for (int innerIndex = 0; innerIndex < Vector<int>.Count; ++innerIndex)
+                            {
+                                var index = bodyReferencesForSource[innerIndex];
+                                if (index >= 0 && Bodies.IsEncodedDynamicReference(index))
+                                {
+                                    var broadcasted = new Vector<int>(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<int>.One);
+                                        matchesTotal += matchesInLane;
+                                    }
+                                    Debug.Assert(matchesTotal == 1, "A dynamic body reference should occur no more than once in any constraint bundle.");
+                                }
+                            }
+                        }
+                    }
+                }
+                //Console.WriteLine($"Average fallback occupancy: {Vector<int>.Count * occupiedLaneCountAcrossBatch / (double)(totalBundleCount * Vector<int>.Count):G3} / {Vector<int>.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);
+                    }
+                }
+            }
+        }
+
+        unsafe struct ValidateAccumulatedImpulsesEnumerator : IForEach<float>
+        {
+            public int Index;
+            public float* AccumulatedImpulses;
+            public void LoopBody(float impulse)
+            {
+                AccumulatedImpulses[Index] = impulse;
+            }
+        }
+
+
+        internal 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<Vector<int>>();
+                    for (int bundleIndex = 0; bundleIndex < typeBatch.BundleCount; ++bundleIndex)
+                    {
+                        var countInBundle = typeBatch.ConstraintCount - bundleIndex * Vector<int>.Count;
+                        if (countInBundle > Vector<int>.Count)
+                            countInBundle = Vector<int>.Count;
+                        ref var bundleStart = ref Unsafe.As<byte, Vector<int>>(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 & Bodies.BodyReferenceMask].Value);
+                                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 ValidateAccumulatedImpulses()
+        {
+            var impulseMemory = stackalloc float[16];
+            var impulsesEnumerator = new ValidateAccumulatedImpulsesEnumerator { Index = 0, AccumulatedImpulses = impulseMemory };
+            for (int i = 0; i < Sets.Length; ++i)
+            {
+                ref var set = ref Sets[i];
+                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];
+                        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")]
+        internal 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 typeBatchIndex = 0; typeBatchIndex < batch.TypeBatches.Count; ++typeBatchIndex)
+                {
+                    var typeBatch = batch.TypeBatches[typeBatchIndex];
+                    Debug.Assert(TypeProcessors[typeBatch.TypeId].BodiesPerConstraint <= maximumBodyCountInConstraint);
+                    for (int constraintIndex = 0; constraintIndex < typeBatch.ConstraintCount; ++constraintIndex)
+                    {
+                        var constraintHandle = typeBatch.IndexToHandle[constraintIndex];
+                        if (constraintHandle.Value >= 0)
+                        {
+                            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]));
+                            }
+                        }
+                    }
+                }
+            }
+        }
+
         [Conditional("DEBUG")]
         internal unsafe void ValidateExistingHandles(bool activeOnly = false)
         {
@@ -283,18 +729,50 @@ 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];
-                    if (handles.Contains(handle.Value))
-                        ValidateBodyReference(i, 1, ref batch);
+                    int expectedCount = 0;
+                    int bodyReference = i;
+                    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];
+                        for (int constraintForBodyIndex = 0; constraintForBodyIndex < constraints.Count; ++constraintForBodyIndex)
+                        {
+                            if (HandleToConstraint[constraints[constraintForBodyIndex].ConnectingConstraintHandle.Value].BatchIndex == batchIndex)
+                                ++expectedCount;
+                        }
+                        bodyReference |= Bodies.KinematicMask;
+                    }
                     else
-                        ValidateBodyReference(i, 0, ref batch);
+                    {
+                        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(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)
@@ -310,7 +788,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)
             {
@@ -341,7 +819,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);
+                                EnumerateConnectedRawBodyReferences(ref typeBatch, indexInTypeBatch, ref enumerator);
 
                                 for (int i = 0; i < processor.BodiesPerConstraint; ++i)
                                 {
@@ -349,11 +827,11 @@ internal unsafe void ValidateExistingHandles(bool activeOnly = false)
                                     int bodyIndex;
                                     if (setIndex == 0)
                                     {
-                                        bodyIndex = constraintBodyReferences[i];
+                                        bodyIndex = constraintBodyReferences[i] & Bodies.BodyReferenceMask;
                                     }
                                     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;
                                     }
@@ -375,10 +853,12 @@ 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);
+                            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];
+                            bodyReferenceInConstraint &= Bodies.BodyReferenceMask;
+                            Debug.Assert(bodyReferenceInConstraint == bodyReference,
                                 "If a body refers to a constraint, the constraint should refer to the body.");
                         }
                     }
@@ -386,9 +866,80 @@ internal unsafe void ValidateExistingHandles(bool activeOnly = false)
             }
         }
 
+        [Conditional("DEBUG")]
+        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 = constraintStart; indexInTypeBatch < end; ++indexInTypeBatch)
+                {
+                    //Fallback batches can have empty slots, marked with a -1 in the handle slot.
+                    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);
+                        Debug.Assert(constraintLocation.IndexInTypeBatch == indexInTypeBatch);
+                        Debug.Assert(constraintLocation.TypeId == typeBatch.TypeId);
+                        Debug.Assert(batch.TypeIndexToTypeBatchIndex[constraintLocation.TypeId] == typeBatchIndex);
+                    }
+                }
+            }
+            else
+            {
+                for (int indexInTypeBatch = constraintStart; indexInTypeBatch < end; ++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);
+                    Debug.Assert(constraintLocation.IndexInTypeBatch == indexInTypeBatch);
+                    Debug.Assert(constraintLocation.TypeId == typeBatch.TypeId);
+                    Debug.Assert(batch.TypeIndexToTypeBatchIndex[constraintLocation.TypeId] == typeBatchIndex);
+                }
+            }
+        }
+        [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")]
+        internal 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")]
         internal 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)
             {
@@ -400,16 +951,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);
                         }
                     }
                 }
@@ -439,40 +981,116 @@ internal void AssertConstraintHandleExists(ConstraintHandle handle)
         /// <returns>Index of the batch that the constraint would fit in.</returns>
         /// <remarks>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.</remarks>
-        internal unsafe int FindCandidateBatch(Span<BodyHandle> bodyHandles)
+        internal int FindCandidateBatch(CollidablePair collidablePair)
         {
             ref var set = ref ActiveSet;
             GetSynchronizedBatchCount(out var synchronizedBatchCount, out var fallbackExists);
-            var bodyHandlesAsIntegers = MemoryMarshal.Cast<BodyHandle, int>(bodyHandles);
-            for (int batchIndex = 0; batchIndex < synchronizedBatchCount; ++batchIndex)
+            var aIsDynamic = collidablePair.A.Mobility == Collidables.CollidableMobility.Dynamic;
+            if (aIsDynamic && collidablePair.B.Mobility == Collidables.CollidableMobility.Dynamic)
+            {
+                //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
             {
-                if (batchReferencedHandles[batchIndex].CanFit(bodyHandlesAsIntegers))
-                    return batchIndex;
+                //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;
         }
 
-        internal unsafe void AllocateInBatch(int targetBatchIndex, ConstraintHandle constraintHandle, Span<BodyHandle> bodyHandles, int typeId, out ConstraintReference reference)
+        internal unsafe void AllocateInBatch(int targetBatchIndex, ConstraintHandle constraintHandle, Span<BodyHandle> dynamicBodyHandles, Span<int> encodedBodyIndices, 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);
-            if (targetBatchIndex < FallbackBatchThreshold)
+            for (int j = 0; j < encodedBodyIndices.Length; ++j)
             {
-                ref var handlesSet = ref batchReferencedHandles[targetBatchIndex];
-                for (int j = 0; j < bodyHandles.Length; ++j)
+                //Include the body in the constrained kinematics set if necessary.
+                var encodedBodyIndex = encodedBodyIndices[j];
+                if (Bodies.IsEncodedKinematicReference(encodedBodyIndex))
                 {
-                    handlesSet.Add(bodyHandles[j].Value, pool);
+                    ConstrainedKinematicHandles.Add(bodies.ActiveSet.IndexToHandle[encodedBodyIndex & Bodies.BodyReferenceMask].Value, pool);
                 }
             }
+            var typeProcessor = TypeProcessors[typeId];
+            Debug.Assert(typeProcessor.BodiesPerConstraint == encodedBodyIndices.Length);
+            var typeBatch = batch.GetOrCreateTypeBatch(typeId, typeProcessor, GetMinimumCapacityForType(typeId), pool);
+            int indexInTypeBatch;
+            if (targetBatchIndex == FallbackBatchThreshold)
+                indexInTypeBatch = typeProcessor.AllocateInTypeBatchForFallback(ref *typeBatch, constraintHandle, encodedBodyIndices, pool);
             else
+                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.
+            //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 < dynamicBodyHandles.Length; ++i)
+            {
+                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)
+            {
+                ActiveSet.SequentialFallback.AllocateForActive(dynamicBodyHandles, bodies, pool);
+            }
+        }
+
+        internal void GetBlockingBodyHandles(Span<BodyHandle> bodyHandles, ref Span<BodyHandle> blockingBodyHandlesAllocation, Span<int> encodedBodyIndices)
+        {
+            //Kinematics do not block allocation in a batch; they are treated as read only by the solver.
+            int blockingCount = 0;
+            var solverStates = bodies.ActiveSet.DynamicsState;
+            for (int i = 0; i < bodyHandles.Length; ++i)
             {
-                Debug.Assert(targetBatchIndex == FallbackBatchThreshold);
-                ActiveSet.Fallback.AllocateForActive(constraintHandle, bodyHandles, bodies, typeId, pool);
+                var location = bodies.HandleToLocation[bodyHandles[i].Value];
+                Debug.Assert(location.SetIndex == 0);
+                if (Bodies.IsKinematicUnsafeGCHole(ref solverStates[location.Index].Inertia.Local))
+                {
+                    encodedBodyIndices[i] = location.Index | Bodies.KinematicMask;
+                }
+                else
+                {
+                    blockingBodyHandlesAllocation[blockingCount++] = bodyHandles[i];
+                    encodedBodyIndices[i] = location.Index;
+                }
             }
+            blockingBodyHandlesAllocation = blockingBodyHandlesAllocation.Slice(0, blockingCount);
         }
 
-        internal unsafe bool TryAllocateInBatch(int typeId, int targetBatchIndex, Span<BodyHandle> bodyHandles, out ConstraintHandle constraintHandle, out ConstraintReference reference)
+        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 bool TryAllocateInBatch(int typeId, int targetBatchIndex, Span<BodyHandle> dynamicBodyHandles, Span<int> encodedBodyIndices, out ConstraintHandle constraintHandle, out ConstraintReference reference)
         {
             ref var set = ref ActiveSet;
             Debug.Assert(targetBatchIndex <= set.Batches.Count,
@@ -480,17 +1098,8 @@ internal unsafe bool TryAllocateInBatch(int typeId, int targetBatchIndex, Span<B
             if (targetBatchIndex == set.Batches.Count)
             {
                 //No batch available. Have to create a new one.
-                if (set.Batches.Count == set.Batches.Span.Length)
-                    set.Batches.Resize(set.Batches.Count + 1, pool);
-                set.Batches.AllocateUnsafely() = new ConstraintBatch(pool, TypeProcessors.Length);
-                if (targetBatchIndex < FallbackBatchThreshold)
-                {
-                    //This batch is not the fallback batch, so create an index set for it.
-                    if (set.Batches.Count == batchReferencedHandles.Span.Length)
-                        batchReferencedHandles.Resize(set.Batches.Count + 1, pool);
-                    batchReferencedHandles.AllocateUnsafely() = new IndexSet(pool, bodies.ActiveSet.Count);
-                }
                 //Note that if there is no constraint batch for the given index, there is no way for the constraint add to be blocked. It's guaranteed success.
+                AllocateNewConstraintBatch();
             }
             else
             {
@@ -498,7 +1107,7 @@ internal unsafe bool TryAllocateInBatch(int typeId, int targetBatchIndex, Span<B
                 if (targetBatchIndex < FallbackBatchThreshold)
                 {
                     //A non-fallback constraint batch already exists here. This may fail.
-                    if (!batchReferencedHandles[targetBatchIndex].CanFit(MemoryMarshal.Cast<BodyHandle, int>(bodyHandles)))
+                    if (!batchReferencedHandles[targetBatchIndex].CanFit(MemoryMarshal.Cast<BodyHandle, int>(dynamicBodyHandles)))
                     {
                         //This batch cannot hold the constraint.
                         constraintHandle = new ConstraintHandle(-1);
@@ -508,7 +1117,7 @@ internal unsafe bool TryAllocateInBatch(int typeId, int targetBatchIndex, Span<B
                 }
             }
             constraintHandle = new ConstraintHandle(HandlePool.Take());
-            AllocateInBatch(targetBatchIndex, constraintHandle, bodyHandles, typeId, out reference);
+            AllocateInBatch(targetBatchIndex, constraintHandle, dynamicBodyHandles, encodedBodyIndices, typeId, out reference);
 
             if (constraintHandle.Value >= HandleToConstraint.Length)
             {
@@ -516,12 +1125,13 @@ internal unsafe bool TryAllocateInBatch(int typeId, int targetBatchIndex, Span<B
                 Debug.Assert(constraintHandle.Value < HandleToConstraint.Length, "Handle indices should never jump by more than 1 slot, so doubling should always be sufficient.");
             }
             ref var constraintLocation = ref HandleToConstraint[constraintHandle.Value];
+#if DEBUG
             //Note that new constraints are always active. It is assumed at this point that all connected bodies have been forced active.
-            for (int i = 0; i < bodyHandles.Length; ++i)
+            for (int i = 0; i < dynamicBodyHandles.Length; ++i)
             {
-                ref var bodyLocation = ref bodies.HandleToLocation[bodyHandles[i].Value];
-                Debug.Assert(bodyLocation.SetIndex == 0, "New constraints should only be created involving already-activated bodies.");
+                Debug.Assert(bodies.HandleToLocation[dynamicBodyHandles[i].Value].SetIndex == 0, "New constraints should only be created involving already-activated bodies.");
             }
+#endif
             constraintLocation.SetIndex = 0;
             constraintLocation.IndexInTypeBatch = reference.IndexInTypeBatch;
             constraintLocation.TypeId = typeId;
@@ -536,20 +1146,11 @@ internal unsafe bool TryAllocateInBatch(int typeId, int targetBatchIndex, Span<B
         /// <typeparam name="TDescription">Type of the description to apply.</typeparam>
         /// <param name="constraintReference">Reference of the constraint being updated.</param>
         /// <param name="description">Description to apply to the slot.</param>
-        [MethodImpl(MethodImplOptions.NoInlining)]
-        public void ApplyDescriptionWithoutWaking<TDescription>(ref ConstraintReference constraintReference, ref TDescription description)
+        public unsafe void ApplyDescriptionWithoutWaking<TDescription>(in ConstraintReference constraintReference, in TDescription description)
             where TDescription : unmanaged, IConstraintDescription<TDescription>
         {
             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);
         }
 
         /// <summary>
@@ -558,22 +1159,11 @@ public void ApplyDescriptionWithoutWaking<TDescription>(ref ConstraintReference
         /// <typeparam name="TDescription">Type of the description to apply.</typeparam>
         /// <param name="constraintHandle">Handle of the constraint being updated.</param>
         /// <param name="description">Description to apply to the slot.</param>
-        public void ApplyDescriptionWithoutWaking<TDescription>(ConstraintHandle constraintHandle, ref TDescription description)
-            where TDescription : unmanaged, IConstraintDescription<TDescription>
-        {
-            GetConstraintReference(constraintHandle, out var constraintReference);
-            ApplyDescriptionWithoutWaking(ref constraintReference, ref description);
-        }
-        /// <summary>
-        /// Applies a description to a constraint slot without waking up the associated island.
-        /// </summary>
-        /// <typeparam name="TDescription">Type of the description to apply.</typeparam>
-        /// <param name="constraintHandle">Handle of the constraint being updated.</param>
-        /// <param name="description">Description to apply to the slot.</param>
-        public void ApplyDescriptionWithoutWaking<TDescription>(ConstraintHandle constraintHandle, TDescription description)
+        public void ApplyDescriptionWithoutWaking<TDescription>(ConstraintHandle constraintHandle, in TDescription description)
             where TDescription : unmanaged, IConstraintDescription<TDescription>
         {
-            ApplyDescriptionWithoutWaking(constraintHandle, ref description);
+            var constraintReference = GetConstraintReference(constraintHandle);
+            ApplyDescriptionWithoutWaking(constraintReference, description);
         }
 
         /// <summary>
@@ -582,33 +1172,25 @@ public void ApplyDescriptionWithoutWaking<TDescription>(ConstraintHandle constra
         /// <typeparam name="TDescription">Type of the description to apply.</typeparam>
         /// <param name="constraintHandle">Handle of the constraint being updated.</param>
         /// <param name="description">Description to apply to the slot.</param>
-        public void ApplyDescription<TDescription>(ConstraintHandle constraintHandle, ref TDescription description)
+        public void ApplyDescription<TDescription>(ConstraintHandle constraintHandle, in TDescription description)
             where TDescription : unmanaged, IConstraintDescription<TDescription>
         {
             awakener.AwakenConstraint(constraintHandle);
-            ApplyDescriptionWithoutWaking(constraintHandle, ref description);
-        }
-        /// <summary>
-        /// Applies a description to a constraint slot, waking up the connected bodies if necessary.
-        /// </summary>
-        /// <typeparam name="TDescription">Type of the description to apply.</typeparam>
-        /// <param name="constraintHandle">Handle of the constraint being updated.</param>
-        /// <param name="description">Description to apply to the slot.</param>
-        public void ApplyDescription<TDescription>(ConstraintHandle constraintHandle, TDescription description)
-            where TDescription : unmanaged, IConstraintDescription<TDescription>
-        {
-            ApplyDescription(constraintHandle, ref description);
+            ApplyDescriptionWithoutWaking(constraintHandle, description);
         }
 
-        void Add<TDescription>(Span<BodyHandle> bodyHandles, ref TDescription description, out ConstraintHandle handle)
+        void Add<TDescription>(Span<BodyHandle> bodyHandles, in TDescription description, out ConstraintHandle handle)
             where TDescription : unmanaged, IConstraintDescription<TDescription>
         {
             ref var set = ref ActiveSet;
+            Span<BodyHandle> blockingBodyHandles = stackalloc BodyHandle[bodyHandles.Length];
+            Span<int> encodedBodyIndices = stackalloc int[bodyHandles.Length];
+            GetBlockingBodyHandles(bodyHandles, ref blockingBodyHandles, encodedBodyIndices);
             for (int i = 0; i <= set.Batches.Count; ++i)
             {
-                if (TryAllocateInBatch(description.ConstraintTypeId, i, bodyHandles, out handle, out var reference))
+                if (TryAllocateInBatch(TDescription.ConstraintTypeId, i, blockingBodyHandles, encodedBodyIndices, out handle, out var reference))
                 {
-                    ApplyDescriptionWithoutWaking(ref reference, ref description);
+                    ApplyDescriptionWithoutWaking(reference, description);
                     return;
                 }
             }
@@ -621,21 +1203,22 @@ void Add<TDescription>(Span<BodyHandle> bodyHandles, ref TDescription descriptio
         /// </summary>
         /// <typeparam name="TDescription">Type of the constraint description to add.</typeparam>
         /// <param name="bodyHandles">Body handles used by the constraint.</param>
+        /// <param name="description">Description of the constraint to add.</param>
         /// <returns>Allocated constraint handle.</returns>
-        public ConstraintHandle Add<TDescription>(Span<BodyHandle> bodyHandles, ref TDescription description)
+        public ConstraintHandle Add<TDescription>(Span<BodyHandle> bodyHandles, in TDescription description)
             where TDescription : unmanaged, IConstraintDescription<TDescription>
         {
-            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)
             {
                 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];
@@ -645,42 +1228,18 @@ public ConstraintHandle Add<TDescription>(Span<BodyHandle> bodyHandles, ref TDes
             return constraintHandle;
         }
 
-        /// <summary>
-        /// Allocates a constraint slot and sets up a constraint with the specified description.
-        /// </summary>
-        /// <typeparam name="TDescription">Type of the constraint description to add.</typeparam>
-        /// <param name="bodyHandles">Body handles referenced by the constraint.</param>
-        /// <returns>Allocated constraint handle.</returns>
-        public ConstraintHandle Add<TDescription>(Span<BodyHandle> bodyHandles, TDescription description)
-            where TDescription : unmanaged, IConstraintDescription<TDescription>
-        {
-            return Add(bodyHandles, ref description);
-        }
-
         /// <summary>
         /// Allocates a one-body constraint slot and sets up a constraint with the specified description.
         /// </summary>
         /// <typeparam name="TDescription">Type of the constraint description to add.</typeparam>
         /// <param name="bodyHandle">Body connected to the constraint.</param>
+        /// <param name="description">Description of the constraint to add.</param>
         /// <returns>Allocated constraint handle.</returns>
-        public unsafe ConstraintHandle Add<TDescription>(BodyHandle bodyHandle, ref TDescription description)
-            where TDescription : unmanaged, IOneBodyConstraintDescription<TDescription>
-        {
-            Span<BodyHandle> bodyHandles = stackalloc BodyHandle[] { bodyHandle };
-            return Add(bodyHandles, ref description);
-        }
-
-        /// <summary>
-        /// Allocates a one-body constraint slot and sets up a constraint with the specified description.
-        /// </summary>
-        /// <typeparam name="TDescription">Type of the constraint description to add.</typeparam>
-        /// <param name="bodyHandle">First body of the constraint.</param>
-        /// <returns>Allocated constraint handle.</returns>
-        public unsafe ConstraintHandle Add<TDescription>(BodyHandle bodyHandle, TDescription description)
+        public unsafe ConstraintHandle Add<TDescription>(BodyHandle bodyHandle, in TDescription description)
             where TDescription : unmanaged, IOneBodyConstraintDescription<TDescription>
         {
             Span<BodyHandle> bodyHandles = stackalloc BodyHandle[] { bodyHandle };
-            return Add(bodyHandles, ref description);
+            return Add(bodyHandles, description);
         }
 
         /// <summary>
@@ -689,25 +1248,13 @@ public unsafe ConstraintHandle Add<TDescription>(BodyHandle bodyHandle, TDescrip
         /// <typeparam name="TDescription">Type of the constraint description to add.</typeparam>
         /// <param name="bodyHandleA">First body of the constraint.</param>
         /// <param name="bodyHandleB">Second body of the constraint.</param>
+        /// <param name="description">Description of the constraint to add.</param>
         /// <returns>Allocated constraint handle.</returns>
-        public unsafe ConstraintHandle Add<TDescription>(BodyHandle bodyHandleA, BodyHandle bodyHandleB, ref TDescription description)
+        public unsafe ConstraintHandle Add<TDescription>(BodyHandle bodyHandleA, BodyHandle bodyHandleB, in TDescription description)
             where TDescription : unmanaged, ITwoBodyConstraintDescription<TDescription>
         {
             Span<BodyHandle> bodyHandles = stackalloc BodyHandle[] { bodyHandleA, bodyHandleB };
-            return Add(bodyHandles, ref description);
-        }
-
-        /// <summary>
-        /// Allocates a two-body constraint slot and sets up a constraint with the specified description.
-        /// </summary>
-        /// <typeparam name="TDescription">Type of the constraint description to add.</typeparam>
-        /// <param name="bodyHandleA">First body of the constraint.</param>
-        /// <param name="bodyHandleB">Second body of the constraint.</param>
-        /// <returns>Allocated constraint handle.</returns>
-        public unsafe ConstraintHandle Add<TDescription>(BodyHandle bodyHandleA, BodyHandle bodyHandleB, TDescription description)
-            where TDescription : unmanaged, ITwoBodyConstraintDescription<TDescription>
-        {
-            return Add(bodyHandleA, bodyHandleB, ref description);
+            return Add(bodyHandles, description);
         }
 
         /// <summary>
@@ -717,26 +1264,13 @@ public unsafe ConstraintHandle Add<TDescription>(BodyHandle bodyHandleA, BodyHan
         /// <param name="bodyHandleA">First body of the constraint.</param>
         /// <param name="bodyHandleB">Second body of the constraint.</param>
         /// <param name="bodyHandleC">Third body of the constraint.</param>
+        /// <param name="description">Description of the constraint to add.</param>
         /// <returns>Allocated constraint handle.</returns>
-        public unsafe ConstraintHandle Add<TDescription>(BodyHandle bodyHandleA, BodyHandle bodyHandleB, BodyHandle bodyHandleC, ref TDescription description)
+        public unsafe ConstraintHandle Add<TDescription>(BodyHandle bodyHandleA, BodyHandle bodyHandleB, BodyHandle bodyHandleC, in TDescription description)
             where TDescription : unmanaged, IThreeBodyConstraintDescription<TDescription>
         {
             Span<BodyHandle> bodyHandles = stackalloc BodyHandle[] { bodyHandleA, bodyHandleB, bodyHandleC };
-            return Add(bodyHandles, ref description);
-        }
-
-        /// <summary>
-        /// Allocates a three-body constraint slot and sets up a constraint with the specified description.
-        /// </summary>
-        /// <typeparam name="TDescription">Type of the constraint description to add.</typeparam>
-        /// <param name="bodyHandleA">First body of the constraint.</param>
-        /// <param name="bodyHandleB">Second body of the constraint.</param>
-        /// <param name="bodyHandleC">Third body of the constraint.</param>
-        /// <returns>Allocated constraint handle.</returns>
-        public unsafe ConstraintHandle Add<TDescription>(BodyHandle bodyHandleA, BodyHandle bodyHandleB, BodyHandle bodyHandleC, TDescription description)
-            where TDescription : unmanaged, IThreeBodyConstraintDescription<TDescription>
-        {
-            return Add(bodyHandleA, bodyHandleB, bodyHandleC, ref description);
+            return Add(bodyHandles, description);
         }
 
         /// <summary>
@@ -747,27 +1281,13 @@ public unsafe ConstraintHandle Add<TDescription>(BodyHandle bodyHandleA, BodyHan
         /// <param name="bodyHandleB">Second body of the constraint.</param>
         /// <param name="bodyHandleC">Third body of the constraint.</param>
         /// <param name="bodyHandleD">Fourth body of the constraint.</param>
+        /// <param name="description">Description of the constraint to add.</param>
         /// <returns>Allocated constraint handle.</returns>
-        public unsafe ConstraintHandle Add<TDescription>(BodyHandle bodyHandleA, BodyHandle bodyHandleB, BodyHandle bodyHandleC, BodyHandle bodyHandleD, ref TDescription description)
+        public unsafe ConstraintHandle Add<TDescription>(BodyHandle bodyHandleA, BodyHandle bodyHandleB, BodyHandle bodyHandleC, BodyHandle bodyHandleD, in TDescription description)
             where TDescription : unmanaged, IFourBodyConstraintDescription<TDescription>
         {
             Span<BodyHandle> bodyHandles = stackalloc BodyHandle[] { bodyHandleA, bodyHandleB, bodyHandleC, bodyHandleD };
-            return Add(bodyHandles, ref description);
-        }
-
-        /// <summary>
-        /// Allocates a four-body constraint slot and sets up a constraint with the specified description.
-        /// </summary>
-        /// <typeparam name="TDescription">Type of the constraint description to add.</typeparam>
-        /// <param name="bodyHandleA">First body of the constraint.</param>
-        /// <param name="bodyHandleB">Second body of the constraint.</param>
-        /// <param name="bodyHandleC">Third body of the constraint.</param>
-        /// <param name="bodyHandleD">Fourth body of the constraint.</param>
-        /// <returns>Allocated constraint handle.</returns>
-        public unsafe ConstraintHandle Add<TDescription>(BodyHandle bodyHandleA, BodyHandle bodyHandleB, BodyHandle bodyHandleC, BodyHandle bodyHandleD, TDescription description)
-            where TDescription : unmanaged, IFourBodyConstraintDescription<TDescription>
-        {
-            return Add(bodyHandleA, bodyHandleB, bodyHandleC, bodyHandleD, ref description);
+            return Add(bodyHandles, description);
         }
 
         //This is split out for use by the multithreaded constraint remover.
@@ -802,16 +1322,9 @@ internal void RemoveBatchIfEmpty(ref ConstraintBatch batch, int batchIndex)
                         if (lastBatch.TypeBatches.Count == 0)
                         {
                             lastBatch.Dispose(pool);
-                            //The fallback batch has no batch referenced handles.
-                            if (lastBatchIndex < FallbackBatchThreshold)
-                            {
-                                batchReferencedHandles[lastBatchIndex].Dispose(pool);
-                                --batchReferencedHandles.Count;
-                            }
+                            batchReferencedHandles[lastBatchIndex].Dispose(pool);
+                            --batchReferencedHandles.Count;
                             --set.Batches.Count;
-                            Debug.Assert(set.Batches.Count == batchReferencedHandles.Count ||
-                                (set.Batches.Count == FallbackBatchThreshold + 1 && batchReferencedHandles.Count == FallbackBatchThreshold),
-                                "All synchronized batches should have a 1:1 mapping with batchReferencedHandles entries, but the fallback batch doesn't have one.");
                         }
                         else
                         {
@@ -825,27 +1338,45 @@ internal void RemoveBatchIfEmpty(ref ConstraintBatch batch, int batchIndex)
         /// <summary>
         /// Removes a constraint from a batch, performing any necessary batch cleanup, but does not return the constraint's handle to the pool.
         /// </summary>
-        /// <param name="constraintHandle">Handle of the constraint being removed.</param>
         /// <param name="batchIndex">Index of the batch to remove from.</param>
         /// <param name="typeId">Type id of the constraint to remove.</param>
         /// <param name="indexInTypeBatch">Index of the constraint to remove within its type batch.</param>
-        internal unsafe void RemoveFromBatch(ConstraintHandle constraintHandle, 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)
             {
-                //If this is the fallback batch, it does not track any referenced handles.
                 //Note that we have to remove from fallback first because it accesses the batch's information.
-                ActiveSet.Fallback.Remove(this, pool, ref batch, constraintHandle, typeId, indexInTypeBatch);
-                batch.Remove(typeId, indexInTypeBatch, this);
+                ActiveSet.SequentialFallback.Remove(this, pool, ref batch, ref batchReferencedHandles[batchIndex], typeId, indexInTypeBatch);
             }
             else
             {
-                batch.RemoveWithHandles(typeId, indexInTypeBatch, batchReferencedHandles.GetPointer(batchIndex), this);
+                batch.RemoveBodyHandlesFromBatchForConstraint(typeId, indexInTypeBatch, batchIndex, this);
             }
+            batch.Remove(typeId, indexInTypeBatch, batchIndex == FallbackBatchThreshold, this);
             RemoveBatchIfEmpty(ref batch, batchIndex);
         }
 
+        /// <summary>
+        /// Enumerates the bodies attached to an active constraint and removes the constraint's handle from all of the connected body constraint reference lists.
+        /// </summary>
+        struct RemoveConstraintReferencesFromBodiesEnumerator : IForEach<int>
+        {
+            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.");
+                }
+            }
+        }
+
         /// <summary>
         /// 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.
@@ -861,18 +1392,24 @@ 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;
-            EnumerateConnectedBodies(handle, ref enumerator);
+            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);
         }
 
+        /// <summary>
+        /// Gets the constraint description associated with a constraint reference.
+        /// </summary>
+        /// <typeparam name="TConstraintDescription">Type of the constraint description to retrieve.</typeparam>
+        /// <param name="constraintReference">Reference to the constraint to retrieve.</param>
+        /// <param name="description">Retrieved description of the constraint.</param>
         public void GetDescription<TConstraintDescription>(ConstraintReference constraintReference, out TConstraintDescription description)
             where TConstraintDescription : unmanaged, IConstraintDescription<TConstraintDescription>
         {
@@ -880,10 +1417,16 @@ public void GetDescription<TConstraintDescription>(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);
         }
 
+        /// <summary>
+        /// Gets the constraint description associated with a constraint handle.
+        /// </summary>
+        /// <typeparam name="TConstraintDescription">Type of the constraint description to retrieve.</typeparam>
+        /// <param name="handle">Handle of the constraint to retrieve.</param>
+        /// <param name="description">Retrieved description of the constraint.</param>
         public void GetDescription<TConstraintDescription>(ConstraintHandle handle, out TConstraintDescription description)
             where TConstraintDescription : unmanaged, IConstraintDescription<TConstraintDescription>
         {
@@ -891,10 +1434,10 @@ public void GetDescription<TConstraintDescription>(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);
         }
 
 
@@ -914,10 +1457,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;
@@ -932,31 +1476,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.Fallback.UpdateForBodyMemoryMove(originalBodyIndex, newBodyLocation);
-            }
-        }
-
-        /// <summary>
-        /// Changes the body references of all constraints associated with two bodies in response to them swapping slots in memory.
-        /// </summary>
-        /// <param name="a">First swapped body index.</param>
-        /// <param name="b">Second swapped body index.</param>
-        internal void UpdateForBodyMemorySwap(int a, int b)
-        {
-            var aInFallback = UpdateConstraintsForBodyMemoryMove(a, b);
-            var bInFallback = UpdateConstraintsForBodyMemoryMove(b, a);
-            if (aInFallback && bInFallback)
-            {
-                ActiveSet.Fallback.UpdateForBodyMemorySwap(a, b);
-            }
-            else if (aInFallback)
-            {
-                ActiveSet.Fallback.UpdateForBodyMemoryMove(a, b);
-            }
-            else if (bInFallback)
-            {
-                ActiveSet.Fallback.UpdateForBodyMemoryMove(b, a);
+                //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);
             }
         }
 
@@ -972,9 +1493,9 @@ internal void UpdateForBodyMemorySwap(int a, int b)
         /// <param name="scale">Scale to apply to accumulated impulses.</param>
         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];
@@ -1047,22 +1568,311 @@ public unsafe float GetAccumulatedImpulseMagnitudeSquared(ConstraintHandle const
         /// </summary>
         /// <param name="constraintHandle">Constraint to look up the accumulated impulses of.</param>
         /// <returns>Magnitude of the accumulated impulses associated with the given constraint.</returns>
-        public unsafe float GetAccumulatedImpulseMagnitude(ConstraintHandle constraintHandle)
+        public float GetAccumulatedImpulseMagnitude(ConstraintHandle constraintHandle)
         {
             return (float)Math.Sqrt(GetAccumulatedImpulseMagnitudeSquared(constraintHandle));
         }
 
+
+        internal void TryRemoveDynamicBodyFromFallback(BodyHandle bodyHandle, int bodyIndex, ref QuickList<int> allocationIdsToFree)
+        {
+            if (ActiveSet.SequentialFallback.TryRemoveDynamicBodyFromTracking(bodyIndex, ref allocationIdsToFree))
+            {
+                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);
+            }
+        }
+
+        private struct DynamicToKinematicEnumerator : IForEach<int>
+        {
+            public int DynamicCount;
+            [MethodImpl(MethodImplOptions.AggressiveInlining)]
+            public void LoopBody(int encodedBodyReference)
+            {
+                ++DynamicCount;
+            }
+        }
+        internal unsafe void UpdateReferencesForBodyBecomingKinematic(BodyHandle bodyHandle, int bodyIndex)
+        {
+            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.
+            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<int>.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<int>.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<int> allocationIdsToFree = new(new Buffer<int>(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<int>
+        {
+            public const int MaximumBodiesPerConstraint = 4;
+
+            public Buffer<BodyHandle> IndexToHandle;
+            public int* DynamicBodyHandles;
+            public int DynamicCount;
+            public int* EncodedBodyIndices;
+            public int EncodedCount;
+
+            [MethodImpl(MethodImplOptions.AggressiveInlining)]
+            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))
+                {
+                    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)
+            {
+                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<int>(dynamicBodyHandles, enumerator.DynamicCount);
+                var encodedBodyIndicesSpan = new Span<int>(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();
+                    }
+                }
+                //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<BodyHandle>(dynamicBodyHandles, enumerator.DynamicCount), encodedBodyIndicesSpan);
+            }
+            if (constraints.Count > 0)
+            {
+                ConstrainedKinematicHandles.FastRemove(bodyHandle.Value);
+            }
+        }
+
+        internal interface IConstraintReferenceReportType { }
+        internal struct ReportEncodedReferences : IConstraintReferenceReportType { }
+        internal struct ReportDecodedReferences : IConstraintReferenceReportType { }
+        internal struct ReportDecodedDynamicReferences : IConstraintReferenceReportType { }
+
+        /// <summary>
+        /// Enumerates body references in the constraint. Reports data according to the TReportType.
+        /// </summary>
+        /// <typeparam name="TEnumerator">Type of the enumerator called for each body index in the constraint.</typeparam>
+        /// <typeparam name="TReportType">Type of information to report to the enumerator.</typeparam>
+        /// <param name="typeBatch">Type batch containing the constraint to enumerate.</param>
+        /// <param name="indexInTypeBatch">Index of the constraint to enumerate in the type batch.</param>
+        /// <param name="enumerator">Enumerator to call for each connected body reference.</param>
+        internal unsafe void EnumerateConnectedBodyReferences<TEnumerator, TReportType>(ref TypeBatch typeBatch, int indexInTypeBatch, ref TEnumerator enumerator) where TEnumerator : IForEach<int> where TReportType : unmanaged, IConstraintReferenceReportType
+        {
+            var bodiesPerConstraint = TypeProcessors[typeBatch.TypeId].BodiesPerConstraint;
+            //Type batches store body references in AOSOA format, with one Vector<int> for each constraint body reference in sequence, tightly packed.
+            //We can extract directly from memory.
+            var bytesPerBundle = bodiesPerConstraint * Unsafe.SizeOf<Vector<int>>();
+            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<Vector<int>>());
+                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 (Bodies.IsEncodedDynamicReference(raw))
+                        enumerator.LoopBody(raw & Bodies.BodyReferenceMask);
+                }
+            }
+        }
+        /// <summary>
+        /// 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.
+        /// </summary>
+        /// <typeparam name="TEnumerator">Type of the enumerator to call on each connected body reference.</typeparam>
+        /// <param name="typeBatch">Type batch containing the constraint to enumerate.</param>
+        /// <param name="indexInTypeBatch">Index of the constraint to enumerate in the type batch.</param>
+        /// <param name="enumerator">Enumerator to call for each connected body reference.</param>
+        public void EnumerateConnectedRawBodyReferences<TEnumerator>(ref TypeBatch typeBatch, int indexInTypeBatch, ref TEnumerator enumerator) where TEnumerator : IForEach<int>
+        {
+            EnumerateConnectedBodyReferences<TEnumerator, ReportEncodedReferences>(ref typeBatch, indexInTypeBatch, ref enumerator);
+        }
+
         /// <summary>
-        /// 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.
+        /// This will report the raw body reference (body index if awake, handle if asleep) and any encoded metadata, like whether the body is kinematic.
         /// </summary>
+        /// <typeparam name="TEnumerator">Type of the enumerator to call on each connected body reference.</typeparam>
         /// <param name="constraintHandle">Constraint to enumerate.</param>
-        /// <param name="enumerator">Enumerator to use.</param>
-        internal void EnumerateConnectedBodies<TEnumerator>(ConstraintHandle constraintHandle, ref TEnumerator enumerator) where TEnumerator : IForEach<int>
+        /// <param name="enumerator">Enumerator to call for each connected body reference.</param>
+        public void EnumerateConnectedRawBodyReferences<TEnumerator>(ConstraintHandle constraintHandle, ref TEnumerator enumerator) where TEnumerator : IForEach<int>
+        {
+            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.");
+            EnumerateConnectedBodyReferences<TEnumerator, ReportEncodedReferences>(ref typeBatch, constraintLocation.IndexInTypeBatch, ref enumerator);
+        }
+
+        /// <summary>
+        /// 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.
+        /// </summary>
+        /// <typeparam name="TEnumerator">Type of the enumerator to call on each connected body reference.</typeparam>
+        /// <param name="typeBatch">Type batch containing the constraint to enumerate.</param>
+        /// <param name="indexInTypeBatch">Index of the constraint to enumerate in the type batch.</param>
+        /// <param name="enumerator">Enumerator to call for each connected body reference.</param>
+        public void EnumerateConnectedBodyReferences<TEnumerator>(ref TypeBatch typeBatch, int indexInTypeBatch, ref TEnumerator enumerator) where TEnumerator : IForEach<int>
+        {
+            EnumerateConnectedBodyReferences<TEnumerator, ReportDecodedReferences>(ref typeBatch, indexInTypeBatch, ref enumerator);
+        }
+
+        /// <summary>
+        /// 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.
+        /// </summary>
+        /// <typeparam name="TEnumerator">Type of the enumerator to call on each connected body reference.</typeparam>
+        /// <param name="constraintHandle">Constraint to enumerate.</param>
+        /// <param name="enumerator">Enumerator to call for each connected body reference.</param>
+        public void EnumerateConnectedBodyReferences<TEnumerator>(ConstraintHandle constraintHandle, ref TEnumerator enumerator) where TEnumerator : IForEach<int>
+        {
+            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.");
+            EnumerateConnectedBodyReferences<TEnumerator, ReportDecodedReferences>(ref typeBatch, constraintLocation.IndexInTypeBatch, ref enumerator);
+        }
+
+        /// <summary>
+        /// 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.
+        /// </summary>
+        /// <typeparam name="TEnumerator">Type of the enumerator to call on each connected dynamic body reference.</typeparam>
+        /// <param name="typeBatch">Type batch containing the constraint to enumerate.</param>
+        /// <param name="indexInTypeBatch">Index of the constraint to enumerate in the type batch.</param>
+        /// <param name="enumerator">Enumerator to call for each connected dynamic body reference.</param>
+        public void EnumerateConnectedDynamicBodies<TEnumerator>(ref TypeBatch typeBatch, int indexInTypeBatch, ref TEnumerator enumerator) where TEnumerator : IForEach<int>
+        {
+            EnumerateConnectedBodyReferences<TEnumerator, ReportDecodedDynamicReferences>(ref typeBatch, indexInTypeBatch, ref enumerator);
+        }
+
+        /// <summary>
+        /// 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.
+        /// </summary>
+        /// <typeparam name="TEnumerator">Type of the enumerator to call on each connected dynamic body reference.</typeparam>
+        /// <param name="constraintHandle">Constraint to enumerate.</param>
+        /// <param name="enumerator">Enumerator to call for each connected dynamic body reference.</param>
+        public void EnumerateConnectedDynamicBodies<TEnumerator>(ConstraintHandle constraintHandle, ref TEnumerator enumerator) where TEnumerator : IForEach<int>
         {
             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].EnumerateConnectedBodyIndices(ref typeBatch, constraintLocation.IndexInTypeBatch, ref enumerator);
+            EnumerateConnectedBodyReferences<TEnumerator, ReportDecodedDynamicReferences>(ref typeBatch, constraintLocation.IndexInTypeBatch, ref enumerator);
         }
 
         internal void GetSynchronizedBatchCount(out int synchronizedBatchCount, out bool fallbackExists)
@@ -1086,12 +1896,11 @@ internal void GetSynchronizedBatchCount(out int synchronizedBatchCount, out bool
         public void Clear()
         {
             ref var activeSet = ref ActiveSet;
-            //Fallback batches don't have any batch referenced handles.
-            GetSynchronizedBatchCount(out var synchronizedBatchCount, out _);
-            for (int batchIndex = 0; batchIndex < synchronizedBatchCount; ++batchIndex)
+            for (int batchIndex = 0; batchIndex < activeSet.Batches.Count; ++batchIndex)
             {
                 batchReferencedHandles[batchIndex].Dispose(pool);
             }
+            ConstrainedKinematicHandles.Clear();
             batchReferencedHandles.Clear();
             ActiveSet.Clear(pool);
             //All inactive sets are returned to the pool.
@@ -1119,12 +1928,12 @@ public void EnsureSolverCapacities(int bodyHandleCapacity, int constraintHandleC
             }
             //Note that we can't shrink below the bodies handle capacity, since the handle distribution could be arbitrary.
             var targetBatchReferencedHandleSize = Math.Max(bodies.HandlePool.HighestPossiblyClaimedId + 1, bodyHandleCapacity);
-            GetSynchronizedBatchCount(out var synchronizedBatchCount, out var fallbackExists);
-            //The fallback batch does not have any referenced handles.
-            for (int i = 0; i < synchronizedBatchCount; ++i)
+            for (int i = 0; i < ActiveSet.Batches.Count; ++i)
             {
                 batchReferencedHandles[i].EnsureCapacity(targetBatchReferencedHandleSize, pool);
             }
+
+            ConstrainedKinematicHandles.EnsureCapacity(bodyHandleCapacity, pool);
         }
 
         void ResizeHandleCapacity(int constraintHandleCapacity)
@@ -1152,12 +1961,13 @@ public void ResizeSolverCapacities(int bodyHandleCapacity, int constraintHandleC
             }
             //Note that we can't shrink below the bodies handle capacity, since the handle distribution could be arbitrary.
             var targetBatchReferencedHandleSize = Math.Max(bodies.HandlePool.HighestPossiblyClaimedId + 1, bodyHandleCapacity);
-            GetSynchronizedBatchCount(out var synchronizedBatchCount, out var fallbackExists);
-            //The fallback batch does not have any referenced handles.
-            for (int i = 0; i < synchronizedBatchCount; ++i)
+            for (int i = 0; i < ActiveSet.Batches.Count; ++i)
             {
                 batchReferencedHandles[i].Resize(targetBatchReferencedHandleSize, pool);
             }
+
+            var targetConstrainedKinematicsCapacity = Math.Max(ConstrainedKinematicHandles.Count, bodyHandleCapacity);
+            ConstrainedKinematicHandles.Resize(targetConstrainedKinematicsCapacity, pool);
         }
 
         internal void ResizeSetsCapacity(int setsCapacity, int potentiallyAllocatedCount)
@@ -1207,13 +2017,12 @@ public void ResizeTypeBatchCapacities()
         /// </remarks>
         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);
             }
             batchReferencedHandles.Dispose(pool);
+            ConstrainedKinematicHandles.Dispose(pool);
             for (int i = 0; i < Sets.Length; ++i)
             {
                 if (Sets[i].Allocated)
@@ -1223,7 +2032,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 3234b9a24..000000000
--- a/BepuPhysics/Solver_IncrementalContactUpdate.cs
+++ /dev/null
@@ -1,84 +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
-    {
-        struct IncrementalContactDataUpdateFilter : ITypeBatchSolveFilter
-        {
-            public bool AllowFallback { get { return false; } }
-
-            [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)
-            {
-                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<IncrementalContactDataUpdateFilter>(dt, threadDispatcher, 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<int> 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<int>().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<int>().Return(ref StageIndices);
-//            context.WorkBlocks.Dispose(bufferPool.SpecializeFor<WorkBlock>());
-//            context.BatchBoundaries.Dispose(bufferPool.SpecializeFor<int>());
-//            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_Solve.cs b/BepuPhysics/Solver_Solve.cs
index f4f10ae7d..1542c9220 100644
--- a/BepuPhysics/Solver_Solve.cs
+++ b/BepuPhysics/Solver_Solve.cs
@@ -3,78 +3,43 @@
 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;
 
 namespace BepuPhysics
 {
     public partial class Solver
     {
+        protected enum SolverStageType
+        {
+            IncrementalUpdate,
+            IntegrateConstrainedKinematics,
+            WarmStart,
+            Solve,
+        }
+
+        protected struct SolverSyncStage
+        {
+            public Buffer<int> Claims;
+            public int BatchIndex;
+            public int WorkBlockStartIndex;
+            public SolverStageType StageType;
+
+            public SolverSyncStage(Buffer<int> claims, int workBlockStartIndex, SolverStageType type, int batchIndex = -1)
+            {
+                Claims = claims;
+                BatchIndex = batchIndex;
+                WorkBlockStartIndex = workBlockStartIndex;
+                StageType = type;
+            }
+        }
 
-        //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.
-
-
-        struct WorkBlock
+        protected struct WorkBlock
         {
             public int BatchIndex;
             public int TypeBatchIndex;
@@ -88,489 +53,396 @@ struct WorkBlock
             public int End;
         }
 
-        struct FallbackScatterWorkBlock
-        {
-            public int Start;
-            public int End;
-        }
-
-        interface ITypeBatchSolveFilter
+        protected struct IntegrationWorkBlock
         {
-            bool AllowFallback { get; }
-            bool AllowType(int typeId);
-        }
-
-        struct MainSolveFilter : ITypeBatchSolveFilter
-        {
-            public bool AllowFallback
-            {
-                [MethodImpl(MethodImplOptions.AggressiveInlining)]
-                get
-                {
-                    return true;
-                }
-            }
-
-            [MethodImpl(MethodImplOptions.AggressiveInlining)]
-            public bool AllowType(int typeId)
-            {
-                return true;
-            }
+            public int StartBundleIndex;
+            public int EndBundleIndex;
         }
 
-        private unsafe void BuildWorkBlocks<TTypeBatchFilter>(BufferPool pool, int minimumBlockSizeInBundles, int targetBlocksPerBatch, ref TTypeBatchFilter typeBatchFilter) where TTypeBatchFilter : ITypeBatchSolveFilter
+        //This is up in Solver, instead of Solver<T>, due to explicit layout.
+        [StructLayout(LayoutKind.Explicit)]
+        protected struct SubstepMultithreadingContext
         {
-            ref var activeSet = ref ActiveSet;
-            context.ConstraintBlocks.Blocks = new QuickList<WorkBlock>(targetBlocksPerBatch * activeSet.Batches.Count, pool);
-            pool.Take(activeSet.Batches.Count, out context.BatchBoundaries);
-            for (int batchIndex = 0; batchIndex < activeSet.Batches.Count; ++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;
-                    var typeBatchMaximumBlockCount = typeBatch.BundleCount / (float)minimumBlockSizeInBundles;
-                    var typeBatchBlockCount = Math.Max(1, (int)Math.Min(typeBatchMaximumBlockCount, 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 context.ConstraintBlocks.Blocks.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);
-                    }
-                }
-                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<FallbackScatterWorkBlock>(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 };
-                }
-            }
-        }
+            [FieldOffset(0)]
+            public Buffer<SolverSyncStage> Stages;
+            [FieldOffset(16)]
+            public Buffer<WorkBlock> IncrementalUpdateBlocks;
+            [FieldOffset(32)]
+            public Buffer<IntegrationWorkBlock> KinematicIntegrationBlocks;
+            [FieldOffset(48)]
+            public Buffer<WorkBlock> ConstraintBlocks;
+            [FieldOffset(64)]
+            public Buffer<int> ConstraintBatchBoundaries;
+            [FieldOffset(80)]
+            public float Dt;
+            [FieldOffset(84)]
+            public float InverseDt;
+            [FieldOffset(88)]
+            public int WorkerCount;
+            [FieldOffset(92)]
+            public int HighestVelocityIterationCount;
+            [FieldOffset(96)]
+            public Buffer<int> VelocityIterationCounts;
 
 
-        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.
             /// <summary>
-            /// Inclusive start of blocks known to be claimed by any worker.
+            /// Monotonically increasing index of executed stages during a frame.
             /// </summary>
-            public int Min;
+            [FieldOffset(256)]
+            public int SyncIndex;
+
             /// <summary>
-            /// Exclusive end of blocks known to be claimed by any worker.
+            /// Counter of work completed for the current stage.
             /// </summary>
-            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;
 
-            }
-        }
-        struct WorkBlocks<T> where T : unmanaged
-        {
-            public QuickList<T> Blocks;
-            public Buffer<int> 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.
-        struct MultithreadingParameters
-        {
-            public float Dt;
-            public WorkBlocks<WorkBlock> ConstraintBlocks;
-            public Buffer<int> BatchBoundaries;
-            public WorkBlocks<FallbackScatterWorkBlock> FallbackBlocks;
-            public int WorkerCompletedCount;
-            public int WorkerCount;
-            public Buffer<FallbackTypeBatchResults> FallbackResults;
+        public abstract IndexSet PrepareConstraintIntegrationResponsibilities(IThreadDispatcher threadDispatcher = null);
+        public abstract void DisposeConstraintIntegrationResponsibilities();
+        public abstract void Solve(float dt, IThreadDispatcher threadDispatcher = null);
+    }
 
-            public Buffer<WorkerBounds> WorkerBoundsA;
-            public Buffer<WorkerBounds> WorkerBoundsB;
 
+    /// <summary>
+    /// Handles integration-aware substepped solving.
+    /// </summary>
+    /// <typeparam name="TIntegrationCallbacks">Type of integration callbacks being used during the substepped solve.</typeparam>
+    public unsafe class Solver<TIntegrationCallbacks> : 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, SolveDescription solveDescription,
+            int initialCapacity,
+            int initialIslandCapacity,
+            int minimumCapacityPerTypeBatch, PoseIntegrator<TIntegrationCallbacks> poseIntegrator)
+            : base(bodies, pool, solveDescription, initialCapacity, initialIslandCapacity, minimumCapacityPerTypeBatch)
+        {
+            PoseIntegrator = poseIntegrator;
+            solveWorker = SolveWorker;
+            constraintIntegrationResponsibilitiesWorker = ConstraintIntegrationResponsibilitiesWorker;
         }
-        MultithreadingParameters context;
 
-
-        [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        void MergeWorkerBounds(ref WorkerBounds bounds, ref Buffer<WorkerBounds> allWorkerBounds, int workerIndex)
+        /// <summary>
+        /// Pose integrator used by the simulation.
+        /// </summary>
+        public PoseIntegrator<TIntegrationCallbacks> 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 IncrementalUpdateForSubstepFilter : ITypeBatchSolveFilter
+        {
+            public TypeProcessor[] TypeProcessors;
+            public bool IncludeFallbackBatchForWorkBlocks { get { return true; } }
+
+            [MethodImpl(MethodImplOptions.AggressiveInlining)]
+            public bool AllowType(int typeId)
             {
-                WorkerBounds.MergeIfTouching(ref bounds, ref allWorkerBounds[i]);
+                return TypeProcessors[typeId].RequiresIncrementalSubstepUpdates;
             }
         }
-        [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        int TraverseForwardUntilBlocked<TStageFunction, TBlock>(ref TStageFunction stageFunction, ref WorkBlocks<TBlock> blocks, int blockIndex, ref WorkerBounds bounds, ref Buffer<WorkerBounds> 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);
-                    //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<TStageFunction, TBlock>(ref TStageFunction stageFunction, ref WorkBlocks<TBlock> blocks, int blockIndex, ref WorkerBounds bounds, ref Buffer<WorkerBounds> allWorkerBounds, int workerIndex,
-            int batchStart, int claimedState, int unclaimedState)
-            where TStageFunction : IStageFunction
-            where TBlock : unmanaged
+        private void WarmStartBlock<TBatchShouldIntegratePoses>(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)
             {
-                if (Interlocked.CompareExchange(ref blocks.Claims[blockIndex], claimedState, unclaimedState) == unclaimedState)
+                Buffer<IndexSet> noFlagsRequired = default;
+                typeProcessor.WarmStart<TIntegrationCallbacks, BatchShouldAlwaysIntegrate, TBatchShouldIntegratePoses>(
+                    ref typeBatch, ref noFlagsRequired, bodies, ref PoseIntegrator.Callbacks,
+                    dt, inverseDt, startBundle, endBundle, workerIndex);
+            }
+            else
+            {
+                if (coarseBatchIntegrationResponsibilities[batchIndex][typeBatchIndex])
                 {
-                    lowestLocallyClaimedIndex = blockIndex;
-                    bounds.Min = blockIndex;
-                    Debug.Assert(blockIndex >= batchStart);
-                    stageFunction.Execute(this, blockIndex);
-                    //Decrement or exit.
-                    if (blockIndex == batchStart)
-                        break;
-                    --blockIndex;
+                    typeProcessor.WarmStart<TIntegrationCallbacks, BatchShouldConditionallyIntegrate, TBatchShouldIntegratePoses>(
+                        ref typeBatch, ref integrationFlags[batchIndex][typeBatchIndex], bodies, ref PoseIntegrator.Callbacks,
+                        dt, inverseDt, startBundle, endBundle, workerIndex);
                 }
                 else
                 {
-                    //Already claimed.
-                    bounds.Min = blockIndex;
-                    break;
+                    typeProcessor.WarmStart<TIntegrationCallbacks, BatchShouldNeverIntegrate, TBatchShouldIntegratePoses>(
+                        ref typeBatch, ref integrationFlags[batchIndex][typeBatchIndex], bodies, ref PoseIntegrator.Callbacks,
+                        dt, inverseDt, startBundle, endBundle, workerIndex);
                 }
             }
-            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
+
+        //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 WarmStartStageFunction : IStageFunction
         {
             public float Dt;
             public float InverseDt;
+            public int SubstepIndex;
+            public Solver<TIntegrationCallbacks> 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.substepContext.ConstraintBlocks[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);
+                if (SubstepIndex == 0)
+                {
+                    this.solver.WarmStartBlock<DisallowPoseIntegration>(workerIndex, block.BatchIndex, block.TypeBatchIndex, block.StartBundle, block.End, ref typeBatch, typeProcessor, Dt, InverseDt);
+                }
                 else
-                    typeProcessor.JacobiPrestep(ref typeBatch, solver.bodies, ref solver.ActiveSet.Fallback, Dt, InverseDt, block.StartBundle, block.End);
-            }
-        }
-        struct WarmStartStageFunction : 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.WarmStart(ref typeBatch, ref solver.bodies.ActiveSet.Velocities, block.StartBundle, block.End);
+                {
+                    this.solver.WarmStartBlock<AllowPoseIntegration>(workerIndex, block.BatchIndex, block.TypeBatchIndex, block.StartBundle, block.End, ref typeBatch, typeProcessor, Dt, InverseDt);
+                }
 
             }
         }
+
         struct SolveStageFunction : IStageFunction
         {
+            public float Dt;
+            public float InverseDt;
+            public Solver<TIntegrationCallbacks> 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.substepContext.ConstraintBlocks[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.Solve(ref typeBatch, solver.bodies, Dt, InverseDt, block.StartBundle, block.End);
             }
         }
 
-        struct WarmStartFallbackStageFunction : IStageFunction
+        struct IncrementalUpdateStageFunction : 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.JacobiWarmStart(ref typeBatch, ref solver.bodies.ActiveSet.Velocities, ref solver.context.FallbackResults[block.TypeBatchIndex], block.StartBundle, block.End);
+            public float Dt;
+            public float InverseDt;
+            public Solver<TIntegrationCallbacks> solver;
 
-            }
-        }
-        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 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.JacobiSolveIteration(ref typeBatch, ref solver.bodies.ActiveSet.Velocities, ref solver.context.FallbackResults[block.TypeBatchIndex], block.StartBundle, block.End);
+                solver.TypeProcessors[typeBatch.TypeId].IncrementallyUpdateForSubstep(ref typeBatch, solver.bodies, Dt, InverseDt, block.StartBundle, block.End);
             }
         }
 
-        struct FallbackScatterStageFunction : IStageFunction
+        struct IntegrateConstrainedKinematicsStageFunction : IStageFunction
         {
-            [MethodImpl(MethodImplOptions.AggressiveInlining)]
-            public void Execute(Solver solver, int blockIndex)
-            {
-                ref var block = ref solver.context.FallbackBlocks.Blocks[blockIndex];
-                solver.ActiveSet.Fallback.ScatterVelocities(solver.bodies, solver, ref solver.context.FallbackResults, block.Start, block.End);
-            }
-        }
-
-
-        //TODO: It's very likely that this spin wait isn't ideal for some newer systems like threadripper.
-        /// <summary>
-        /// Behaves like a framework SpinWait, but never voluntarily relinquishes the timeslice to off-core threads.
-        /// </summary>
-        /// <remarks><para>There are three big reasons for using this over the regular framework SpinWait:</para>
-        /// <para>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.</para>
-        /// <para>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.)</para>
-        /// <para>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.</para>
-        /// <para>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.</para></remarks>
-        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;
+            public float Dt;
+            public float InverseDt;
+            public int SubstepIndex;
+            public Solver<TIntegrationCallbacks> 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);
                 }
-
             }
         }
 
-
-        void InterstageSync(ref int syncStageIndex)
+        void ExecuteWorkerStage<TStageFunction>(ref TStageFunction stageFunction, int workerIndex, int workerStart, int availableBlocksStartIndex, ref Buffer<int> 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)
+            if (workerStart == -1)
             {
-                var spinWait = new LocalSpinWait();
-                while (Volatile.Read(ref context.WorkerCompletedCount) < neededCompletionCount)
-                {
-                    spinWait.SpinOnce();
-                }
+                //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;
             }
-        }
-
-        private void ExecuteStage<TStageFunction, TBlock>(ref TStageFunction stageFunction, ref WorkBlocks<TBlock> blocks,
-            ref Buffer<WorkerBounds> allWorkerBounds, ref Buffer<WorkerBounds> 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)
+            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)
             {
-                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)
+                //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<TStageFunction>(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 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 - warmStartLookback);
+        }
+        [MethodImpl(MethodImplOptions.AggressiveInlining)]
+        int GetPreviousSyncIndexForSolve(int syncIndex, int synchronizedBatchCount)
+        {
+            return Math.Max(0, syncIndex - synchronizedBatchCount);
         }
 
-        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)
             {
@@ -582,266 +454,1033 @@ static int GetUniformlyDistributedStart(int workerIndex, int blockCount, int wor
             return offset + blocksPerWorker * workerIndex + Math.Min(remainder, workerIndex);
         }
 
+        Action<int> solveWorker;
         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);
+            //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<int> 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<int>(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);
-            }
-            var fallbackScatterStage = new FallbackScatterStageFunction();
-            if (fallbackExists)
+                Dt = substepContext.Dt,
+                InverseDt = substepContext.InverseDt,
+                solver = this
+            };
+            var integrateConstrainedKinematicsStage = new IntegrateConstrainedKinematicsStageFunction
             {
-                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;
+                Dt = substepContext.Dt,
+                InverseDt = substepContext.InverseDt,
+                solver = this
+            };
+            var warmstartStage = new WarmStartStageFunction
+            {
+                Dt = substepContext.Dt,
+                InverseDt = substepContext.InverseDt,
+                solver = this
+            };
+            var solveStage = new SolveStageFunction
+            {
+                Dt = substepContext.Dt,
+                InverseDt = substepContext.InverseDt,
+                solver = this
+            };
 
-            var solveStage = new SolveStageFunction();
-            var solveFallbackStage = new SolveFallbackStageFunction();
-            for (int iterationIndex = 0; iterationIndex < iterationCount; ++iterationIndex)
+            var maximumSyncStagesPerSubstep = 2 + synchronizedBatchCount * (1 + substepContext.HighestVelocityIterationCount);
+            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.
+                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);
+                    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.
+                    //(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.
+                    if (substepIndex > 0)
+                    {
+                        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, 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, warmStartLookback), 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<DisallowPoseIntegration>(0, FallbackBatchThreshold, j, 0, typeBatch.BundleCount, ref typeBatch, TypeProcessors[typeBatch.TypeId], substepContext.Dt, substepContext.InverseDt);
+                            }
+                            else
+                            {
+                                WarmStartBlock<AllowPoseIntegration>(0, FallbackBatchThreshold, j, 0, typeBatch.BundleCount, ref typeBatch, TypeProcessors[typeBatch.TypeId], substepContext.Dt, substepContext.InverseDt);
+                            }
+                        }
+                    }
+                    var velocityIterationCountForSubstep = substepContext.VelocityIterationCounts[substepIndex];
+                    for (int iterationIndex = 0; iterationIndex < velocityIterationCountForSubstep; ++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].Solve(ref typeBatch, bodies, substepContext.Dt, substepContext.InverseDt, 0, typeBatch.BundleCount);
+                            }
+                        }
+                    }
+                    OnSubstepEnded(substepIndex);
                 }
-                if (fallbackExists)
+                //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 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
+                    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 >= maximumSyncStagesPerSubstep)
+                        {
+                            syncIndexInSubstep -= maximumSyncStagesPerSubstep;
+                            ++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, maximumSyncStagesPerSubstep), syncIndex, ref substepContext.CompletedWorkBlockCount);
+                            break;
+                        case SolverStageType.IntegrateConstrainedKinematics:
+                            integrateConstrainedKinematicsStage.SubstepIndex = substepIndex;
+                            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, 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);
+                            break;
+                    }
+                    latestCompletedSyncIndex = syncIndex;
+
                 }
-                claimedState ^= 1;
-                unclaimedState ^= 1;
             }
         }
-
-        [Conditional("DEBUG")]
-        void ValidateWorkBlocks<TTypeBatchSolveFilter>(ref TTypeBatchSolveFilter filter) where TTypeBatchSolveFilter : ITypeBatchSolveFilter
+        Buffer<IntegrationWorkBlock> BuildKinematicIntegrationWorkBlocks(int minimumBlockSizeInBundles, int maximumBlockSizeInBundles, int targetBlockCount)
         {
-            ref var activeSet = ref ActiveSet;
-            int[][][] batches = new int[activeSet.Batches.Count][][];
-            for (int i = 0; i < activeSet.Batches.Count; ++i)
+            var bundleCount = BundleIndexing.GetBundleCount(ConstrainedKinematicHandles.Count);
+            if (bundleCount > 0)
             {
-                var typeBatches = batches[i] = new int[activeSet.Batches[i].TypeBatches.Count][];
-                for (int j = 0; j < typeBatches.Length; ++j)
+                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<IntegrationWorkBlock> workBlocks);
+                for (int i = 0; i < blockCount; ++i)
                 {
-                    typeBatches[j] = new int[activeSet.Batches[i].TypeBatches[j].BundleCount];
+                    var bundleCountForBlock = bundlesPerBlock;
+                    if (i < remainder)
+                        ++bundleCountForBlock;
+                    workBlocks[i] = new IntegrationWorkBlock { StartBundleIndex = previousEnd, EndBundleIndex = previousEnd += bundleCountForBlock };
                 }
+                return workBlocks;
+
             }
+            return default;
+        }
 
-            for (int blockIndex = 0; blockIndex < context.ConstraintBlocks.Blocks.Count; ++blockIndex)
+        protected void BuildWorkBlocks<TTypeBatchFilter>(
+            BufferPool pool, int minimumBlockSizeInBundles, int maximumBlockSizeInBundles, int targetBlocksPerBatch, ref TTypeBatchFilter typeBatchFilter,
+            out QuickList<WorkBlock> workBlocks, out Buffer<int> batchBoundaries) where TTypeBatchFilter : ITypeBatchSolveFilter
+        {
+            ref var activeSet = ref ActiveSet;
+            int batchCount;
+            if (typeBatchFilter.IncludeFallbackBatchForWorkBlocks)
             {
-                ref var block = ref context.ConstraintBlocks.Blocks[blockIndex];
-                for (int bundleIndex = block.StartBundle; bundleIndex < block.End; ++bundleIndex)
-                {
-                    ref var visitedCount = ref batches[block.BatchIndex][block.TypeBatchIndex][bundleIndex];
-                    ++visitedCount;
-                    Debug.Assert(visitedCount == 1);
-                }
+                batchCount = activeSet.Batches.Count;
             }
-
-            for (int batchIndex = 0; batchIndex < batches.Length; ++batchIndex)
+            else
+            {
+                GetSynchronizedBatchCount(out batchCount, out _);
+            }
+            workBlocks = new QuickList<WorkBlock>(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;
             }
-
         }
 
-        void ExecuteMultithreaded<TTypeBatchSolveFilter>(float dt, IThreadDispatcher threadDispatcher, Action<int> workDelegate) where TTypeBatchSolveFilter : struct, ITypeBatchSolveFilter
+        int GetVelocityIterationCountForSubstepIndex(int substepIndex)
         {
-            var filter = default(TTypeBatchSolveFilter);
-            var workerCount = context.WorkerCount = threadDispatcher.ThreadCount;
-            context.WorkerCompletedCount = 0;
-            context.Dt = dt;
+            if (VelocityIterationScheduler != null)
+            {
+                var scheduledCount = VelocityIterationScheduler(substepIndex);
+                return scheduledCount < 1 ? VelocityIterationCount : scheduledCount;
+            }
+            return VelocityIterationCount;
+        }
+        //Buffer<Buffer<int>> debugStageWorkBlocksCompleted;
+        protected void ExecuteMultithreaded(float dt, IThreadDispatcher threadDispatcher, Action<int> 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.
             //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 = 4;
+            const int minimumBlockSizeInBundles = 1;
+            const int maximumBlockSizeInBundles = 1024;
 
             var targetBlocksPerBatch = workerCount * targetBlocksPerBatchPerWorker;
-            BuildWorkBlocks(pool, minimumBlockSizeInBundles, 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.
-            context.ConstraintBlocks.CreateClaims(pool);
-            if (filter.AllowFallback && ActiveSet.Batches.Count > FallbackBatchThreshold)
+            var mainFilter = new MainSolveFilter();
+            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 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);
+            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);
+            substepContext.HighestVelocityIterationCount = 0;
+            for (int i = 0; i < substepContext.VelocityIterationCounts.Length; ++i)
             {
-                Debug.Assert(context.FallbackBlocks.Blocks.Count > 0);
-                FallbackBatch.AllocateResults(this, pool, ref ActiveSet.Batches[FallbackBatchThreshold], out context.FallbackResults);
-                context.FallbackBlocks.CreateClaims(pool);
+                substepContext.HighestVelocityIterationCount = Math.Max(substepContext.VelocityIterationCounts[i], substepContext.HighestVelocityIterationCount);
             }
-            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)
+            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<int>(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;
+            int highestJobCountInSolve = 0;
+            for (int batchIndex = 0; batchIndex < stagesPerIteration; ++batchIndex)
             {
-                context.WorkerBoundsA[i] = new WorkerBounds { Min = int.MaxValue, Max = int.MinValue };
+                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;
+                highestJobCountInSolve = Math.Max(highestJobCountInSolve, workBlocksInBatch);
             }
+            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;
+                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;
+                    highestJobCountInSolve = Math.Max(highestJobCountInSolve, 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<int>(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);
-
-            context.ConstraintBlocks.Dispose(pool);
-            if (filter.AllowFallback && ActiveSet.Batches.Count > FallbackBatchThreshold)
             {
-                FallbackBatch.DisposeResults(this, pool, ref ActiveSet.Batches[FallbackBatchThreshold], ref context.FallbackResults);
-                context.FallbackBlocks.Dispose(pool);
+                //workDelegate(0);
+                threadDispatcher.DispatchWorkers(workDelegate, highestJobCountInSolve);
             }
-            pool.Return(ref context.BatchBoundaries);
-            pool.Return(ref context.WorkerBoundsA);
-            pool.Return(ref context.WorkerBoundsB);
+
+            //pool.Take<int>(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<int>(workerCount, out var workerBlocksCompletedSums);
+            //pool.Take<double>(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);
+            pool.Return(ref substepContext.VelocityIterationCounts);
+
+
+
         }
 
+        struct IsFallbackBatch { }
+        struct IsNotFallbackBatch { }
 
-        public void Solve(float dt, IThreadDispatcher threadDispatcher = null)
+        bool ComputeIntegrationResponsibilitiesForConstraintRegion<TFallbackness>(int batchIndex, int typeBatchIndex, int constraintStart, int exclusiveConstraintEnd) where TFallbackness : unmanaged
         {
-            if (threadDispatcher == null)
+            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<int>();
+            var bodiesPerConstraintInTypeBatch = TypeProcessors[typeBatch.TypeId].BodiesPerConstraint;
+            var intsPerBundle = Vector<int>.Count * bodiesPerConstraintInTypeBatch;
+            var bundleStartIndex = constraintStart / Vector<float>.Count;
+            var bundleEndIndex = (exclusiveConstraintEnd + Vector<float>.Count - 1) / Vector<float>.Count;
+            Debug.Assert(bundleStartIndex >= 0 && bundleEndIndex <= typeBatch.BundleCount);
+            ref var activeSet = ref bodies.ActiveSet;
+
+            for (int bundleIndex = bundleStartIndex; bundleIndex < bundleEndIndex; ++bundleIndex)
             {
-                var inverseDt = 1f / dt;
-                ref var activeSet = ref ActiveSet;
-                GetSynchronizedBatchCount(out var synchronizedBatchCount, out var fallbackExists);
-                for (int i = 0; i < synchronizedBatchCount; ++i)
+                int bundleStartIndexInConstraints = bundleIndex * Vector<int>.Count;
+                int countInBundle = Math.Min(Vector<float>.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 batch = ref activeSet.Batches[i];
-                    for (int j = 0; j < batch.TypeBatches.Count; ++j)
+                    ref var integrationFlagsForBodyInConstraint = ref integrationFlagsForTypeBatch[bodyIndexInConstraint];
+                    var bundleStart = bundleBodyReferencesStart + bodyIndexInConstraint * Vector<int>.Count;
+                    for (int bundleInnerIndex = 0; bundleInnerIndex < countInBundle; ++bundleInnerIndex)
                     {
-                        ref var typeBatch = ref batch.TypeBatches[j];
-                        TypeProcessors[typeBatch.TypeId].Prestep(ref typeBatch, bodies, dt, inverseDt, 0, typeBatch.BundleCount);
+                        //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);
+                            }
+                        }
                     }
                 }
-                if (fallbackExists)
+            }
+            //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<IsFallbackBatch>(job.batch, job.typeBatch, job.start, job.end);
+                else
+                    jobAlignedIntegrationResponsibilities[jobIndex] = ComputeIntegrationResponsibilitiesForConstraintRegion<IsNotFallbackBatch>(job.batch, job.typeBatch, job.start, job.end);
+            }
+        }
+
+        int nextConstraintIntegrationResponsibilityJobIndex;
+        QuickList<(int batch, int typeBatch, int start, int end)> integrationResponsibilityPrepassJobs;
+        Buffer<bool> jobAlignedIntegrationResponsibilities;
+        Buffer<IndexSet> bodiesFirstObservedInBatches;
+        Buffer<Buffer<Buffer<IndexSet>>> integrationFlags;
+        /// <summary>
+        /// 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.
+        /// </summary>
+        Buffer<Buffer<bool>> coarseBatchIntegrationResponsibilities;
+        Action<int> constraintIntegrationResponsibilitiesWorker;
+        IndexSet mergedConstrainedBodyHandles;
+
+        public override 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[FallbackBatchThreshold];
-                    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].JacobiPrestep(ref typeBatch, bodies, ref activeSet.Fallback, 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);
+                        }
                     }
                 }
-                //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)
+
+                //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);
+                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;
+                pool.Take<bool>(batchReferencedHandles.Count, out var batchHasAnyIntegrationResponsibilities);
+                for (int batchIndex = 1; batchIndex < bodiesFirstObservedInBatches.Length; ++batchIndex)
                 {
-                    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].WarmStart(ref typeBatch, ref bodies.ActiveSet.Velocities, 0, typeBatch.BundleCount);
+                    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);
+
+                    var scalarLoopStartIndex = 0;
+                    ulong horizontalMerge = 0;
+                    if (Vector512.IsHardwareAccelerated)
+                    {
+                        var bundleCount512 = flagBundleCount / 8;
+                        var horizontal512Merge = Vector512<ulong>.Zero;
+                        for (int bundleIndex256 = 0; bundleIndex256 < bundleCount512; ++bundleIndex256)
+                        {
+                            //These will *almost* always be aligned, but guaranteeing it is not worth the complexity.
+                            var mergeBundle = Vector512.Load((ulong*)((Vector512<ulong>*)mergedConstrainedBodyHandles.Flags.Memory + bundleIndex256));
+                            var batchBundle = Vector512.Load((ulong*)((Vector512<ulong>*)batchHandles.Flags.Memory + bundleIndex256));
+                            Vector512.BitwiseOr(mergeBundle, batchBundle).Store((ulong*)((Vector512<ulong>*)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 = Vector512.AndNot(batchBundle, mergeBundle);
+                            horizontal512Merge = Vector512.BitwiseOr(firstObservedBundle, horizontal512Merge);
+                            firstObservedBundle.Store((ulong*)((Vector512<ulong>*)firstObservedInBatch.Flags.Memory + bundleIndex256));
+                        }
+                        var notEqual = Vector512.Xor(Vector512.Equals(horizontal512Merge, Vector512<ulong>.Zero), Vector512<ulong>.AllBitsSet);
+                        horizontalMerge = Vector512.ExtractMostSignificantBits(notEqual);
+
+                        scalarLoopStartIndex = bundleCount512 * 8;
                     }
+                    else if (Vector256.IsHardwareAccelerated)
+                    {
+                        var bundleCount256 = flagBundleCount / 4;
+                        var horizontal256Merge = Vector256<ulong>.Zero;
+                        for (int bundleIndex256 = 0; bundleIndex256 < bundleCount256; ++bundleIndex256)
+                        {
+                            //These will *almost* always be aligned, but guaranteeing it is not worth the complexity.
+                            var mergeBundle = Vector256.Load((ulong*)((Vector256<ulong>*)mergedConstrainedBodyHandles.Flags.Memory + bundleIndex256));
+                            var batchBundle = Vector256.Load((ulong*)((Vector256<ulong>*)batchHandles.Flags.Memory + bundleIndex256));
+                            Vector256.BitwiseOr(mergeBundle, batchBundle).Store((ulong*)((Vector256<ulong>*)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 = Vector256.AndNot(batchBundle, mergeBundle);
+                            horizontal256Merge = Vector256.BitwiseOr(firstObservedBundle, horizontal256Merge);
+                            firstObservedBundle.Store((ulong*)((Vector256<ulong>*)firstObservedInBatch.Flags.Memory + bundleIndex256));
+                        }
+                        var notEqual = Vector256.Xor(Vector256.Equals(horizontal256Merge, Vector256<ulong>.Zero), Vector256<ulong>.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;
                 }
-                Buffer<FallbackTypeBatchResults> fallbackResults = default;
-                if (fallbackExists)
+
+                //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)
                 {
-                    ref var batch = ref activeSet.Batches[FallbackBatchThreshold];
-                    FallbackBatch.AllocateResults(this, pool, ref batch, out fallbackResults);
-                    for (int j = 0; j < batch.TypeBatches.Count; ++j)
+                    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 typeBatch = ref batch.TypeBatches[j];
-                        TypeProcessors[typeBatch.TypeId].JacobiWarmStart(ref typeBatch, ref bodies.ActiveSet.Velocities, ref fallbackResults[j], 0, typeBatch.BundleCount);
+                        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, 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)
+                        {
+                            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);
                     }
-                    activeSet.Fallback.ScatterVelocities(bodies, this, ref fallbackResults, 0, activeSet.Fallback.BodyCount);
+                    integrationResponsibilityPrepassJobs.Dispose(pool);
                 }
-                for (int iterationIndex = 0; iterationIndex < iterationCount; ++iterationIndex)
+                if (useSingleThreadedPath)
                 {
-                    for (int i = 0; i < synchronizedBatchCount; ++i)
+                    for (int i = 1; i < synchronizedBatchCount; ++i)
                     {
-                        ref var batch = ref activeSet.Batches[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];
-                            TypeProcessors[typeBatch.TypeId].SolveIteration(ref typeBatch, ref bodies.ActiveSet.Velocities, 0, typeBatch.BundleCount);
+                            coarseBatchIntegrationResponsibilities[i][j] = ComputeIntegrationResponsibilitiesForConstraintRegion<IsNotFallbackBatch>(i, j, 0, typeBatch.ConstraintCount);
                         }
                     }
-                    if (fallbackExists)
+                    if (fallbackExists && batchHasAnyIntegrationResponsibilities[FallbackBatchThreshold])
                     {
-                        ref var batch = ref activeSet.Batches[FallbackBatchThreshold];
+                        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);
+                            coarseBatchIntegrationResponsibilities[FallbackBatchThreshold][j] = ComputeIntegrationResponsibilitiesForConstraintRegion<IsFallbackBatch>(FallbackBatchThreshold, j, 0, typeBatch.ConstraintCount);
                         }
-                        activeSet.Fallback.ScatterVelocities(bodies, this, ref fallbackResults, 0, activeSet.Fallback.BodyCount);
                     }
                 }
-                if (fallbackExists)
+                //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)
                 {
-                    FallbackBatch.DisposeResults(this, pool, ref activeSet.Batches[FallbackBatchThreshold], ref fallbackResults);
+                    mergedConstrainedBodyHandles.AddUnsafely(ConstrainedKinematicHandles[i]);
                 }
+                return mergedConstrainedBodyHandles;
             }
             else
             {
-                ExecuteMultithreaded<MainSolveFilter>(dt, threadDispatcher, solveWorker);
+                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;
+                for (int substepIndex = 0; substepIndex < substepCount; ++substepIndex)
+                {
+                    OnSubstepStarted(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];
+                                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);
+                    }
+                    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<DisallowPoseIntegration>(0, i, j, 0, typeBatch.BundleCount, ref typeBatch, TypeProcessors[typeBatch.TypeId], substepDt, inverseDt);
+                            }
+                            else
+                            {
+                                WarmStartBlock<AllowPoseIntegration>(0, i, j, 0, typeBatch.BundleCount, ref typeBatch, TypeProcessors[typeBatch.TypeId], substepDt, inverseDt);
+                            }
+                        }
+                    }
+                    var velocityIterationCount = GetVelocityIterationCountForSubstepIndex(substepIndex);
+                    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].Solve(ref typeBatch, bodies, substepDt, inverseDt, 0, typeBatch.BundleCount);
+                            }
+                        }
+                    }
+                    OnSubstepEnded(substepIndex);
+                }
+            }
+            else
+            {
+                ExecuteMultithreaded(substepDt, threadDispatcher, solveWorker);
+            }
+        }
     }
 }
diff --git a/BepuPhysics/StaticDescription.cs b/BepuPhysics/StaticDescription.cs
index b6499c1f7..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
 {
     /// <summary>
     /// 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.
     /// </summary>
+    [StructLayout(LayoutKind.Sequential)]
     public struct StaticDescription
     {
         /// <summary>
@@ -13,56 +15,66 @@ public struct StaticDescription
         /// </summary>
         public RigidPose Pose;
         /// <summary>
-        /// Collidable properties of the static.
+        /// Shape of the static.
         /// </summary>
-        public CollidableDescription Collidable;
+        public TypedIndex Shape;
+        /// <summary>
+        /// Continuous collision detection settings for the static.
+        /// </summary>
+        public ContinuousDetection Continuity;
 
         /// <summary>
         /// Builds a new static description.
         /// </summary>
-        /// <param name="position">Position of the static.</param>
-        /// <param name="orientation">Orientation of the static.</param>
-        /// <param name="collidable">Collidable description for the static.</param>
-        public StaticDescription(in Vector3 position, in Quaternion orientation, in CollidableDescription collidable)
+        /// <param name="pose">Pose of the static collidable.</param>
+        /// <param name="shape">Shape of the static.</param>
+        /// <param name="continuity">Continuous collision detection settings for the static.</param>
+        public StaticDescription(RigidPose pose, TypedIndex shape, ContinuousDetection continuity)
         {
-            Pose.Position = position;
-            Pose.Orientation = orientation;
-            Collidable = collidable;
+            Pose = pose;
+            Shape = shape;
+            Continuity = continuity;
         }
 
         /// <summary>
-        /// Builds a new static description.
+        /// Builds a new static description with <see cref="ContinuousDetectionMode.Discrete"/> continuity.
         /// </summary>
-        /// <param name="position">Position of the static.</param>
-        /// <param name="collidable">Collidable description for the static.</param>
-        public StaticDescription(in Vector3 position, in CollidableDescription collidable) : this(position, Quaternion.Identity, collidable)
+        /// <param name="pose">Pose of the static collidable.</param>
+        /// <param name="shape">Shape of the static.</param>
+        public StaticDescription(RigidPose pose, TypedIndex shape)
         {
+            Pose = pose;
+            Shape = shape;
+            Continuity = ContinuousDetection.Discrete;
         }
 
         /// <summary>
-        /// Builds a new static description with discrete continuity.
+        /// Builds a new static description.
         /// </summary>
         /// <param name="position">Position of the static.</param>
         /// <param name="orientation">Orientation of the static.</param>
-        /// <param name="shapeIndex">Index of the static's shape in the simulation shapes set.</param>
-        /// <param name="speculativeMargin">Distance beyond the surface of the static to allow speculative contacts to be generated.</param>
-        public StaticDescription(in Vector3 position, in Quaternion orientation, TypedIndex shapeIndex, float speculativeMargin)
+        /// <param name="shape">Shape of the static.</param>
+        /// <param name="continuity">Continuous collision detection settings for the static.</param>
+        public StaticDescription(Vector3 position, Quaternion orientation, TypedIndex shape, ContinuousDetection continuity)
         {
             Pose.Position = position;
             Pose.Orientation = orientation;
-            Collidable.Continuity = new ContinuousDetectionSettings();
-            Collidable.Shape = shapeIndex;
-            Collidable.SpeculativeMargin = speculativeMargin;
+            Shape = shape;
+            Continuity = continuity;
         }
 
         /// <summary>
-        /// Builds a new static description with discrete continuity.
+        /// Builds a new static description with <see cref="ContinuousDetectionMode.Discrete"/> continuity.
         /// </summary>
         /// <param name="position">Position of the static.</param>
-        /// <param name="shapeIndex">Index of the static's shape in the simulation shapes set.</param>
-        /// <param name="speculativeMargin">Distance beyond the surface of the body to allow speculative contacts to be generated.</param>
-        public StaticDescription(in Vector3 position, TypedIndex shapeIndex, float speculativeMargin) : this(position, Quaternion.Identity, shapeIndex, speculativeMargin)
+        /// <param name="orientation">Orientation of the static.</param>
+        /// <param name="shape">Shape of the static.</param>
+        public StaticDescription(Vector3 position, Quaternion orientation, TypedIndex shape)
         {
+            Pose.Position = position;
+            Pose.Orientation = orientation;
+            Shape = shape;
+            Continuity = ContinuousDetection.Discrete;
         }
     }
 
diff --git a/BepuPhysics/StaticReference.cs b/BepuPhysics/StaticReference.cs
index fc2eb5787..c3b013b24 100644
--- a/BepuPhysics/StaticReference.cs
+++ b/BepuPhysics/StaticReference.cs
@@ -49,35 +49,27 @@ public bool Exists
         /// <summary>
         /// Gets a the static's index in the statics collection.
         /// </summary>
-        public int Index
-        {
-            [MethodImpl(MethodImplOptions.AggressiveInlining)]
-            get { return Statics.HandleToIndex[Handle.Value]; }
-        }
+        public int Index => Statics.HandleToIndex[Handle.Value];
+
+        /// <summary>
+        /// Gets a reference to the entirety of the static's memory.
+        /// </summary>
+        public ref Static Static => ref Statics.GetDirectReference(Handle);
 
         /// <summary>
         /// Gets a reference to the static's pose.
         /// </summary>
-        public ref RigidPose Pose
-        {
-            [MethodImpl(MethodImplOptions.AggressiveInlining)]
-            get
-            {
-                return ref Statics.Poses[Statics.HandleToIndex[Handle.Value]];
-            }
-        }
+        public ref RigidPose Pose => ref Statics.GetDirectReference(Handle).Pose;
 
         /// <summary>
-        /// Gets a reference to the static's collidable.
+        /// Gets a reference to the static's collision continuity settings.
         /// </summary>
-        public ref Collidable Collidable
-        {
-            [MethodImpl(MethodImplOptions.AggressiveInlining)]
-            get
-            {
-                return ref Statics.Collidables[Statics.HandleToIndex[Handle.Value]];
-            }
-        }
+        public ref ContinuousDetection Continuity => ref Statics.GetDirectReference(Handle).Continuity;
+
+        /// <summary>
+        /// Gets the shape used by the static. To set the shape, use <see cref="SetShape(TypedIndex)"/> or <see cref="ApplyDescription(in StaticDescription)"/>.
+        /// </summary>
+        public TypedIndex Shape => Statics.GetDirectReference(Handle).Shape;
 
         /// <summary>
         /// <para>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.</para>
@@ -101,6 +93,15 @@ public void GetDescription(out StaticDescription description)
             Statics.GetDescription(Handle, out description);
         }
 
+        /// <summary>
+        /// Gets a description of the static.
+        /// </summary>
+        /// <returns>Description of the static.</returns>
+        public StaticDescription GetDescription()
+        {
+            return Statics.GetDescription(Handle);
+        }
+
         /// <summary>
         /// Sets a static's properties according to a description.
         /// </summary>
@@ -142,7 +143,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 f2c40aa41..2f6289cb3 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;
@@ -46,14 +44,57 @@ public bool ShouldAwaken(BodyReference body)
         }
     }
 
+    /// <summary>
+    /// Awakening filter that prevents any bodies from being awoken by the static's state change.
+    /// </summary>
+    public struct StaticsShouldntAwakenBodies : IStaticChangeAwakeningFilter
+    {
+        public bool AllowAwakening => false;
+        public bool ShouldAwaken(BodyReference body) => false;
+    }
+
+    /// <summary>
+    /// Stores data for a static collidable in the simulation. Statics can be posed and collide, but have no velocity and no dynamic behavior.
+    /// </summary>
+    /// <remarks>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.</remarks>
+    public struct Static
+    {
+        /// <summary>
+        /// Pose of the static collidable.
+        /// </summary>
+        public RigidPose Pose;
+
+        /// <summary>
+        /// Continuous collision detection settings for this collidable. Includes the collision detection mode to use and tuning variables associated with those modes.
+        /// </summary>
+        /// <remarks>Note that statics cannot move, so there is no difference between <see cref="ContinuousDetectionMode.Discrete"/> and <see cref="ContinuousDetectionMode.Passive"/> for them.
+        /// Enabling <see cref="ContinuousDetectionMode.Continuous"/> will still require that pairs associated with the static use swept continuous collision detection.</remarks>
+        public ContinuousDetection Continuity;
+
+        /// <summary>
+        /// 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 <see cref="Statics.SetShape"/> or Statics.ApplyDescription; those functions update the relevant state.
+        /// </summary>
+        public TypedIndex Shape;
+        //Note that statics do not store a 'speculative margin' independently of the contini
+        /// <summary>
+        /// 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.
+        /// </summary>
+        public int BroadPhaseIndex;
+    }
+
     /// <summary>
-    /// Collection of allocated static collidables.
+    /// Collection of allocated statics.
     /// </summary>
     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.
+
         /// <summary>
         /// 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.
         /// </summary>
         public Buffer<int> HandleToIndex;
         /// <summary>
@@ -63,10 +104,10 @@ public class Statics
         /// <summary>
         /// 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 <see cref="SetShape"/> or <see cref="ApplyDescription(StaticHandle, in StaticDescription)"/> to handle the bookkeeping changes automatically if changing the shape.
         /// </summary>
-        public Buffer<Collidable> Collidables;
+        public Buffer<Static> StaticsBuffer;
 
-        public Buffer<RigidPose> Poses;
         public IdPool HandlePool;
         protected BufferPool pool;
         public int Count;
@@ -76,7 +117,49 @@ public class Statics
         internal BroadPhase broadPhase;
         internal IslandAwakener awakener;
 
-        public unsafe Statics(BufferPool pool, Shapes shapes, Bodies bodies, BroadPhase broadPhase, int initialCapacity = 4096)
+        /// <summary>
+        /// Gets a reference to the memory backing a static collidable. The <see cref="StaticReference"/> type is a helper that exposes common operations for statics.
+        /// </summary>
+        /// <param name="handle">Handle of the static to retrieve a reference for.</param>
+        /// <returns>Reference to the memory backing a static collidable.</returns>
+        /// <remarks>Equivalent to <see cref="GetStaticReference(StaticHandle)"/>.</remarks>
+        public StaticReference this[StaticHandle handle]
+        {
+            get
+            {
+                ValidateExistingHandle(handle);
+                return new StaticReference(handle, this);
+            }
+        }
+
+        /// <summary>
+        /// Gets a direct reference to the memory backing a static.
+        /// </summary>
+        /// <param name="handle">Handle of the static to get a reference of.</param>
+        /// <returns>Direct reference to the memory backing a static.</returns>
+        /// <remarks>This is distinct from the <see cref="this[StaticHandle]"/> indexer in that this returns the direct memory reference. <see cref="StaticReference"/> includes a layer of indirection that can expose more features.</remarks>
+        public ref Static GetDirectReference(StaticHandle handle)
+        {
+            ValidateExistingHandle(handle);
+            return ref StaticsBuffer[HandleToIndex[handle.Value]];
+        }
+
+        /// <summary>
+        /// Gets a reference to the raw memory backing a static collidable.
+        /// </summary>
+        /// <param name="index">Index of the static to retrieve a memory reference for.</param>
+        /// <returns>Direct reference to the memory backing a static collidable.</returns>
+        public ref Static this[int index]
+        {
+            get
+            {
+                Debug.Assert(index >= 0 && index < Count);
+                ValidateExistingHandle(IndexToHandle[index]);
+                return ref StaticsBuffer[index];
+            }
+        }
+
+        public Statics(BufferPool pool, Shapes shapes, Bodies bodies, BroadPhase broadPhase, int initialCapacity = 4096)
         {
             this.pool = pool;
             InternalResize(Math.Max(1, initialCapacity));
@@ -93,11 +176,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<int>(targetCapacity);
-            Debug.Assert(Poses.Length != BufferPool.GetCapacityForCount<RigidPoses>(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<Static>(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 +202,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.");
@@ -128,33 +210,37 @@ public 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<int>
+        struct SleepingBodyCollector<TFilter> : IBreakableForEach<int> where TFilter : struct, IStaticChangeAwakeningFilter
         {
+            Bodies bodies;
             BroadPhase broadPhase;
             BufferPool pool;
-            public QuickList<BodyHandle> SleepingBodyHandles;
+            public QuickList<int> 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<BodyHandle>(32, pool);
+                this.pool = pool;
+                SleepingSets = new QuickList<int>(32, pool);
+                Filter = filter;
             }
 
             [MethodImpl(MethodImplOptions.AggressiveInlining)]
             public void Dispose()
             {
-                SleepingBodyHandles.Dispose(pool);
+                SleepingSets.Dispose(pool);
             }
 
             [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)
                 {
-                    SleepingBodyHandles.Add(leaf.BodyHandle, pool);
+                    if (Filter.ShouldAwaken(bodies[leaf.BodyHandle]))
+                        SleepingSets.Add(bodies.HandleToLocation[leaf.BodyHandle.Value].SetIndex, pool);
                 }
                 return true;
             }
@@ -164,21 +250,19 @@ void AwakenBodiesInBounds<TFilter>(ref BoundingBox bounds, ref TFilter filter) w
         {
             if (filter.AllowAwakening)
             {
-                var collector = new SleepingBodyCollector(broadPhase, pool);
-                broadPhase.StaticTree.GetOverlaps(bounds, ref collector);
-                for (int i = 0; i < collector.SleepingBodyHandles.Count; ++i)
-                {
-                    if (filter.ShouldAwaken(bodies.GetBodyReference(collector.SleepingBodyHandles[i])))
-                        awakener.AwakenBody(collector.SleepingBodyHandles[i]);
-                }
+                var collector = new SleepingBodyCollector<TFilter>(bodies, broadPhase, pool, ref filter);
+                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;
                 collector.Dispose();
             }
         }
 
-        unsafe void AwakenBodiesInExistingBounds<TFilter>(ref Collidable collidable, ref TFilter filter) where TFilter : struct, IStaticChangeAwakeningFilter
+        unsafe void AwakenBodiesInExistingBounds<TFilter>(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 +281,9 @@ public void RemoveAt<TAwakeningFilter>(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 +297,7 @@ public void RemoveAt<TAwakeningFilter>(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;
+                    GetDirectReference(movedLeaf.StaticHandle).BroadPhaseIndex = removedBroadPhaseIndex;
                 }
                 else
                 {
@@ -232,9 +316,7 @@ public void RemoveAt<TAwakeningFilter>(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] = StaticsBuffer[movedStaticOriginalIndex];
                 //Point the static handles at the new location.
                 var lastHandle = IndexToHandle[movedStaticOriginalIndex];
                 HandleToIndex[lastHandle.Value] = index;
@@ -283,8 +365,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, collidable.Shape, out var bodyBounds);
             broadPhase.UpdateStaticBounds(collidable.BroadPhaseIndex, bodyBounds.Min, bodyBounds.Max);
         }
 
@@ -296,19 +378,15 @@ void ComputeNewBoundsAndAwaken<TAwakeningFilter>(in RigidPose pose, TypedIndex s
             AwakenBodiesInBounds(ref bounds, ref filter);
         }
 
-
-
-
         internal void ApplyDescriptionByIndexWithoutBroadPhaseModification<TAwakeningFilter>(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];
-            Debug.Assert(description.Collidable.Shape.Exists, "Static collidables must have a shape. Their only purpose is colliding.");
-            collidable.Continuity = description.Collidable.Continuity;
-            collidable.SpeculativeMargin = description.Collidable.SpeculativeMargin;
-            collidable.Shape = description.Collidable.Shape;
+            ref var collidable = ref this[index];
+            collidable.Pose = description.Pose;
+            Debug.Assert(description.Shape.Exists, "Static collidables must have a shape. Their only purpose is colliding.");
+            collidable.Continuity = description.Continuity;
+            collidable.Shape = description.Shape;
 
-            ComputeNewBoundsAndAwaken(description.Pose, description.Collidable.Shape, ref filter, out bounds);
+            ComputeNewBoundsAndAwaken(description.Pose, description.Shape, ref filter, out bounds);
         }
 
         /// <summary>
@@ -334,7 +412,7 @@ public StaticHandle Add<TAwakeningFilter>(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;
         }
 
@@ -347,6 +425,29 @@ public StaticHandle Add(in StaticDescription description)
         {
             var defaultFilter = default(StaticsShouldntAwakenKinematics);
             return Add(description, ref defaultFilter);
+        }
+
+        /// <summary>
+        /// Adds a new static body to the simulation. No attempt is made to awaken sleeping bodies near the static.
+        /// </summary>
+        /// <param name="description">Description of the static to add.</param>
+        /// <returns>Handle of the new static.</returns>
+        /// <remarks>
+        /// For most use cases, defaulting to the <see cref="Add"/> 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.
+        /// <para>
+        /// 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.
+        /// </para>
+        /// <para>
+        /// Other custom filters can be used to control which bodies are awoken; see <see cref="Add{TAwakeningFilter}(in StaticDescription, ref TAwakeningFilter)"/> and the <see cref="IStaticChangeAwakeningFilter"/> interface.
+        /// </para>
+        /// </remarks>
+        public StaticHandle AddWithoutAwakeningBodies(in StaticDescription description)
+        {
+            var filter = default(StaticsShouldntAwakenBodies);
+            return Add(description, ref filter);
         }
 
         /// <summary>
@@ -361,10 +462,10 @@ public void SetShape<TAwakeningFilter>(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);
         }
 
@@ -387,16 +488,17 @@ public void SetShape(StaticHandle handle, TypedIndex newShape)
         /// <param name="description">Description to apply to the static.</param>
         /// <param name="filter">Filter to apply to sleeping bodies near the static to see if they should be awoken.</param>
         /// <typeparam name="TAwakeningFilter">Type of the filter to apply to sleeping bodies.</typeparam>
-        public unsafe void ApplyDescription<TAwakeningFilter>(StaticHandle handle, in StaticDescription description, ref TAwakeningFilter filter) where TAwakeningFilter : struct, IStaticChangeAwakeningFilter
+        public void ApplyDescription<TAwakeningFilter>(StaticHandle handle, in StaticDescription description, ref TAwakeningFilter filter) where TAwakeningFilter : struct, IStaticChangeAwakeningFilter
         {
             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.
-            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);
         }
 
         /// <summary>
@@ -405,7 +507,7 @@ public unsafe void ApplyDescription<TAwakeningFilter>(StaticHandle handle, in St
         /// </summary>
         /// <param name="handle">Handle of the static to apply the description to.</param>
         /// <param name="description">Description to apply to the static.</param>
-        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);
@@ -420,12 +522,21 @@ public void GetDescription(StaticHandle handle, out StaticDescription descriptio
         {
             ValidateExistingHandle(handle);
             var index = HandleToIndex[handle.Value];
-            BundleIndexing.GetBundleIndices(index, out var bundleIndex, out var innerIndex);
-            description.Pose = Poses[index];
-            ref var collidable = ref Collidables[index];
-            description.Collidable.Continuity = collidable.Continuity;
-            description.Collidable.Shape = collidable.Shape;
-            description.Collidable.SpeculativeMargin = collidable.SpeculativeMargin;
+            ref var collidable = ref this[index];
+            description.Pose = collidable.Pose;
+            description.Continuity = collidable.Continuity;
+            description.Shape = collidable.Shape;
+        }
+
+        /// <summary>
+        /// Gets the current description of the static referred to by a given handle.
+        /// </summary>
+        /// <param name="handle">Handle of the static to look up the description of.</param>
+        /// <returns>Gathered description of the handle-referenced static.</returns>
+        public StaticDescription GetDescription(StaticHandle handle)
+        {
+            GetDescription(handle, out var description);
+            return description;
         }
 
         /// <summary>
@@ -481,52 +592,10 @@ public void EnsureCapacity(int capacity)
         /// <remarks>The object can be reused if it is reinitialized by using EnsureCapacity or Resize.</remarks>
         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<float>.Count) = source.Position.Y;
-            Unsafe.Add(ref targetPositionSlot, 2 * Vector<float>.Count) = source.Position.Z;
-            targetOrientationSlot = source.Orientation.X;
-            Unsafe.Add(ref targetOrientationSlot, Vector<float>.Count) = source.Orientation.Y;
-            Unsafe.Add(ref targetOrientationSlot, 2 * Vector<float>.Count) = source.Orientation.Z;
-            Unsafe.Add(ref targetOrientationSlot, 3 * Vector<float>.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 RigidPoses poses, out Vector<int> shapeIndices, out Vector<float> maximumExpansion)
-        {
-            Debug.Assert(count <= Vector<float>.Count);
-            Unsafe.SkipInit(out poses);
-            Unsafe.SkipInit(out shapeIndices);
-            Unsafe.SkipInit(out maximumExpansion);
-            ref var targetPositionBase = ref Unsafe.As<Vector<float>, float>(ref poses.Position.X);
-            ref var targetOrientationBase = ref Unsafe.As<Vector<float>, float>(ref poses.Orientation.X);
-            ref var targetShapeBase = ref Unsafe.As<Vector<int>, int>(ref shapeIndices);
-            ref var targetExpansionBase = ref Unsafe.As<Vector<float>, 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/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
-{
-    /// <summary>
-    /// 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.
-    /// </summary>
-    public class SubsteppingTimestepper : ITimestepper
-    {
-        /// <summary>
-        /// Gets or sets the number of substeps to execute during each timestep.
-        /// </summary>
-        public int SubstepCount { get; set; }
-
-        /// <summary>
-        /// Fires after the sleeper completes and before bodies are integrated.
-        /// </summary>
-        public event TimestepperStageHandler Slept;
-        /// <summary>
-        /// Fires after bodies have their bounding boxes updated for the frame's predicted motion and before collision detection.
-        /// </summary>
-        public event TimestepperStageHandler BeforeCollisionDetection;
-        /// <summary>
-        /// Fires after all collisions have been identified, but before the substep loop begins.
-        /// </summary>
-        public event TimestepperStageHandler CollisionsDetected;
-        /// <summary>
-        /// Fires at the beginning of a substep.
-        /// </summary>
-        public event TimestepperSubstepStageHandler SubstepStarted;
-        /// <summary>
-        /// Fires after contact constraints are incrementally updated at the beginning of substeps after the first and before velocities are integrated.
-        /// </summary>
-        public event TimestepperSubstepStageHandler ContactConstraintsUpdatedForSubstep;
-        /// <summary>
-        /// Fires after bodies have their velocities integrated and before the solver executes.
-        /// </summary>
-        public event TimestepperSubstepStageHandler VelocitiesIntegrated;
-        /// <summary>
-        /// Fires after the solver executes and before body poses are integrated.
-        /// </summary>
-        public event TimestepperSubstepStageHandler ConstraintsSolved;
-        /// <summary>
-        /// Fires after bodies have their poses integrated and before the substep ends.
-        /// </summary>
-        public event TimestepperSubstepStageHandler PosesIntegrated;
-        /// <summary>
-        /// Fires at the end of a substep.
-        /// </summary>
-        public event TimestepperSubstepStageHandler SubstepEnded;
-        /// <summary>
-        /// Fires after all substeps are finished executing and before data structures are incrementally optimized.
-        /// </summary>
-        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/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.
     /// </summary>
     [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.
     /// </summary>
     [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 ad6ba3ff8..03eaae9fe 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
 {
@@ -26,7 +24,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 +38,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;
@@ -113,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);
     }
 
 
@@ -170,7 +168,22 @@ public RayBatcher(BufferPool pool, int rayCapacity = 2048, int treeDepthForPreal
             ResizeRayStacks(rayCapacity, treeDepthForPreallocation);
 
             stackPointer = stackPointerA0 = stackPointerB = stackPointerA1 = 0;
+        }
 
+        /// <summary>
+        /// Disposes all the resources backing the ray batcher.
+        /// </summary>
+        public void Dispose()
+        {
+            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;
         }
 
         void ResizeRayStacks(int rayCapacity, int treeDepthForPreallocation)
@@ -259,7 +272,7 @@ interface ITreeRaySource
             int this[int rayIndex] { get; }
         }
 
-        unsafe struct RootRaySource : ITreeRaySource
+        struct RootRaySource : ITreeRaySource
         {
             int rayCount;
 
@@ -314,7 +327,7 @@ public int this[int rayIndex]
             }
         }
 
-        unsafe void TestNode<TRaySource>(ref Node node, byte depth, ref TRaySource raySource) where TRaySource : struct, ITreeRaySource
+        void TestNode<TRaySource>(ref Node node, byte depth, ref TRaySource raySource) where TRaySource : struct, ITreeRaySource
         {
             int a0Start = stackPointerA0;
             int bStart = stackPointerB;
@@ -419,9 +432,7 @@ public unsafe void TestRays<TLeafTester>(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;
 
@@ -498,7 +509,7 @@ public unsafe void TestRays<TLeafTester>(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
@@ -507,7 +518,7 @@ public unsafe void TestRays<TLeafTester>(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);
                     }
                 }
             }
@@ -547,21 +558,5 @@ public void ResetRays()
             batchRayCount = 0;
         }
 
-        /// <summary>
-        /// Disposes all the resources backing the ray batcher.
-        /// </summary>
-        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;
-        }
-
-
     }
 }
diff --git a/BepuPhysics/Trees/Tree.cs b/BepuPhysics/Trees/Tree.cs
index 017212fd0..08c0d44e9 100644
--- a/BepuPhysics/Trees/Tree.cs
+++ b/BepuPhysics/Trees/Tree.cs
@@ -1,65 +1,96 @@
 using BepuUtilities.Memory;
 using System;
 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
     {
+        /// <summary>
+        /// Buffer of nodes in the tree.
+        /// </summary>
         public Buffer<Node> Nodes;
+        /// <summary>
+        /// Buffer of metanodes in the tree. Metanodes contain metadata that aren't read during most query operations but are useful for bookkeeping.
+        /// </summary>
         public Buffer<Metanode> Metanodes;
-        int nodeCount;
-        public int NodeCount
-        {
-            readonly get
-            {
-                return nodeCount;
-            }
-            set
-            {
-                nodeCount = value;
-            }
-        }
-
+        /// <summary>
+        /// Buffer of leaves in the tree.
+        /// </summary>
         public Buffer<Leaf> Leaves;
-        int leafCount;
-        public int LeafCount
-        {
-            readonly get
-            {
-                return leafCount;
-            }
-            set
-            {
-                leafCount = value;
-            }
-        }
+        /// <summary>
+        /// Number of nodes in the tree.
+        /// </summary>
+        public int NodeCount;
+        /// <summary>
+        /// Number of leaves in the tree.
+        /// </summary>
+        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++;
         }
 
+        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++;
+        }
+
+        /// <summary>
+        /// Gets bounds pointerse for a leaf in the tree.
+        /// </summary>
+        /// <param name="leafIndex">Index of the leaf in the tree.</param>
+        /// <param name="minPointer">Pointer to the minimum bounds vector in the tree.</param>
+        /// <param name="maxPointer">Pointer to the maximum bounds vector in the tree.</param>
+        [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;
+        }
+
+        /// <summary>
+        /// Applies updated bounds to the given leaf index in the tree, refitting the tree to match.
+        /// </summary>
+        /// <param name="leafIndex">Index of the leaf in the tree to update.</param>
+        /// <param name="min">New minimum bounds for the leaf.</param>
+        /// <param name="max">New maximum bounds for the leaf.</param>
+        [MethodImpl(MethodImplOptions.AggressiveInlining)]
+        public 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);
+        }
 
         /// <summary>
         /// Constructs an empty tree.
         /// </summary>
+        /// <param name="pool">Buffer pool to use to allocate resources in the tree.</param>
         /// <param name="initialLeafCapacity">Initial number of leaves to allocate room for.</param>
-        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.");
@@ -77,19 +108,19 @@ public Tree(Span<byte> 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<byte, int>(ref data[0]);
-            nodeCount = leafCount - 1;
-            var leafByteCount = leafCount * sizeof(Leaf);
-            var nodeByteCount = nodeCount * sizeof(Node);
-            var metanodeByteCount = nodeCount * sizeof(Metanode);
+            LeafCount = Unsafe.As<byte, int>(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);
@@ -98,7 +129,6 @@ public Tree(Span<byte> data, BufferPool pool)
         /// <summary>
         /// Gets the number of bytes required to store the tree.
         /// </summary>
-        /// <param name="tree">Tree to measure.</param>
         /// <returns>Number of bytes required to store the tree.</returns>
         public readonly int GetSerializedByteCount()
         {
@@ -108,7 +138,6 @@ public readonly int GetSerializedByteCount()
         /// <summary>
         /// Writes a tree into a byte buffer.
         /// </summary>
-        /// <param name="tree">Tree to write into the buffer.</param>
         /// <param name="bytes">Buffer to hold the tree's data.</param>
         public readonly void Serialize(Span<byte> bytes)
         {
@@ -138,7 +167,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;
@@ -152,27 +181,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<Leaf>(Math.Max(leafCount, targetLeafSlotCount));
+            var leafCapacityForTarget = BufferPool.GetCapacityForCount<Leaf>(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<Node>(Math.Max(nodeCount, leafCapacityForTarget - 1));
-            var metanodeCapacityForTarget = BufferPool.GetCapacityForCount<Metanode>(Math.Max(nodeCount, leafCapacityForTarget - 1));
+            var nodeCapacityForTarget = BufferPool.GetCapacityForCount<Node>(Math.Max(NodeCount, leafCapacityForTarget - 1));
+            var metanodeCapacityForTarget = BufferPool.GetCapacityForCount<Metanode>(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)
             {
@@ -186,7 +215,7 @@ public void Resize(BufferPool pool, int targetLeafSlotCount)
         /// </summary>
         public void Clear()
         {
-            leafCount = 0;
+            LeafCount = 0;
             InitializeRoot();
         }
 
@@ -215,7 +244,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 9409a55db..6ea6fef77 100644
--- a/BepuPhysics/Trees/Tree_Add.cs
+++ b/BepuPhysics/Trees/Tree_Add.cs
@@ -1,195 +1,220 @@
 using BepuUtilities;
 using System.Numerics;
 using System.Runtime.CompilerServices;
-using System;
-using System.Diagnostics;
 using BepuUtilities.Memory;
 
-namespace BepuPhysics.Trees
+namespace BepuPhysics.Trees;
+
+partial struct Tree
 {
-    partial struct Tree
+    private struct InsertShouldNotRotate { }
+    private struct InsertShouldRotateBottomUp { }
+
+    /// <summary>
+    /// Adds a leaf to the tree with the given bounding box and returns the index of the added leaf.
+    /// </summary>
+    /// <param name="bounds">Extents of the leaf bounds.</param>
+    /// <param name="pool">Resource pool to use if resizing is required.</param>
+    /// <returns>Index of the leaf allocated in the tree's leaf array.</returns>
+    /// <remarks>This performs no incremental refinement. When acting on the same tree, it's slightly cheaper than <see cref="Add"/>, 
+    /// but the quality of the tree depends on insertion order.<para/>
+    /// 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.<para/>
+    /// This is typically best reserved for cases where the insertion order is known to be randomized or otherwise conducive to building decent trees.</remarks>
+    public int AddWithoutRefinement(BoundingBox bounds, BufferPool pool)
     {
-        /// <summary>
-        /// 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.
-        /// </summary>
-        /// <param name="newLeafBounds">Bounding box of the leaf being added.</param>
-        /// <param name="parentIndex">Index of the parent node that the existing leaf belongs to.</param>
-        /// <param name="indexInParent">Index of the child wtihin the parent node that the existing leaf belongs to.</param>
-        /// <param name="merged">Bounding box holding both the new and existing leaves.</param>
-        /// <returns>Index of the leaf </returns>
-        [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;
-
-            //Update the old leaf node with the new index information.
-            var oldLeafIndex = Encode(newNode.A.Index);
-            Leaves[oldLeafIndex] = new Leaf(newNodeIndex, 0);
+        return Add<InsertShouldNotRotate>(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;
-        }
+    /// <summary>
+    /// Adds a leaf to the tree with the given bounding box and returns the index of the added leaf.
+    /// </summary>
+    /// <param name="bounds">Extents of the leaf bounds.</param>
+    /// <param name="pool">Resource pool to use if resizing is required.</param>
+    /// <returns>Index of the leaf allocated in the tree's leaf array.</returns>
+    /// <remarks>Performs incrementally refining tree rotations when returning along the insertion path, unlike <see cref="AddWithoutRefinement"/>.<para/>
+    /// This is about twice the cost of <see cref="AddWithoutRefinement"/> (outside of pathological cases).<para/>
+    /// Trees built with repeated insertions of this kind tend to have better quality and fewer pathological cases compared to <see cref="AddWithoutRefinement"/>.</remarks>
+    public int Add(BoundingBox bounds, BufferPool pool)
+    {
+        return Add<InsertShouldRotateBottomUp>(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 int Add<TShouldRotate>(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<BoundingBox, NodeChild>(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<BoundingBox, BoundingBox4>(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)
+            ref var node = ref Nodes[nodeIndex];
+            // 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 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<BoundingBox4, NodeChild>(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<BoundingBox, NodeChild>(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;
             }
-
         }
 
-        /// <summary>
-        /// Adds a leaf to the tree with the given bounding box and returns the index of the added leaf.
-        /// </summary>
-        /// <param name="bounds">Extents of the leaf bounds.</param>
-        /// <param name="pool">Resource pool to use if resizing is required.</param>
-        /// <returns>Index of the leaf allocated in the tree's leaf array.</returns>
-        public unsafe int Add(ref 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)
+            var parentIndex = Leaves[newLeafIndex].NodeIndex;
+            while (parentIndex >= 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);
-            }
-
-            //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)
-            {
-                //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 void TryRotateNode(int rotationRootIndex)
+    {
+        ref var root = ref Nodes[rotationRootIndex];
+        var costA = EstimateCost(root.A);
+        var costB = EstimateCost(root.B);
+        var originalCost = costA + costB;
+        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 costAA = EstimateCost(a.A);
+            var costAB = EstimateCost(a.B);
+            var costAAB = ComputeBoundsMetric(Unsafe.As<NodeChild, BoundingBox4>(ref aaB)) * (a.A.LeafCount + root.B.LeafCount) + costAB;
+            var costABB = ComputeBoundsMetric(Unsafe.As<NodeChild, BoundingBox4>(ref abB)) * (a.B.LeafCount + root.B.LeafCount) + costAA;
+            rightUsesA = costAAB < costABB;
+            rightRotationCostChange = float.Min(costAAB, costABB) - originalCost;
+        }
+        if (root.B.Index >= 0)
         {
-            return ComputeBoundsMetric(ref bounds.Min, ref bounds.Max);
+            //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(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<NodeChild, BoundingBox4>(ref baA)) * (b.A.LeafCount + root.A.LeafCount) + costBB;
+            var costBBA = ComputeBoundsMetric(Unsafe.As<NodeChild, BoundingBox4>(ref bbA)) * (b.B.LeafCount + root.A.LeafCount) + costBA;
+            leftUsesA = costBAA < costBBA;
+            leftRotationCostChange = float.Min(costBAA, costBBA) - originalCost;
         }
-        [MethodImpl(MethodImplOptions.NoInlining)]
-        internal static float ComputeBoundsMetric(ref Vector3 min, ref Vector3 max)
+        if (float.Min(leftRotationCostChange, rightRotationCostChange) < 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;
+            //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_BinnedBuilder.cs b/BepuPhysics/Trees/Tree_BinnedBuilder.cs
new file mode 100644
index 000000000..c72e514cd
--- /dev/null
+++ b/BepuPhysics/Trees/Tree_BinnedBuilder.cs
@@ -0,0 +1,1600 @@
+using BepuUtilities;
+using BepuUtilities.Collections;
+using BepuUtilities.Memory;
+using BepuUtilities.TaskScheduling;
+using System;
+using System.Diagnostics;
+using System.Numerics;
+using System.Runtime.CompilerServices;
+using System.Runtime.InteropServices;
+using System.Runtime.Intrinsics;
+using System.Runtime.Intrinsics.X86;
+using System.Threading;
+
+namespace BepuPhysics.Trees;
+partial struct Tree
+{
+    struct LeavesHandledInPostPass { }
+
+    [MethodImpl(MethodImplOptions.AggressiveInlining)]
+    static void BuildNode<TLeaves>(
+        BoundingBox4 a, BoundingBox4 b,
+        int leafCountA, int leafCountB,
+        Buffer<NodeChild> subtrees, Buffer<Node> nodes, Buffer<Metanode> 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<Leaf>), "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<BoundingBox4, NodeChild>(ref a);
+        node.B = Unsafe.As<BoundingBox4, NodeChild>(ref b);
+        node.A.LeafCount = leafCountA;
+        node.B.LeafCount = leafCountB;
+        if (subtreeCountA == 1)
+        {
+            aIndex = subtrees[0].Index;
+            if (typeof(TLeaves) == typeof(Buffer<Leaf>))
+            {
+                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<TLeaves, Buffer<Leaf>>(ref leaves)[Encode(aIndex)] = new Leaf(nodeIndex, 0);
+            }
+        }
+        else
+        {
+            aIndex = nodeIndex + 1;
+        }
+        node.A.Index = aIndex;
+        if (subtreeCountB == 1)
+        {
+            bIndex = subtrees[^1].Index;
+            if (typeof(TLeaves) == typeof(Buffer<Leaf>))
+            {
+                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<TLeaves, Buffer<Leaf>>(ref leaves)[Encode(bIndex)] = new Leaf(nodeIndex, 1);
+            }
+        }
+        else
+        {
+            bIndex = nodeIndex + subtreeCountA; //parentNodeIndex + 1 + (subtreeCountA - 1)
+        }
+        node.B.Index = bIndex;
+    }
+
+    /// <summary>
+    /// Computes a local cost estimate for a node child using its bounds and leaf count.
+    /// Handy for 
+    /// </summary>
+    /// <param name="child">Child to estimate the cost of.</param>
+    /// <returns>Estimated cost of the child.</returns>
+    internal static float EstimateCost(NodeChild child) => ComputeBoundsMetric(Unsafe.As<NodeChild, BoundingBox4>(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<BoundingBox4> binBoundingBoxes, out Buffer<BoundingBox4> binCentroidBoundingBoxes,
+            out Buffer<BoundingBox4> binBoundingBoxesScan, out Buffer<BoundingBox4> binCentroidBoundingBoxesScan, out Buffer<int> binLeafCounts);
+    }
+
+
+    struct Context<TLeaves, TThreading>
+        where TLeaves : unmanaged
+        where TThreading : unmanaged, IBinnedBuilderThreading
+    {
+        public int MinimumBinCount;
+        public int MaximumBinCount;
+        public float LeafToBinMultiplier;
+        public int MicrosweepThreshold;
+
+        public bool Deterministic;
+
+        public TLeaves Leaves;
+        public Buffer<NodeChild> SubtreesPing;
+        public Buffer<NodeChild> SubtreesPong;
+        public Buffer<Node> Nodes;
+        public Buffer<Metanode> Metanodes;
+
+        public Buffer<byte> BinIndices;
+
+        public TThreading Threading;
+
+        public Context(int minimumBinCount, int maximumBinCount, float leafToBinMultiplier, int microsweepThreshold, bool deterministic,
+            Buffer<NodeChild> subtreesPing, Buffer<NodeChild> subtreesPong, TLeaves leaves, Buffer<Node> nodes, Buffer<Metanode> metanodes, Buffer<byte> 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<NodeChild> { 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<NodeChild> { 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<NodeChild> { 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 static NodeTimes[] Times;
+
+    static unsafe void MicroSweepForBinnedBuilder<TLeaves, TThreading>(
+        Vector4 centroidMin, Vector4 centroidMax, ref TLeaves leaves,
+        Buffer<NodeChild> subtrees, Buffer<Node> nodes, Buffer<Metanode> metanodes, int nodeIndex, int parentNodeIndex, int childIndexInParent, Context<TLeaves, TThreading>* 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)
+        {
+            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<NodeChild, BoundingBox4>(ref subtreeA), Unsafe.As<NodeChild, BoundingBox4>(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);
+
+        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<BoundingBox4>() * binBoundingBoxes.Length >= (paddedKeyCount * 2 + subtreeCount) * Unsafe.SizeOf<int>(),
+                "The bins should preallocate enough space to handle the needs of microsweeps. They reuse the same allocations.");
+            var keys = new Buffer<float>(binBoundingBoxes.Memory, paddedKeyCount);
+            var targetIndices = new Buffer<int>(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)
+            {
+                keys[i] = float.MaxValue;
+            }
+            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<NodeChild>();
+            subtrees.CopyTo(0, subtreeCache, 0, subtreeCount);
+            for (int i = 0; i < subtreeCount; ++i)
+            {
+                var targetIndex = targetIndices[i];
+                subtrees[targetIndex] = subtreeCache[i];
+            }
+        }
+        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
+            {
+                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<BoundingBox4>();
+        binBoundingBoxesScan[0] = boundingBoxes[0];
+        int totalLeafCount = subtrees[0].LeafCount;
+        for (int i = 1; i < subtreeCount; ++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;
+        }
+
+        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)
+        {
+            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;
+        }
+        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, centroidMin, centroidMax, context, workerIndex);
+            return;
+        }
+
+        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)
+        {
+            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);
+        }
+        if (subtreeCountB > 1)
+        {
+            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);
+        }
+    }
+
+    [MethodImpl(MethodImplOptions.AggressiveInlining)]
+    private static int ComputeBinIndex(Vector4 centroidMin, bool useX, bool useY, Vector128<int> 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<BoundingBox4> BinBoundingBoxes;
+        public Buffer<BoundingBox4> BinCentroidBoundingBoxes;
+        public Buffer<BoundingBox4> BinBoundingBoxesScan;
+        public Buffer<BoundingBox4> BinCentroidBoundingBoxesScan;
+        public Buffer<int> BinLeafCounts;
+
+        public SingleThreaded(Buffer<byte> binAllocationBuffer, int binCapacity)
+        {
+            int start = 0;
+            BinBoundingBoxes = Suballocate<BoundingBox4>(binAllocationBuffer, ref start, binCapacity);
+            BinCentroidBoundingBoxes = Suballocate<BoundingBox4>(binAllocationBuffer, ref start, binCapacity);
+            BinBoundingBoxesScan = Suballocate<BoundingBox4>(binAllocationBuffer, ref start, binCapacity);
+            BinCentroidBoundingBoxesScan = Suballocate<BoundingBox4>(binAllocationBuffer, ref start, binCapacity);
+            BinLeafCounts = Suballocate<int>(binAllocationBuffer, ref start, binCapacity);
+        }
+
+        public void GetBins(int workerIndex,
+            out Buffer<BoundingBox4> binBoundingBoxes, out Buffer<BoundingBox4> binCentroidBoundingBoxes,
+            out Buffer<BoundingBox4> binBoundingBoxesScan, out Buffer<BoundingBox4> binCentroidBoundingBoxesScan, out Buffer<int> binLeafCounts)
+        {
+            binBoundingBoxes = BinBoundingBoxes;
+            binCentroidBoundingBoxes = BinCentroidBoundingBoxes;
+            binBoundingBoxesScan = BinBoundingBoxesScan;
+            binCentroidBoundingBoxesScan = BinCentroidBoundingBoxesScan;
+            binLeafCounts = BinLeafCounts;
+        }
+    }
+
+    static Buffer<T> Suballocate<T>(Buffer<byte> buffer, ref int start, int count) where T : unmanaged
+    {
+        var size = count * Unsafe.SizeOf<T>();
+        var previousStart = start;
+        start += size;
+        return buffer.Slice(previousStart, size).As<T>();
+    }
+
+    /// <summary>
+    /// Stores resources required by a worker to dispatch and manage multithreaded work.
+    /// </summary>
+    /// <remarks>
+    /// 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.
+    /// </remarks>
+    struct BinnedBuildWorkerContext
+    {
+        /// <summary>
+        /// Bins associated with this worker for the duration of a node. This allocation will persist across the build.
+        /// </summary>
+        public Buffer<BoundingBox4> BinBoundingBoxes;
+        /// <summary>
+        /// Centroid bound bins associated with this worker for the duration of a node. This allocation will persist across the build.
+        /// </summary>
+        public Buffer<BoundingBox4> BinCentroidBoundingBoxes;
+        /// <summary>
+        /// Bins associated with this worker for use in the SAH scan. This allocation will persist across the build.
+        /// </summary>
+        public Buffer<BoundingBox4> BinBoundingBoxesScan;
+        /// <summary>
+        /// Centroid bound bins associated with this worker for use in the SAH scan. This allocation will persist across the build.
+        /// </summary>
+        public Buffer<BoundingBox4> BinCentroidBoundingBoxesScan;
+        /// <summary>
+        /// Bin leaf counts associated with this worker for the duration of a node. This allocation will persist across the build.
+        /// </summary>
+        public Buffer<int> BinLeafCounts;
+
+        public BinnedBuildWorkerContext(Buffer<byte> binAllocationBuffer, ref int binStart, int binCapacity)
+        {
+            BinBoundingBoxes = Suballocate<BoundingBox4>(binAllocationBuffer, ref binStart, binCapacity);
+            BinCentroidBoundingBoxes = Suballocate<BoundingBox4>(binAllocationBuffer, ref binStart, binCapacity);
+            BinBoundingBoxesScan = Suballocate<BoundingBox4>(binAllocationBuffer, ref binStart, binCapacity);
+            BinCentroidBoundingBoxesScan = Suballocate<BoundingBox4>(binAllocationBuffer, ref binStart, binCapacity);
+            BinLeafCounts = Suballocate<int>(binAllocationBuffer, ref binStart, binCapacity);
+        }
+    }
+    unsafe struct MultithreadBinnedBuildContext : IBinnedBuilderThreading
+    {
+        public TaskStack* TaskStack;
+        /// <summary>
+        /// The number of subtrees present at the root of the build.
+        /// </summary>
+        public int OriginalSubtreeCount;
+        /// <summary>
+        /// 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.
+        /// </summary>
+        public int TopLevelTargetTaskCount;
+        public Buffer<BinnedBuildWorkerContext> Workers;
+
+        public void GetBins(int workerIndex,
+            out Buffer<BoundingBox4> binBoundingBoxes, out Buffer<BoundingBox4> binCentroidBoundingBoxes,
+            out Buffer<BoundingBox4> binBoundingBoxesScan, out Buffer<BoundingBox4> binCentroidBoundingBoxesScan, out Buffer<int> binLeafCounts)
+        {
+            ref var worker = ref Workers[workerIndex];
+            binBoundingBoxes = worker.BinBoundingBoxes;
+            binCentroidBoundingBoxes = worker.BinCentroidBoundingBoxes;
+            binBoundingBoxesScan = worker.BinBoundingBoxesScan;
+            binCentroidBoundingBoxesScan = worker.BinCentroidBoundingBoxesScan;
+            binLeafCounts = worker.BinLeafCounts;
+        }
+
+        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 = 1024;
+    const int MinimumSubtreesPerThreadForBinning = 1024;
+    const int MinimumSubtreesPerThreadForPartitioning = 1024;
+    const int MinimumSubtreesPerThreadForNodeJob = 256;
+    const int TargetTaskCountMultiplierForNodePushOverInnerLoop = 8;
+    /// <summary>
+    /// Random value stored in the upper 32 bits of the job tag submitted for internal multithreading operations.
+    /// </summary>
+    /// <remarks>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.</remarks>
+    const ulong JobFilterTagHeader = 0xB0A1BF32ul << 32;
+
+    [MethodImpl(MethodImplOptions.AggressiveInlining)]
+    static BoundingBox4 ComputeCentroidBounds(Buffer<BoundingBox4> 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;
+    }
+
+    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 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;
+        }
+
+        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;
+        }
+    }
+
+    struct CentroidPrepassTaskContext
+    {
+        public SharedTaskData TaskData;
+        /// <summary>
+        /// 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.
+        /// </summary>
+        public Buffer<BoundingBox4> PrepassWorkers;
+        /// <summary>
+        /// Buffer containing the bounding boxes of all subtrees in the node.
+        /// </summary>
+        public Buffer<BoundingBox4> Bounds;
+
+        public CentroidPrepassTaskContext(BufferPool pool, SharedTaskData taskData, Buffer<BoundingBox4> bounds)
+        {
+            TaskData = taskData;
+            pool.Take(int.Min(taskData.WorkerCount, taskData.TaskCount), out PrepassWorkers);
+            Debug.Assert(PrepassWorkers.Length >= 2);
+            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 *(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);
+        }
+    }
+
+    unsafe static BoundingBox4 MultithreadedCentroidPrepass(MultithreadBinnedBuildContext* context, Buffer<BoundingBox4> 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)
+        {
+            //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;
+    }
+
+    struct BinSubtreesWorkerContext
+    {
+        public Buffer<BoundingBox4> BinBoundingBoxes;
+        public Buffer<BoundingBox4> BinCentroidBoundingBoxes;
+        public Buffer<int> BinLeafCounts;
+    }
+    unsafe struct BinSubtreesTaskContext
+    {
+        public SharedTaskData TaskData;
+        /// <summary>
+        /// 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.
+        /// </summary>
+        public Buffer<BinSubtreesWorkerContext> BinSubtreesWorkers;
+        /// <summary>
+        /// 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.
+        /// </summary>
+        public Buffer<bool> WorkerHelpedWithBinning;
+
+        /// <summary>
+        /// Buffer containing all subtrees in this node.
+        /// </summary>
+        public Buffer<NodeChild> Subtrees;
+
+        /// <summary>
+        /// Stores the bin indices of all subtrees in the node.
+        /// </summary>
+        public Buffer<byte> BinIndices;
+
+        public int BinCount;
+        public bool UseX, UseY;
+        public Vector128<int> PermuteMask;
+        public int AxisIndex;
+        public Vector4 CentroidBoundsMin;
+        public Vector4 OffsetToBinIndex;
+        public Vector4 MaximumBinIndex;
+
+        public BinSubtreesTaskContext(BufferPool pool, SharedTaskData taskData, Buffer<NodeChild> subtrees, Buffer<byte> binIndices,
+            int binCount, bool useX, bool useY, Vector128<int> 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<byte>(allocationSize, out var allocation);
+            int start = 0;
+            BinSubtreesWorkers = Suballocate<BinSubtreesWorkerContext>(allocation, ref start, effectiveWorkerCount);
+            for (int i = 0; i < effectiveWorkerCount; ++i)
+            {
+                ref var worker = ref BinSubtreesWorkers[i];
+                worker.BinBoundingBoxes = Suballocate<BoundingBox4>(allocation, ref start, BinCount);
+                worker.BinCentroidBoundingBoxes = Suballocate<BoundingBox4>(allocation, ref start, BinCount);
+                worker.BinLeafCounts = Suballocate<int>(allocation, ref start, BinCount);
+            }
+            WorkerHelpedWithBinning = Suballocate<bool>(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
+    /// <summary>
+    /// Marks a <see cref="BinSubtrees"/> call as requiring the bin indices to be written to the binIndices buffer.
+    /// </summary>
+    private struct DoWriteBinIndices { }
+    /// <summary>
+    /// Marks a <see cref="BinSubtrees"/> call as not allowing the bin indices to be written to the binIndices buffer.
+    /// </summary>
+    private struct DoNotWriteBinIndices { }
+    [MethodImpl(MethodImplOptions.AggressiveInlining)]
+    static void BinSubtrees<TShouldWriteBinIndices>(Vector4 centroidBoundsMin,
+        bool useX, bool useY, Vector128<int> permuteMask, int axisIndex, Vector4 offsetToBinIndex, Vector4 maximumBinIndex,
+        Buffer<NodeChild> subtrees, Buffer<BoundingBox4> binBoundingBoxes, Buffer<BoundingBox4> binCentroidBoundingBoxes, Buffer<int> binLeafCounts, Buffer<byte> 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<NodeChild, BoundingBox4>(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)
+        {
+            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);
+                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);
+        //We always write bin indices, because threading always has a bufferpool available to allocate bin indices from.
+        Debug.Assert(context.BinIndices.Allocated);
+        BinSubtrees<DoWriteBinIndices>(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<int> permuteMask, int axisIndex, Vector4 offsetToBinIndex, Vector4 maximumBinIndex,
+        Buffer<NodeChild> subtrees, Buffer<byte> 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)
+        {
+            //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 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;
+                }
+            }
+        }
+
+        //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);
+    }
+
+    [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;
+
+        /// <summary>
+        /// Buffer containing all subtrees in this node.
+        /// </summary>
+        public Buffer<NodeChild> Subtrees;
+        /// <summary>
+        /// Buffer that will contain the partitioned subtrees pulled from <see cref="Subtrees"/>.
+        /// </summary>
+        public Buffer<NodeChild> SubtreesNext;
+        /// <summary>
+        /// Buffer containing bin indices for all subtrees in the node (encoded with one byte per subtree).
+        /// </summary>
+        public Buffer<byte> BinIndices;
+        public int BinSplitIndex;
+
+        public PartitionCounters Counters;
+
+        public PartitionTaskContext(SharedTaskData taskData, Buffer<NodeChild> subtrees, Buffer<NodeChild> subtreesNext, Buffer<byte> binIndices, int binSplitIndex)
+        {
+            TaskData = taskData;
+            Subtrees = subtrees;
+            SubtreesNext = subtreesNext;
+            BinIndices = binIndices;
+            BinSplitIndex = binSplitIndex;
+
+            Counters = new PartitionCounters();
+        }
+    }
+
+    unsafe static void PartitionSubtreesWorker(long taskId, void* untypedContext, int workerIndex, IThreadDispatcher dispatcher)
+    {
+        ref var context = ref *(PartitionTaskContext*)untypedContext;
+        Buffer<byte> 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<BoundingBox4>();
+        var subtrees = context.Subtrees;
+        var subtreesNext = context.SubtreesNext;
+        var splitIndexBundle = new Vector<byte>((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);
+
+            int scalarLoopStartIndex;
+            if (Vector<byte>.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<ushort> localCountABundle = Vector<ushort>.Zero;
+                scalarLoopStartIndex = (countInBatch / Vector<byte>.Count) * Vector<byte>.Count;
+                for (int indexInBatch = 0; indexInBatch < scalarLoopStartIndex; indexInBatch += Vector<byte>.Count)
+                {
+                    var subtreeIndex = indexInBatch + batchStart;
+                    var binIndicesBundle = *(Vector<byte>*)(binIndices.Memory + subtreeIndex);
+                    var belongsToABundle = Vector.LessThan(binIndicesBundle, splitIndexBundle);
+                    *(Vector<byte>*)(slotBelongsToA + indexInBatch) = belongsToABundle;
+                    var increment = Vector.BitwiseAnd(belongsToABundle, Vector<byte>.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];
+            }
+
+        }
+    }
+
+    unsafe static (int subtreeCountA, int subtreeCountB) MultithreadedPartition(MultithreadBinnedBuildContext* context,
+        Buffer<NodeChild> subtrees, Buffer<NodeChild> subtreesNext, Buffer<byte> 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<TLeaves, TThreading>
+        where TLeaves : unmanaged where TThreading : unmanaged, IBinnedBuilderThreading
+    {
+        public Context<TLeaves, TThreading>* 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<TLeaves, TThreading>(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<TLeaves, TThreading>*)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<TLeaves, TThreading>(
+        Buffer<NodeChild> subtrees, Buffer<Node> nodes, Buffer<Metanode> metanodes, int nodeIndex, int parentNodeIndex, int childIndexInParent,
+        Context<TLeaves, TThreading>* 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<BoundingBox4>();
+        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<TLeaves, TThreading>(Buffer<NodeChild> subtrees, Buffer<BoundingBox4> boundingBoxes, Buffer<Node> nodes, Buffer<Metanode> metanodes,
+        bool usePongBuffer, int subtreeRegionStartIndex, int nodeIndex, int subtreeCount, int parentNodeIndex, int childIndexInParent,
+        BoundingBox4 centroidBounds, Context<TLeaves, TThreading>* 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);
+    }
+
+
+    static unsafe void HandleMicrosweepDegeneracy<TLeaves, TThreading>(ref TLeaves leaves,
+        Buffer<NodeChild> subtrees, Buffer<Node> nodes, Buffer<Metanode> metanodes, int nodeIndex, int parentNodeIndex, int childIndexInParent, Vector4 centroidMin, Vector4 centroidMax, Context<TLeaves, TThreading>* 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 BinnedBuildNode<TLeaves, TThreading>(
+        bool usePongBuffer, int subtreeRegionStartIndex, int nodeIndex, int subtreeCount, int parentNodeIndex, int childIndexInParent,
+        BoundingBox4 centroidBounds, Context<TLeaves, TThreading>* 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<BoundingBox4>();
+        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))
+            {
+                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)
+            {
+                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;
+        }
+
+        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 offsetToBinIndex = new Vector4(binCount) / centroidSpan;
+        //Avoid letting NaNs into the offsetToBinIndex scale.
+        offsetToBinIndex = Vector128.ConditionalSelect(axisIsDegenerate, Vector128<float>.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)
+        {
+            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)
+            {
+                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,
+            //no bufferpool provided,
+            //tree size too large for stack allocation.
+            if (subtreeBinIndices.Allocated)
+                BinSubtrees<DoWriteBinIndices>(centroidBounds.Min, useX, useY, permuteMask, axisIndex, offsetToBinIndex, maximumBinIndex, subtrees, binBoundingBoxes, binCentroidBoundingBoxes, binLeafCounts, subtreeBinIndices);
+            else
+                BinSubtrees<DoNotWriteBinIndices>(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, 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)
+                {
+                    var targetIndex = subtreeBinIndices[i] >= splitIndex ? subtreeCount - ++subtreeCountB : subtreeCountA++;
+                    subtreesNext[targetIndex] = subtrees[i];
+                }
+            }
+            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)
+            {
+                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;
+                }
+            }
+        }
+        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<TThreading, MultithreadBinnedBuildContext>(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<TLeaves, TThreading> 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<TLeaves, TThreading>, &nodePushContext, (long)(subtreeRegionStartIndex + subtreeCountA) | ((long)subtreeCountB << 32) | (usePongBuffer ? 1L << 63 : 0));
+            nodeBContinuation = threading.TaskStack->AllocateContinuationAndPush(new Span<Task>(&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<TThreading, MultithreadBinnedBuildContext>(ref context->Threading).TaskStack->WaitForCompletion(nodeBContinuation, workerIndex, dispatcher);
+        }
+    }
+
+    /// <summary>
+    /// Runs a binned build across the input <see cref="NodeChild"/> buffer.
+    /// </summary>
+    /// <param name="subtrees">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.</param>
+    /// <param name="subtreesPong">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.</param>
+    /// <param name="nodes">Buffer holding the nodes created by the build process.<para/>
+    /// Nodes are created in a depth first ordering with respect to the input buffer.</param>
+    /// <param name="metanodes">Buffer holding the metanodes created by the build process.<para/>
+    /// 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.</param>
+    /// <param name="leaves">Buffer holding the leaf references created by the build process.<para/>
+    /// The indices written by the build process are those defined in the inputs; any <see cref="NodeChild.Index"/> that is negative is encoded according to <see cref="Tree.Encode(int)"/> and points into the leaf buffer.
+    /// If a default-valued (unallocated) buffer is passed in, the binned builder will ignore leaves.</param>
+    /// <param name="binIndices">Buffer to be used for caching bin indices during execution. If subtreesPong is defined, binIndices must also be defined, and vice versa.</param>
+    /// <param name="dispatcher">Thread dispatcher used to accelerate the build process.</param>
+    /// <param name="taskStackPointer">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.</param>
+    /// <param name="workerIndex">Index of the current worker.</param>
+    /// <param name="workerCount">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.</param>
+    /// <param name="targetTaskCount">Number of tasks to try to use in the builder.</param>
+    /// <param name="pool">Buffer pool used to preallocate temporary resources for building.</param>
+    /// <param name="minimumBinCount">Minimum number of bins the builder should use per node.</param>
+    /// <param name="maximumBinCount">Maximum number of bins the builder should use per node. Must be no higher than 255.</param>
+    /// <param name="leafToBinMultiplier">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.</param>
+    /// <param name="microsweepThreshold">Threshold at or under which the binned builder resorts to local counting sort sweeps.</param>
+    /// <param name="deterministic">Whether to force determinism at a slightly higher cost when using internally multithreaded execution.<para/>
+    /// If the build is single threaded, it is already deterministic and this flag has no effect.</param>
+    static unsafe void BinnedBuilderInternal(Buffer<NodeChild> subtrees, Buffer<NodeChild> subtreesPong, Buffer<Node> nodes, Buffer<Metanode> metanodes, Buffer<Leaf> leaves, Buffer<byte> 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<byte>(binBoundsMemoryAllocation, allocatedByteCount);
+
+            var threading = new SingleThreaded(binBoundsMemory, allocatedBinCount);
+            if (leaves.Allocated)
+            {
+                var context = new Context<Buffer<Leaf>, SingleThreaded>(
+                    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<LeavesHandledInPostPass, SingleThreaded>(
+                    minimumBinCount, maximumBinCount, leafToBinMultiplier, microsweepThreshold, deterministic,
+                     subtrees, default, default, nodes, metanodes, binIndices, threading);
+                BinnedBuildNode(false, 0, 0, subtreeCount, -1, -1, default, &context, workerIndex, null);
+            }
+        }
+        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<byte>(allocatedBinCount * workerCount * (sizeof(BoundingBox4) * 4 + sizeof(int)), out var workerBinsAllocation);
+
+            BinnedBuildWorkerContext* workerContextsPointer = stackalloc BinnedBuildWorkerContext[workerCount];
+            var workerContexts = new Buffer<BinnedBuildWorkerContext>(workerContextsPointer, workerCount);
+
+            int binAllocationStart = 0;
+            for (int i = 0; i < workerCount; ++i)
+            {
+                workerContexts[i] = new BinnedBuildWorkerContext(workerBinsAllocation, ref binAllocationStart, allocatedBinCount);
+            }
+
+            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<Buffer<Leaf>, 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<Buffer<Leaf>>, &context), 0, dispatcher);
+                    TaskStack.DispatchWorkers(dispatcher, taskStackPointer, workerCount);
+                }
+                else
+                {
+                    BinnedBuildNode(false, 0, 0, context.SubtreesPing.Length, -1, -1, default, &context, workerIndex, dispatcher);
+                }
+            }
+            else
+            {
+                var context = new Context<LeavesHandledInPostPass, MultithreadBinnedBuildContext>(
+                    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<LeavesHandledInPostPass>, &context), 0, dispatcher);
+                    TaskStack.DispatchWorkers(dispatcher, taskStackPointer, workerCount);
+                }
+                else
+                {
+                    BinnedBuildNode(false, 0, 0, context.SubtreesPing.Length, -1, -1, default, &context, workerIndex, dispatcher);
+                }
+            }
+
+            if (dispatchInternally)
+                taskStackPointer->Dispose(pool, dispatcher);
+            pool.Return(ref workerBinsAllocation);
+
+        }
+    }
+
+    unsafe static void BinnedBuilderWorkerEntry<TLeaves>(long taskId, void* untypedContext, int workerIndex, IThreadDispatcher dispatcher)
+        where TLeaves : unmanaged
+    {
+        var context = (Context<TLeaves, MultithreadBinnedBuildContext>*)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();
+    }
+
+
+    /// <summary>
+    /// Runs a multithreaded binned build across the subtrees buffer.
+    /// </summary>
+    /// <param name="subtrees">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.</param>
+    /// <param name="nodes">Buffer holding the nodes created by the build process.<para/>
+    /// Nodes are created in a depth first ordering with respect to the input buffer.</param>
+    /// <param name="metanodes">Buffer holding the metanodes created by the build process.<para/>
+    /// 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.</param>
+    /// <param name="leaves">Buffer holding the leaf references created by the build process.<para/>
+    /// The indices written by the build process are those defined in the inputs; any <see cref="NodeChild.Index"/> that is negative is encoded according to <see cref="Tree.Encode(int)"/> and points into the leaf buffer.
+    /// If a default-valued (unallocated) buffer is passed in, the binned builder will ignore leaves.</param>
+    /// <param name="pool">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.
+    /// <param name="dispatcher">Dispatcher used to multithread the execution of the build. If the dispatcher is not null, pool must also not be null.</param>
+    /// <param name="taskStackPointer">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.</param>
+    /// <para/>A pool must be provided if a thread dispatcher is given.</param>
+    /// <param name="workerIndex">Index of the currently executing worker. If not running within a dispatch, 0 is valid.</param>
+    /// <param name="workerCount">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.</param>
+    /// <param name="targetTaskCount">Number of tasks to try to use in the builder. If negative, the dispatcher's thread count will be used.</param>
+    /// <param name="maximumSubtreeStackAllocationCount">Maximum number of subtrees to try putting on the stack for the binned builder's pong buffers.<para/>
+    /// Subtree counts larger than this threshold will either resort to a buffer pool allocation (if available) or slower in-place partition operations.</param>
+    /// <param name="minimumBinCount">Minimum number of bins the builder should use per node.</param>
+    /// <param name="maximumBinCount">Maximum number of bins the builder should use per node.</param>
+    /// <param name="leafToBinMultiplier">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.</param>
+    /// <param name="microsweepThreshold">Threshold at or under which the binned builder resorts to local counting sort sweeps.</param>
+    /// <param name="deterministic">Whether to force determinism at a slightly higher cost when using internally multithreaded execution.<para/>
+    /// If the build is single threaded, it is already deterministic and this flag has no effect.</param>
+    public static unsafe void BinnedBuild(Buffer<NodeChild> subtrees, Buffer<Node> nodes, Buffer<Metanode> metanodes, Buffer<Leaf> 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)
+        {
+            //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<NodeChild> subtreesPong;
+        Buffer<byte> binIndices;
+        bool requiresReturn = false;
+        if (subtrees.Length <= maximumSubtreeStackAllocationCount)
+        {
+            var subtreesPongMemory = stackalloc NodeChild[subtrees.Length];
+            subtreesPong = new Buffer<NodeChild>(subtreesPongMemory, subtrees.Length);
+            var binIndicesMemory = stackalloc byte[subtrees.Length];
+            binIndices = new Buffer<byte>(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);
+        }
+    }
+
+    /// <summary>
+    /// Runs a binned build across the subtrees buffer.
+    /// </summary>
+    /// <param name="subtrees">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.</param>
+    /// <param name="pool">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.
+    /// <para/>A pool must be provided if a thread dispatcher is given.</param>
+    /// <param name="dispatcher">Dispatcher used to multithread the execution of the build. If the dispatcher is not null, pool must also not be null.</param>
+    /// <param name="taskStackPointer">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.</param> 
+    /// <param name="workerIndex">Index of the currently executing worker. If not running within a dispatch, 0 is valid.</param>
+    /// <param name="workerCount">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.</param>
+    /// <param name="targetTaskCount">Number of tasks to try to use in the builder. If negative, the dispatcher's thread count will be used.</param>
+    /// <param name="maximumSubtreeStackAllocationCount">Maximum number of subtrees to try putting on the stack for the binned builder's pong buffers.<para/>
+    /// Subtree counts larger than this threshold will either resort to a buffer pool allocation (if available) or slower in-place partition operations.</param>
+    /// <param name="minimumBinCount">Minimum number of bins the builder should use per node.</param>
+    /// <param name="maximumBinCount">Maximum number of bins the builder should use per node.</param>
+    /// <param name="leafToBinMultiplier">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.</param>
+    /// <param name="microsweepThreshold">Threshold at or under which the binned builder resorts to local counting sort sweeps.</param>
+    /// <param name="deterministic">Whether to force determinism at a slightly higher cost when using internally multithreaded execution.<para/>
+    /// If the build is single threaded, it is already deterministic and this flag has no effect.</param>
+    public unsafe void BinnedBuild(Buffer<NodeChild> 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);
+    }
+}
diff --git a/BepuPhysics/Trees/Tree_BinnedRefine.cs b/BepuPhysics/Trees/Tree_BinnedRefine.cs
index 9e0411cf9..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;
 
@@ -78,9 +77,8 @@ partial struct Tree
             var toReturn = Memory + memoryAllocated;
             memoryAllocated = newSize;
             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<byte> 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.
@@ -513,7 +511,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..d8362f4c3 100644
--- a/BepuPhysics/Trees/Tree_CacheOptimizer.cs
+++ b/BepuPhysics/Trees/Tree_CacheOptimizer.cs
@@ -1,614 +1,199 @@
-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
+    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)
-            {
-                //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;
-
-
-            //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)
-                {
-                    Metanodes[child.Index].Parent = indexA;
-                }
-                else
-                {
-                    var leafIndex = Encode(child.Index);
-                    Leaves[leafIndex] = new Leaf(indexA, 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)
-                {
-                    Metanodes[child.Index].Parent = indexB;
-                }
-                else
-                {
-                    var leafIndex = Encode(child.Index);
-                    Leaves[leafIndex] = new Leaf(indexB, i);
-                }
-            }
+        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);
 
-        [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        unsafe bool TryLock(ref int nodeIndex)
+        if (metaA.Parent == indexA)
         {
-            //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;
-                }
-            }
+            //The original B's parent was A.
+            //That parent has moved.
+            metaA.Parent = indexB;
         }
-
-        unsafe bool TrySwapNodeWithTargetThreadSafe(int swapperIndex, int swapperParentIndex, int swapTargetIndex)
+        else if (metaB.Parent == indexB)
         {
-            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;
+            //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;
 
 
-        public unsafe bool TryLockSwapTargetThreadSafe(ref int swapTargetIndex, int swapperIndex, int swapperParentIndex)
+        //Update the parent pointers of the children.
+        ref var children = ref a.A;
+        for (int i = 0; i < 2; ++i)
         {
-            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 child = ref Unsafe.Add(ref children, i);
+            if (child.Index >= 0)
             {
-                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;
+                Metanodes[child.Index].Parent = indexA;
             }
-            return success;
-        }
-
-        /// <summary>
-        /// Attempts to swap two nodes. Aborts without changing memory if the swap is contested by another thread.
-        /// </summary>
-        /// <remarks>Uses Node.RefineFlag as a lock-keeping mechanism. All refine flags should be cleared to 0 before a multithreaded processing stage that performs swaps.</remarks>
-        /// <param name="aIndex">First node of the swap pair.</param>
-        /// <param name="bIndex">Second node of the swap pair.</param>
-        /// <returns>True if the nodes were swapped, false if the swap was contested.</returns>
-        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))
+            else
             {
-                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;
+                var leafIndex = Encode(child.Index);
+                Leaves[leafIndex] = new Leaf(indexA, i);
             }
-            return success;
         }
-
-
-        /// <summary>
-        /// 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.
-        /// </summary>
-        /// <param name="nodeIndex">Node whose children should be optimized.</param>
-        /// <returns>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.</returns>
-        public unsafe bool IncrementalCacheOptimizeThreadSafe(int nodeIndex)
+        children = ref b.A;
+        for (int i = 0; i < 2; ++i)
         {
-            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 child = ref Unsafe.Add(ref children, i);
+            if (child.Index >= 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;
+                Metanodes[child.Index].Parent = indexB;
             }
             else
             {
-                //Failed parent lock.
-                success = false;
+                var leafIndex = Encode(child.Index);
+                Leaves[leafIndex] = new Leaf(indexB, i);
             }
-            return success;
         }
 
-        public unsafe void IncrementalCacheOptimize(int nodeIndex)
+    }
+
+    /// <summary>
+    /// Computes the index where the given node would be located if the tree were in depth first traversal order.
+    /// </summary>
+    /// <param name="nodeIndex">Node index to find the location for in a depth first traversal order.</param>
+    /// <returns>Target index of the node if the tree were in depth first traversal order.</returns>
+    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)
         {
-            if (leafCount <= 2)
+            ref var metanode = ref Metanodes[chainedNodeIndex];
+            var parent = metanode.Parent;
+            if (parent < 0)
+                break;
+            chainedNodeIndex = parent;
+            ++leftNodeCount;
+            if (metanode.IndexInParent == 1)
             {
-                //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;
+                leftNodeCount += Nodes[parent].A.LeafCount - 1;
             }
+        }
+        return leftNodeCount;
+    }
 
-            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)
+    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)
+        {
+            ref var child = ref Unsafe.Add(ref children, i);
+            if (child.Index >= 0)
             {
-                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;
-                }
+                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);
             }
         }
+    }
 
-
-
-        unsafe void CacheOptimize(int nodeIndex, ref int nextIndex)
+    /// <summary>
+    /// 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.
+    /// </summary>
+    /// <param name="nodeIndex">Node to begin the optimization process at.</param>
+    public void CacheOptimize(int nodeIndex)
+    {
+        if (LeafCount <= 2)
         {
-            ref var node = ref Nodes[nodeIndex];
-            ref var children = ref node.A;
-            for (int i = 0; i < 2; ++i)
-            {
-                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);
-                }
-            }
+            //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);
+    }
 
-        /// <summary>
-        /// 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.
-        /// </summary>
-        /// <param name="nodeIndex">Node to begin the optimization process at.</param>
-        public unsafe void CacheOptimize(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;
-            }
-            var targetIndex = nodeIndex + 1;
+    private 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);
+    }
 
-            CacheOptimize(nodeIndex, ref targetIndex);
-        }
+    /// <summary>
+    /// Starts a cache optimization process at the target node index and the nodeOptimizationCount closest nodes in the tree.
+    /// </summary>
+    /// <param name="nodeIndex">Node index to start the optimization process at.</param>
+    /// <param name="nodeOptimizationCount">Number of nodes to move.</param>
+    /// <remarks>This optimizer will move the targeted node index to the globally optimal location if necessary.</remarks>
+    public 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);
     }
 
+    /// <summary>
+    /// 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.
+    /// </summary>
+    /// <param name="startingNodeIndex">Node index to start the optimization at.</param>
+    /// <param name="targetCount">Number of nodes to try to optimize.</param>
+    /// <returns>Number of nodes optimized.</returns>
+    public int CacheOptimizeRegion(int startingNodeIndex, int targetCount)
+    {
+        if (LeafCount <= 2)
+            return 0;
+        var targetNodeIndex = ComputeCacheOptimalLocation(startingNodeIndex);
+        if (startingNodeIndex != targetNodeIndex)
+            SwapNodes(targetNodeIndex, startingNodeIndex);
+
+        ref var startNode = ref Nodes[targetNodeIndex];
+        //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);
+            if (node.B.Index >= 0 && node.B.Index != targetLocationB)
+                SwapNodes(node.B.Index, targetLocationB);
+        }
+        return nodeCount;
+    }
 }
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<int> subtrees, ref QuickQueue<int> 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<int> subtrees, ref QuickQueue<int> 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_Diagnostics.cs b/BepuPhysics/Trees/Tree_Diagnostics.cs
index a4a072cc7..035de97cb 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
@@ -17,14 +13,14 @@ 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];
             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 +30,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;
@@ -63,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];
@@ -79,7 +75,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 +84,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 +99,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 +107,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.");
@@ -129,26 +125,26 @@ readonly unsafe void Validate(int nodeIndex, int expectedParentIndex, int expect
             }
         }
 
-        readonly unsafe void ValidateLeafNodeIndices()
+        readonly 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}.");
                 }
             }
         }
 
-        readonly unsafe void ValidateLeaves()
+        readonly 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)
                 {
@@ -157,21 +153,21 @@ readonly unsafe void ValidateLeaves()
             }
         }
 
-        public readonly unsafe void Validate()
+        public readonly 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,19 +175,19 @@ 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();
 
         }
 
-        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;
             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);
@@ -205,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;
@@ -220,7 +216,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);
@@ -248,16 +244,16 @@ 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)
+            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..d1106ef93 100644
--- a/BepuPhysics/Trees/Tree_IntertreeQueries.cs
+++ b/BepuPhysics/Trees/Tree_IntertreeQueries.cs
@@ -1,15 +1,11 @@
 using BepuUtilities;
-using System.Collections.Generic;
-using System.Diagnostics;
-using System.Numerics;
 using System.Runtime.CompilerServices;
 
 namespace BepuPhysics.Trees
 {
     partial struct Tree
     {
-        //TODO: No good reason for recursion. This is holdovers from the prototype.
-        unsafe void DispatchTestForNodeAgainstLeaf<TOverlapHandler>(int leafIndex, ref Vector3 leafMin, ref Vector3 leafMax, int nodeIndex, ref TOverlapHandler results) where TOverlapHandler : IOverlapHandler
+        void DispatchTestForNodeAgainstLeaf<TOverlapHandler>(int leafIndex, ref NodeChild leafChild, int nodeIndex, ref TOverlapHandler results) where TOverlapHandler : IOverlapHandler
         {
             if (nodeIndex < 0)
             {
@@ -17,10 +13,10 @@ unsafe void DispatchTestForNodeAgainstLeaf<TOverlapHandler>(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<TOverlapHandler>(int nodeIndex, int leafIndex, ref Vector3 leafMin, ref Vector3 leafMax, ref TOverlapHandler results) where TOverlapHandler : IOverlapHandler
+        private void TestNodeAgainstLeaf<TOverlapHandler>(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 +26,19 @@ private unsafe void TestNodeAgainstLeaf<TOverlapHandler>(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<TOverlapHandler>(int leafIndex, ref Vector3 leafMin, ref Vector3 leafMax, int nodeIndex, ref Tree treeB, ref TOverlapHandler results) where TOverlapHandler : IOverlapHandler
+        void DispatchTestForLeafAgainstNode<TOverlapHandler>(int leafIndex, ref NodeChild leafChild, int nodeIndex, ref Tree treeB, ref TOverlapHandler results) where TOverlapHandler : IOverlapHandler
         {
             if (nodeIndex < 0)
             {
@@ -50,10 +46,11 @@ unsafe void DispatchTestForLeafAgainstNode<TOverlapHandler>(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<TOverlapHandler>(int leafIndex, ref Vector3 leafMin, ref Vector3 leafMax, int nodeIndex, ref Tree treeB, ref TOverlapHandler results)
+
+        void TestLeafAgainstNode<TOverlapHandler>(int leafIndex, ref NodeChild leafChild, int nodeIndex, ref Tree treeB, ref TOverlapHandler results)
             where TOverlapHandler : IOverlapHandler
         {
             ref var node = ref treeB.Nodes[nodeIndex];
@@ -64,20 +61,20 @@ unsafe void TestLeafAgainstNode<TOverlapHandler>(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);
             }
         }
 
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        unsafe void DispatchTestForNodes<TOverlapHandler>(ref NodeChild a, ref NodeChild b, ref Tree treeB, ref TOverlapHandler results) where TOverlapHandler : IOverlapHandler
+        void DispatchTestForNodes<TOverlapHandler>(ref NodeChild a, ref NodeChild b, ref Tree treeB, ref TOverlapHandler results) where TOverlapHandler : IOverlapHandler
         {
             if (a.Index >= 0)
             {
@@ -88,13 +85,13 @@ unsafe void DispatchTestForNodes<TOverlapHandler>(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
             {
@@ -103,16 +100,16 @@ unsafe void DispatchTestForNodes<TOverlapHandler>(ref NodeChild a, ref NodeChild
             }
         }
 
-        private unsafe void GetOverlapsBetweenDifferentNodes<TOverlapHandler>(ref Node a, ref Node b, ref Tree treeB, ref TOverlapHandler results) where TOverlapHandler : IOverlapHandler
+        private void GetOverlapsBetweenDifferentNodes<TOverlapHandler>(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;
             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)
             {
@@ -132,23 +129,23 @@ private unsafe void GetOverlapsBetweenDifferentNodes<TOverlapHandler>(ref Node a
             }
         }
 
-
-        public unsafe void GetOverlaps<TOverlapHandler>(ref Tree treeB, ref TOverlapHandler overlapHandler) where TOverlapHandler : struct, IOverlapHandler
+        /// <summary>
+        /// Gets pairs of leaf indices with bounding boxes which overlap.
+        /// </summary>
+        /// <typeparam name="TOverlapHandler">Type of the <see cref="IOverlapHandler"/> implementation to report pairs to.</typeparam>
+        /// <param name="treeB">Tree to test this tree against.</param>
+        /// <param name="overlapHandler">Handler to report pairs to.</param>
+        public void GetOverlaps<TOverlapHandler>(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)
-            {
-                //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)
+            if (LeafCount == 1 && treeB.LeafCount >= 2)
             {
                 //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);
@@ -157,14 +154,15 @@ public unsafe void GetOverlaps<TOverlapHandler>(ref Tree treeB, ref TOverlapHand
                 {
                     DispatchTestForNodes(ref a.A, ref b.B, ref treeB, ref overlapHandler);
                 }
+                return;
             }
-            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];
                 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);
@@ -173,15 +171,20 @@ public unsafe void GetOverlaps<TOverlapHandler>(ref Tree treeB, ref TOverlapHand
                 {
                     DispatchTestForNodes(ref a.B, ref b.A, ref treeB, ref overlapHandler);
                 }
+                return;
             }
-            else
+            if (LeafCount == 1 && treeB.LeafCount == 1)
             {
-                Debug.Assert(leafCount == 1 && treeB.leafCount == 1);
-                if (Intersects(Nodes[0].A, treeB.Nodes[0].A))
+                //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;
             }
+            //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_IntertreeQueriesMT.cs b/BepuPhysics/Trees/Tree_IntertreeQueriesMT.cs
index c5fe6b7bd..ba9732b39 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;
 
@@ -40,9 +38,9 @@ public MultithreadedIntertreeTest(BufferPool pool)
             /// </summary>
             /// <param name="overlapHandlers">Callbacks used to handle individual overlaps detected by the self test.</param>
             /// <param name="threadCount">Number of threads to prepare jobs for.</param>
-            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)
+                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. 
@@ -50,28 +48,28 @@ 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 = 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);
+                leafThreshold = (int)((treeA.LeafCount + treeB.LeafCount) / targetJobCount);
                 jobs = new QuickList<Job>((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];
                     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]);
@@ -81,13 +79,13 @@ 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];
                     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]);
@@ -99,8 +97,8 @@ 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))
+                    Debug.Assert(treeA.LeafCount == 1 && treeB.LeafCount == 1);
+                    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]);
                     }
@@ -118,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)
@@ -134,7 +132,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 +141,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.
@@ -153,7 +151,7 @@ public unsafe void ExecuteJob(int jobIndex, int workerIndex)
             /// Executes a single worker of the multithreaded self test.
             /// </summary>
             /// <param name="workerIndex">Index of the worker executing this set of tests.</param>
-            public unsafe void PairTest(int workerIndex)
+            public void PairTest(int workerIndex)
             {
                 Debug.Assert(workerIndex >= 0 && workerIndex < OverlapHandlers.Length);
                 int nextNodePairIndex;
@@ -164,7 +162,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)
+            void DispatchTestForLeaf(ref Tree nodeOwner, int leafIndex, ref NodeChild leafChild, int nodeIndex, int nodeLeafCount, ref TOverlapHandler results)
             {
                 if (nodeIndex < 0)
                 {
@@ -185,11 +183,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)
+            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,20 +198,20 @@ 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);
                 }
             }
 
             [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)
                 {
@@ -228,13 +226,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
                 {
@@ -243,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.
 
@@ -251,10 +249,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_MultithreadedRefitRefine.cs b/BepuPhysics/Trees/Tree_MultithreadedRefitRefine.cs
index 66ef6d835..7f119c1f5 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;
@@ -15,12 +14,12 @@ partial struct Tree
         /// <summary>
         /// Caches input and output for the multithreaded execution of a tree's refit and refinement operations.
         /// </summary>
-        public class RefitAndRefineMultithreadedContext
+        public unsafe class RefitAndRefineMultithreadedContext
         {
             Tree Tree;
 
             int RefitNodeIndex;
-            QuickList<int> RefitNodes;
+            public QuickList<int> RefitNodes;
             float RefitCostChange;
 
             int RefinementLeafCountThreshold;
@@ -28,31 +27,27 @@ public class RefitAndRefineMultithreadedContext
             Action<int> RefitAndMarkAction;
 
             int RefineIndex;
-            QuickList<int> RefinementTargets;
-            int MaximumSubtrees;
+            public QuickList<int> RefinementTargets;
+            public int MaximumSubtrees;
             Action<int> RefineAction;
 
-            QuickList<int> CacheOptimizeStarts;
-            int PerWorkerCacheOptimizeCount;
-            Action<int> CacheOptimizeAction;
-
             IThreadDispatcher threadDispatcher;
 
             public RefitAndRefineMultithreadedContext()
             {
-                RefitAndMarkAction = RefitAndMark;
-                RefineAction = Refine;
-                CacheOptimizeAction = CacheOptimize;
+                RefitAndMarkAction = RefitAndMarkForWorker;
+                RefineAction = RefineForWorker;
             }
 
-            public unsafe void RefitAndRefine(ref Tree tree, BufferPool pool, IThreadDispatcher threadDispatcher, int frameIndex,
-                float refineAggressivenessScale = 1, float cacheOptimizeAggressivenessScale = 1)
+
+            public 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/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.
+                    RefitNodes = default;
                     return;
                 }
                 this.threadDispatcher = threadDispatcher;
@@ -66,17 +61,28 @@ public unsafe void RefitAndRefine(ref Tree tree, BufferPool pool, IThreadDispatc
                 //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<int>(estimatedRefinementCandidateCount, threadDispatcher.GetThreadMemoryPool(i));
+                    RefinementCandidates[i] = new QuickList<int>(estimatedRefinementCandidateCount, threadDispatcher.WorkerPools[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],
-                    pool, threadDispatcher.GetThreadMemoryPool(0));
+                    pool, threadDispatcher.WorkerPools[0]);
 
                 RefitNodeIndex = -1;
-                threadDispatcher.DispatchWorkers(RefitAndMarkAction);
+            }
+
+            public void CreateRefinementJobs(BufferPool pool, 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.
+                    RefinementTargets = default;
+                    return;
+                }
                 //Condense the set of candidates into a set of targets.
                 int refinementCandidatesCount = 0;
                 for (int i = 0; i < threadDispatcher.ThreadCount; ++i)
@@ -117,8 +123,15 @@ public unsafe void RefitAndRefine(ref Tree tree, BufferPool pool, IThreadDispatc
                     Tree.Metanodes[0].RefineFlag = 1;
                 }
                 RefineIndex = -1;
+            }
 
-                threadDispatcher.DispatchWorkers(RefineAction);
+            public void CleanUpForRefitAndRefine(BufferPool pool)
+            {
+                if (Tree.LeafCount <= 2)
+                {
+                    //If there are 2 or less leaves, then refit/refine doesn't do anything at all.
+                    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)
@@ -126,53 +139,29 @@ 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 * 2;
-                PerWorkerCacheOptimizeCount = cacheOptimizeCount / cacheOptimizationTasks;
-                var startIndex = (int)(((long)frameIndex * PerWorkerCacheOptimizeCount) % Tree.nodeCount);
-                CacheOptimizeStarts = new QuickList<int>(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);
-
                 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);
                 RefinementTargets.Dispose(pool);
-                CacheOptimizeStarts.Dispose(pool);
                 Tree = default;
                 this.threadDispatcher = null;
             }
 
-            unsafe void CollectNodesForMultithreadedRefit(int nodeIndex,
+            public 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, frameIndex, refineAggressivenessScale);
+                threadDispatcher.DispatchWorkers(RefineAction, RefinementTargets.Count);
+                CleanUpForRefitAndRefine(pool);
+            }
+
+            void CollectNodesForMultithreadedRefit(int nodeIndex,
                 int multithreadingLeafCountThreshold, ref QuickList<int> refitAndMarkTargets,
                 int refinementLeafCountThreshold, ref QuickList<int> refinementCandidates, BufferPool pool, BufferPool threadPool)
             {
@@ -180,7 +169,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);
@@ -211,132 +200,145 @@ unsafe void CollectNodesForMultithreadedRefit(int nodeIndex,
                 }
             }
 
-            unsafe void RefitAndMark(int workerIndex)
+            public 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 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.WorkerPools[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 void ExecuteRefineJob(ref QuickList<int> subtreeReferences, ref QuickList<int> treeletInternalNodes, ref BinnedResources resources, BufferPool threadPool, int refineIndex)
             {
-                var threadPool = threadDispatcher.GetThreadMemoryPool(workerIndex);
-                var subtreeCountEstimate = 1 << SpanHelper.GetContainingPowerOf2(MaximumSubtrees);
+                Tree.BinnedRefine(RefinementTargets[refineIndex], ref subtreeReferences, MaximumSubtrees, ref treeletInternalNodes, ref resources, threadPool);
+                subtreeReferences.Count = 0;
+                treeletInternalNodes.Count = 0;
+            }
+
+            public void RefineForWorker(int workerIndex)
+            {
+                if (RefinementTargets.Count == 0)
+                    return;
+                var threadPool = threadDispatcher.WorkerPools[workerIndex];
+                var subtreeCountEstimate = (int)BitOperations.RoundUpToPowerOf2((uint)MaximumSubtrees);
                 var subtreeReferences = new QuickList<int>(subtreeCountEstimate, threadPool);
                 var treeletInternalNodes = new QuickList<int>(subtreeCountEstimate, threadPool);
 
@@ -345,9 +347,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);
@@ -356,22 +356,9 @@ 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<int> refinementTargets)
+        void CheckForRefinementOverlaps(int nodeIndex, ref QuickList<int> refinementTargets)
         {
             ref var node = ref Nodes[nodeIndex];
             ref var children = ref node.A;
diff --git a/BepuPhysics/Trees/Tree_RayCast.cs b/BepuPhysics/Trees/Tree_RayCast.cs
index e442d6151..ce886befa 100644
--- a/BepuPhysics/Trees/Tree_RayCast.cs
+++ b/BepuPhysics/Trees/Tree_RayCast.cs
@@ -1,16 +1,16 @@
-using System;
-using System.Collections.Generic;
+using BepuUtilities.Memory;
 using System.Diagnostics;
 using System.Numerics;
-using System.Runtime.CompilerServices;
-using System.Text;
 
 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)
+        //Working around https://github.com/dotnet/runtime/issues/95043:
+        //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)
         {
             var t0 = min * ray->InverseDirection - ray->OriginOverDirection;
             var t1 = max * ray->InverseDirection - ray->OriginOverDirection;
@@ -24,10 +24,10 @@ public unsafe static bool Intersects(in Vector3 min, in Vector3 max, TreeRay* ra
         }
 
 
-        internal readonly unsafe void RayCast<TLeafTester>(int nodeIndex, TreeRay* treeRay, RayData* rayData, int* stack, ref TLeafTester leafTester) where TLeafTester : IRayLeafTester
+        internal readonly unsafe void RayCast<TLeafTester>(int nodeIndex, TreeRay* treeRay, RayData* rayData, Buffer<int> 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.");
+            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)
@@ -36,10 +36,10 @@ internal readonly unsafe void RayCast<TLeafTester>(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<TLeafTester>(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<int>(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,42 +90,54 @@ internal readonly unsafe void RayCast<TLeafTester>(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<TLeafTester>(TreeRay* treeRay, RayData* rayData, ref TLeafTester leafTester) where TLeafTester : IRayLeafTester
+        internal readonly unsafe void RayCast<TLeafTester>(TreeRay* treeRay, RayData* rayData, BufferPool pool, 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))
                 {
-                    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<int>(stack, TraversalStackCapacity), pool, ref leafTester);
             }
         }
 
-        public readonly unsafe void RayCast<TLeafTester>(in Vector3 origin, in Vector3 direction, ref float maximumT, ref TLeafTester leafTester, int id = 0) where TLeafTester : IRayLeafTester
+        /// <summary>
+        /// Tests a ray against the tree and invokes the <paramref name="leafTester"/> for each leaf node that the ray intersects.
+        /// </summary>
+        /// <typeparam name="TLeafTester">The type of the <see cref="IRayLeafTester"/> used to process the intersecting leaves.</typeparam>
+        /// <param name="origin">The origin point of the ray.</param>
+        /// <param name="direction">The direction of the ray.</param>
+        /// <param name="maximumT">The maximum parametric distance along the ray to test. This value may be modified by the leaf tester during traversal.</param>
+        /// <param name="pool">The buffer pool used for temporary allocations during the operation. Only used if the tree is pathologically deep; stack memory is used preferentially.</param>
+        /// <param name="leafTester">A reference to the tester that processes the indices of intersecting leaves.</param>
+        /// <param name="id">An optional identifier for the ray that can be used by the leaf tester.</param>
+        public readonly unsafe void RayCast<TLeafTester>(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_Refine2.cs b/BepuPhysics/Trees/Tree_Refine2.cs
new file mode 100644
index 000000000..e390e4ac0
--- /dev/null
+++ b/BepuPhysics/Trees/Tree_Refine2.cs
@@ -0,0 +1,807 @@
+using BepuUtilities;
+using BepuUtilities.Collections;
+using BepuUtilities.Memory;
+using BepuUtilities.TaskScheduling;
+using System;
+using System.Diagnostics;
+using System.Numerics;
+using System.Runtime.CompilerServices;
+using Task = BepuUtilities.TaskScheduling.Task;
+
+namespace BepuPhysics.Trees;
+
+public partial struct Tree
+{
+    const int flagForRootRefinementSubtree = 1 << 30;
+
+    readonly void ReifyRootRefinementNodeChild(ref int index, ref QuickList<int> 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.
+            //NOTE: This means the binned builder *should not touch the metanodes*.
+            ref var childMetanode = ref Metanodes[index];
+            childMetanode.Parent = realNodeIndex;
+            childMetanode.IndexInParent = childIndexInParent;
+        }
+    }
+
+    static void ReifyRootRefinement(int startIndex, int endIndex, QuickList<int> nodeIndices, Buffer<Node> refinementNodes, Tree tree)
+    {
+        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 = nodeIndices[i];
+            ref var refinedNode = ref refinementNodes[i];
+            //Map child indices, and update leaf references.
+            tree.ReifyRootRefinementNodeChild(ref refinedNode.A.Index, ref nodeIndices, realNodeIndex, 0);
+            tree.ReifyRootRefinementNodeChild(ref refinedNode.B.Index, ref nodeIndices, realNodeIndex, 1);
+            tree.Nodes[realNodeIndex] = refinedNode;
+        }
+    }
+
+    readonly void ReifyRootRefinementST(QuickList<int> refinementNodeIndices, Buffer<Node> refinementNodes)
+    {
+        ReifyRootRefinement(0, refinementNodeIndices.Count, refinementNodeIndices, refinementNodes, this);
+    }
+
+    unsafe struct ReifyRefinementContext
+    {
+        public QuickList<int>* RefinementNodeIndices;
+        public Buffer<Node>* 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 *(ReifyRefinementContext*)untypedContext;
+        ReifyRootRefinement(context.StartIndex, context.EndIndex, *context.RefinementNodeIndices, *context.RefinementNodes, *context.Tree);
+    }
+
+    readonly unsafe void ReifyRootRefinementMT(QuickList<int>* refinementNodeIndices, Buffer<Node>* 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<Task> 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(&ReifyRootRefinementTask, contexts + i, i);
+        }
+
+        taskStack->RunTasks(tasks, workerIndex, dispatcher);
+    }
+
+
+    readonly void ReifySubtreeRefinementNodeChild(ref int index, ref QuickList<int> refinementNodeIndices, int realNodeIndex, int childIndexInParent)
+    {
+        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.
+            //NOTE: This means the binned builder *should not touch the metanodes*.
+            ref var childMetanode = ref Metanodes[index];
+            childMetanode.Parent = realNodeIndex;
+            childMetanode.IndexInParent = childIndexInParent;
+        }
+    }
+
+    static void ReifySubtreeRefinement(int startIndex, int endIndex, QuickList<int> nodeIndices, Buffer<Node> refinementNodes, Tree tree)
+    {
+        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 = 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.
+            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<int> refinementNodeIndices, Buffer<Node> 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<int>* refinementNodeIndices, Buffer<Node>* 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<Task> 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<int> 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.
+                FindSubtreeRefinementTargets(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.
+                FindSubtreeRefinementTargets(node.B.Index, leftLeafCount + node.A.LeafCount, subtreeRefinementSize, targetSubtreeRefinementCount, ref startIndex, endIndex, ref refinementTargets);
+            }
+        }
+    }
+    void FindSubtreeRefinementTargets(int subtreeRefinementSize, int targetSubtreeRefinementCount, ref int startIndex, ref QuickList<int> 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 = 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);
+        }
+    }
+
+
+
+    static bool IsNodeChildSubtreeRefinementTarget(Buffer<Vector<int>> 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 search = new Vector<int>(child.Index);
+            for (int i = 0; i < subtreeRefinementBundles.Length; ++i)
+            {
+                if (Vector.EqualsAny(search, subtreeRefinementBundles[i]))
+                    return true;
+            }
+        }
+        return false;
+    }
+
+    /// <summary>
+    /// 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.
+    /// </summary>
+    private static void TryPushChildForRootRefinement(
+         int subtreeRefinementSize, Buffer<Vector<int>> subtreeRefinementRootBundles, int nodeTotalLeafCount, int subtreeBudget, in NodeChild child, ref QuickList<(int nodeIndex, int subtreeBudget)> stack, ref QuickList<NodeChild> 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(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
+            {
+                //Not a subtree refinement, and we know we have budget remaining.
+                stack.AllocateUnsafely() = (child.Index, subtreeBudget);
+            }
+        }
+    }
+
+    unsafe void CollectSubtreesForRootRefinement(int rootRefinementSize, int subtreeRefinementSize, BufferPool pool, in QuickList<int> subtreeRefinementTargets, ref QuickList<int> rootRefinementNodeIndices, ref QuickList<NodeChild> rootRefinementSubtrees)
+    {
+        var rootStack = new QuickList<(int nodeIndex, int subtreeBudget)>(rootRefinementSize, pool);
+        rootStack.AllocateUnsafely() = (0, rootRefinementSize);
+        var subtreeRefinementTargetBundles = new Buffer<Vector<int>>(subtreeRefinementTargets.Span.Memory, BundleIndexing.GetBundleCount(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, subtreeRefinementTargetBundles, nodeTotalLeafCount, bSubtreeBudget, node.B, ref rootStack, ref rootRefinementSubtrees);
+            TryPushChildForRootRefinement(subtreeRefinementSize, subtreeRefinementTargetBundles, nodeTotalLeafCount, aSubtreeBudget, node.A, ref rootStack, ref rootRefinementSubtrees);
+        }
+        rootStack.Dispose(pool);
+    }
+
+    internal struct HeapEntry
+    {
+        public int Index;
+        public float Cost;
+    }
+
+    internal struct BinaryHeap
+    {
+        public Buffer<HeapEntry> Entries;
+        public int Count;
+
+        public BinaryHeap(Buffer<HeapEntry> entries)
+        {
+            Entries = entries;
+            Count = 0;
+        }
+
+        public BinaryHeap(int capacity, BufferPool pool) : this(new Buffer<HeapEntry>(capacity, pool)) { }
+
+        public void Dispose(BufferPool pool)
+        {
+            pool.Return(ref Entries);
+        }
+
+        public 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;
+        }
+
+    }
+
+    /// <summary>
+    /// 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.
+    /// </summary>
+    private void TryPushChildForRootRefinement2(
+         int subtreeRefinementSize, int nodeTotalLeafCount, Buffer<Vector<int>> subtreeRefinementRootBundles, ref NodeChild child, ref BinaryHeap heap, ref QuickList<NodeChild> 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.
+                // 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<NodeChild, BoundingBox4>(ref child)) * child.LeafCount);
+            }
+        }
+    }
+    unsafe void CollectSubtreesForRootRefinementWithPriorityQueue(int rootRefinementSize, int subtreeRefinementSize, BufferPool pool, in QuickList<int> subtreeRefinementTargets, ref QuickList<int> rootRefinementNodeIndices, ref QuickList<NodeChild> 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<HeapEntry>(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<Vector<int>>(subtreeRefinementTargets.Span.Memory, BundleIndexing.GetBundleCount(subtreeRefinementTargets.Count));
+        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<int> subtreeStackBuffer, ref QuickList<int> subtreeRefinementNodeIndices, ref QuickList<NodeChild> subtreeRefinementLeaves)
+    {
+        Debug.Assert(subtreeRefinementLeaves.Count == 0 && subtreeRefinementNodeIndices.Count == 0);
+        var subtreeStack = new QuickList<int>(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;
+        }
+    }
+
+    /// <summary>
+    /// Incrementally refines a subset of the tree by running a binned builder over subtrees.
+    /// </summary>
+    /// <param name="rootRefinementSize">Size of the refinement run on nodes near the root. Nonpositive values will cause the root refinement to be skipped.</param>
+    /// <param name="subtreeRefinementStartIndex">Index used to distribute subtree refinements over multiple executions.</param>
+    /// <param name="subtreeRefinementCount">Number of subtree refinements to execute.</param>
+    /// <param name="subtreeRefinementSize">Target size of subtree refinements. The actual size of refinement will usually be larger or smaller.</param>
+    /// <param name="pool">Pool used for ephemeral allocations during the refinement.</param> 
+    /// <param name="usePriorityQueue">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.</param>
+    /// <remarks>Nodes will not be refit.</remarks>
+    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)
+            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<int>.Count;
+        var subtreeRefinementTargets = new QuickList<int>(subtreeRefinementCapacity, pool);
+        FindSubtreeRefinementTargets(subtreeRefinementSize, subtreeRefinementCount, ref subtreeRefinementStartIndex, ref subtreeRefinementTargets);
+        //Fill the trailing slots in the list with -1 to avoid matches.
+        ((Span<int>)subtreeRefinementTargets.Span)[subtreeRefinementTargets.Count..].Fill(-1);
+
+        var refinementNodesAllocation = new Buffer<Node>(int.Max(rootRefinementSize, subtreeRefinementSize), pool);
+        if (rootRefinementSize > 0) //Skip root refinement if it's zero or negative size.
+        {
+            var rootRefinementSubtrees = new QuickList<NodeChild>(rootRefinementSize, pool);
+            var rootRefinementNodeIndices = new QuickList<int>(rootRefinementSize, pool);
+            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);
+
+            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<int>(subtreeRefinementSize, pool);
+        var subtreeRefinementLeaves = new QuickList<NodeChild>(subtreeRefinementSize, pool);
+        var subtreeStackBuffer = new Buffer<int>(subtreeRefinementSize, pool);
+        for (int i = 0; i < subtreeRefinementTargets.Count; ++i)
+        {
+            //Accumulate nodes and leaves with a prepass.
+            CollectSubtreesForSubtreeRefinement(subtreeRefinementTargets[i], subtreeStackBuffer, ref subtreeRefinementNodeIndices, ref subtreeRefinementLeaves);
+
+            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);
+            ReifySubtreeRefinementST(subtreeRefinementNodeIndices, refinementNodes);
+
+            subtreeRefinementNodeIndices.Count = 0;
+            subtreeRefinementLeaves.Count = 0;
+        }
+
+        subtreeRefinementNodeIndices.Dispose(pool);
+        subtreeRefinementLeaves.Dispose(pool);
+        subtreeRefinementTargets.Dispose(pool);
+        subtreeStackBuffer.Dispose(pool);
+        refinementNodesAllocation.Dispose(pool);
+    }
+
+
+    unsafe struct RefinementContext
+    {
+        public int RootRefinementSize;
+        public int SubtreeRefinementSize;
+        public int TotalLeafCountInSubtrees;
+        public int TargetTaskBudget;
+        public int WorkerCount;
+        public QuickList<int> SubtreeRefinementTargets;
+        public TaskStack* TaskStack;
+        public bool Deterministic;
+        public bool UsePriorityQueue;
+        /// <remarks>
+        /// 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.
+        /// </remarks>
+        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];
+
+        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<NodeChild>(context.RootRefinementSize, pool);
+        var rootRefinementNodeIndices = new QuickList<int>(context.RootRefinementSize, pool);
+
+        //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. 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. 
+        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);
+        var rootRefinementNodes = new Buffer<Node>(rootRefinementNodeIndices.Count, pool);
+        //Passing 'default' for the leaves tells the binned builder to not worry about updating leaves.
+        if (taskCount > 1)
+        {
+            BinnedBuild(rootRefinementSubtrees, rootRefinementNodes, default, default, pool, dispatcher, context.TaskStack, workerIndex, context.WorkerCount, taskCount, deterministic: context.Deterministic);
+            context.Tree.ReifyRootRefinementMT(&rootRefinementNodeIndices, &rootRefinementNodes, taskCount, workerIndex, context.TaskStack, dispatcher);
+        }
+        else
+        {
+            BinnedBuild(rootRefinementSubtrees, rootRefinementNodes, default, default, pool, workerIndex: workerIndex);
+            context.Tree.ReifyRootRefinementST(rootRefinementNodeIndices, rootRefinementNodes);
+        }
+
+        rootRefinementSubtrees.Dispose(pool);
+        rootRefinementNodeIndices.Dispose(pool);
+        rootRefinementNodes.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];
+
+        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<int>(context.SubtreeRefinementSize, pool);
+        var subtreeRefinementLeaves = new QuickList<NodeChild>(context.SubtreeRefinementSize, pool);
+        var subtreeStackBuffer = new Buffer<int>(context.SubtreeRefinementSize, pool);
+
+        //Accumulate nodes and leaves with a prepass.
+        context.Tree.CollectSubtreesForSubtreeRefinement((int)subtreeRefinementTarget, subtreeStackBuffer, ref subtreeRefinementNodeIndices, ref subtreeRefinementLeaves);
+        var refinementNodes = new Buffer<Node>(subtreeRefinementNodeIndices.Count, pool);
+        //Passing 'default' for the leaves tells the binned builder to not worry about updating leaves.
+
+        if (taskCount > 1)
+        {
+            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);
+        }
+
+        refinementNodes.Dispose(pool);
+        subtreeRefinementNodeIndices.Dispose(pool);
+        subtreeRefinementLeaves.Dispose(pool);
+        subtreeStackBuffer.Dispose(pool);
+
+    }
+
+    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)
+            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;
+        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);
+        //We used a vectorized containment test later, so make sure to pad out the refinement target list.
+        var subtreeRefinementCapacity = BundleIndexing.GetBundleCount(subtreeRefinementCount) * Vector<int>.Count;
+        var subtreeRefinementTargets = new QuickList<int>(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<int>)subtreeRefinementTargets.Span)[subtreeRefinementTargets.Count..].Fill(-1);
+
+        //Zero or negative root refine sizes means skip it.
+        var rootRefinementCount = rootRefinementSize > 0 ? 1 : 0;
+        var tasks = new Buffer<Task>(rootRefinementCount + 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,
+            Deterministic = deterministic,
+            UsePriorityQueue = usePriorityQueue,
+            Tree = this
+        };
+        for (int i = 0; i < subtreeRefinementTargets.Count; ++i)
+        {
+            tasks[i] = new Task(&ExecuteSubtreeRefinementTask, &context, subtreeRefinementTargets[i]);
+        }
+        if (rootRefinementSize > 0)
+            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: 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);
+
+        subtreeRefinementTargets.Dispose(pool);
+    }
+
+    /// <summary>
+    /// Incrementally refines a subset of the tree by running a binned builder over subtrees.
+    /// </summary>
+    /// <param name="rootRefinementSize">Size of the refinement run on nodes near the root. Nonpositive values will cause the root refinement to be skipped.</param>
+    /// <param name="subtreeRefinementStartIndex">Index used to distribute subtree refinements over multiple executions.</param>
+    /// <param name="subtreeRefinementCount">Number of subtree refinements to execute.</param>
+    /// <param name="subtreeRefinementSize">Target size of subtree refinements. The actual size of refinement will usually be larger or smaller.</param>
+    /// <param name="pool">Pool used for ephemeral allocations during the refinement.</param>
+    /// <param name="threadDispatcher">Thread dispatcher used during the refinement.</param>
+    /// <param name="deterministic">Whether to force determinism at a slightly higher cost when using internally multithreaded execution for an individual refinement operation.<para/>
+    /// If the refine is single threaded, it is already deterministic and this flag has no effect.</param>
+    /// <param name="usePriorityQueue">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.</param>
+    /// <remarks>Nodes will not be refit.</remarks>
+    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, usePriorityQueue);
+        taskStack.Dispose(pool, threadDispatcher);
+    }
+
+
+    /// <summary>
+    /// Incrementally refines a subset of the tree by running a binned builder over subtrees.
+    /// <para/>Pushes tasks into the provided <see cref="TaskStack"/>. Does not dispatch threads internally; this is intended to be used as a part of a caller-managed dispatch.
+    /// </summary>
+    /// <param name="rootRefinementSize">Size of the refinement run on nodes near the root. Nonpositive values will cause the root refinement to be skipped.</param>
+    /// <param name="subtreeRefinementStartIndex">Index used to distribute subtree refinements over multiple executions.</param>
+    /// <param name="subtreeRefinementCount">Number of subtree refinements to execute.</param>
+    /// <param name="subtreeRefinementSize">Target size of subtree refinements. The actual size of refinement will usually be larger or smaller.</param>
+    /// <param name="pool">Pool used for ephemeral allocations during the refinement.</param>
+    /// <param name="threadDispatcher">Thread dispatcher used during the refinement.</param>
+    /// <param name="deterministic">Whether to force determinism at a slightly higher cost when using internally multithreaded execution for an individual refinement operation.<para/>
+    /// If the refine is single threaded, it is already deterministic and this flag has no effect.</param>
+    /// <param name="taskStack"><see cref="TaskStack"/> that the refine operation will push tasks onto as needed.</param>
+    /// <param name="workerIndex">Index of the worker calling the function.</param>
+    /// <param name="targetTaskCount">Number of tasks the refinement should try to create during execution. If negative, uses <see cref="IThreadDispatcher.ThreadCount"/>.</param>
+    /// <param name="usePriorityQueue">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.</param>
+    /// <remarks>Nodes will not be refit.</remarks>
+    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, usePriorityQueue);
+    }
+}
diff --git a/BepuPhysics/Trees/Tree_RefineCommon.cs b/BepuPhysics/Trees/Tree_RefineCommon.cs
index c215fb4d9..18b8043dd 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
@@ -25,7 +23,7 @@ public SubtreeBinaryHeap(SubtreeHeapEntry* entries)
         }
 
 
-        public unsafe void Insert(ref Node node, ref QuickList<int> subtrees)
+        public void Insert(ref Node node, ref QuickList<int> subtrees)
         {
             ref var children = ref node.A;
             for (int childIndex = 0; childIndex < 2; ++childIndex)
@@ -234,7 +232,7 @@ unsafe void ValidateStaging(Node* stagingNodes, ref QuickList<int> subtreeNodePo
             var internalReferences = new QuickList<int>(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 a60d4cc92..7ff7ffeb5 100644
--- a/BepuPhysics/Trees/Tree_RefinementScheduling.cs
+++ b/BepuPhysics/Trees/Tree_RefinementScheduling.cs
@@ -10,13 +10,12 @@ namespace BepuPhysics.Trees
 {
     partial struct Tree
     {
-
-        unsafe float RefitAndMeasure(ref NodeChild child)
+        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;
@@ -37,7 +36,7 @@ unsafe float RefitAndMeasure(ref NodeChild child)
 
         }
 
-        unsafe float RefitAndMark(ref NodeChild child, int leafCountThreshold, ref QuickList<int> refinementCandidates, BufferPool pool)
+        float RefitAndMark(ref NodeChild child, int leafCountThreshold, ref QuickList<int> 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<int> refinementCandidates, BufferPool pool)
+        float RefitAndMark(int leafCountThreshold, ref QuickList<int> 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<int> refinementC
         }
 
 
-
-
-        unsafe void ValidateRefineFlags(int index)
+        void ValidateRefineFlags(int index)
         {
             ref var metanode = ref Metanodes[index];
             if (metanode.RefineFlag != 0)
@@ -152,10 +149,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 +167,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));
@@ -178,24 +175,10 @@ 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 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<int>(estimatedRefinementCandidateCount, pool);
@@ -250,18 +233,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/BepuPhysics/Trees/Tree_Refit.cs b/BepuPhysics/Trees/Tree_Refit.cs
index 85f13dfde..9e8f9d54f 100644
--- a/BepuPhysics/Trees/Tree_Refit.cs
+++ b/BepuPhysics/Trees/Tree_Refit.cs
@@ -1,6 +1,5 @@
 using BepuUtilities;
 using System.Diagnostics;
-using System.Linq;
 using System.Numerics;
 using System.Runtime.CompilerServices;
 
@@ -12,7 +11,7 @@ partial struct Tree
         /// Refits the bounding box of every parent of the node recursively to the root.
         /// </summary>
         /// <param name="nodeIndex">Node to propagate a node change for.</param>
-        public unsafe 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];
@@ -27,13 +26,12 @@ public unsafe 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.
-        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);
+            Debug.Assert(LeafCount >= 2);
             ref var node = ref Nodes[nodeIndex];
             ref var a = ref node.A;
             if (node.A.Index >= 0)
@@ -47,16 +45,15 @@ 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()
+        /// <summary>
+        /// Updates the bounding boxes of all internal nodes in the tree.
+        /// </summary>
+        public 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_Refit2.cs b/BepuPhysics/Trees/Tree_Refit2.cs
new file mode 100644
index 000000000..5916eca5a
--- /dev/null
+++ b/BepuPhysics/Trees/Tree_Refit2.cs
@@ -0,0 +1,411 @@
+using BepuUtilities;
+using BepuUtilities.Memory;
+using BepuUtilities.TaskScheduling;
+using System;
+using System.Diagnostics;
+
+namespace BepuPhysics.Trees;
+
+partial struct Tree
+{
+    readonly void Refit2(ref NodeChild childInParent)
+    {
+        Debug.Assert(LeafCount >= 2);
+        ref var node = ref Nodes[childInParent.Index];
+        ref var a = ref node.A;
+        if (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);
+    }
+    /// <summary>
+    /// Updates the bounding boxes of all internal nodes in the tree.
+    /// </summary>
+    public readonly void Refit2()
+    {
+        //No point in refitting a tree with no internal nodes!
+        if (LeafCount <= 2)
+            return;
+        NodeChild stub = default;
+        Refit2(ref stub);
+    }
+
+    unsafe struct RefitContext
+    {
+        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<Task>(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
+        {
+            //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)
+            {
+                if (a.LeafCount >= context->LeafCountPerTask)
+                    Refit2WithTaskSpawning(ref a, context, workerIndex, dispatcher);
+                else
+                    Refit2(ref a);
+            }
+            if (b.Index >= 0)
+            {
+                if (b.LeafCount >= context->LeafCountPerTask)
+                    Refit2WithTaskSpawning(ref b, context, workerIndex, dispatcher);
+                else
+                    Refit2(ref b);
+            }
+        }
+        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;
+        if (targetTaskCount < 0)
+            targetTaskCount = dispatcher.ThreadCount;
+        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);
+            TaskStack.DispatchWorkers(dispatcher, taskStack, int.Min(dispatcher.ThreadCount, targetTaskCount));
+        }
+        else
+        {
+            NodeChild stub = default;
+            Refit2WithTaskSpawning(ref stub, &refitContext, workerIndex, dispatcher);
+        }
+    }
+
+    /// <summary>
+    /// Refits all bounding boxes in the tree using multiple threads.
+    /// </summary>
+    /// <param name="pool">Pool used for main thread temporary allocations during execution.</param>
+    /// <param name="dispatcher">Dispatcher used during execution.</param>
+    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);
+    }
+
+    /// <summary>
+    /// Refits all bounding boxes in the tree using multiple threads.<para/>Pushes tasks into the provided <see cref="TaskStack"/>. Does not dispatch threads internally; this is intended to be used as a part of a caller-managed dispatch.
+    /// </summary>
+    /// <param name="pool">Pool used for allocations during execution.</param>
+    /// <param name="dispatcher">Dispatcher used during execution.</param>
+    /// <param name="taskStack"><see cref="TaskStack"/> that the refit operation will push tasks onto as needed.</param>
+    /// <param name="workerIndex">Index of the worker calling the function.</param>
+    /// <param name="targetTaskCount">Number of tasks the refit should try to create during execution.</param>
+    public unsafe readonly void Refit2(BufferPool pool, IThreadDispatcher dispatcher, TaskStack* taskStack, int workerIndex, int targetTaskCount = -1)
+    {
+        Refit2(pool, dispatcher, taskStack, workerIndex, targetTaskCount, internallyDispatch: false);
+    }
+
+    unsafe struct RefitWithCacheOptimizationContext
+    {
+        public Buffer<Node> SourceNodes;
+        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 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.SourceNodes[sourceNodeIndex];
+        var targetNodeIndex = childInParent.Index;
+        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;
+        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, 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);
+    }
+
+
+    /// <summary>
+    /// 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 <see cref="Nodes"/>, <see cref="Metanodes"/>, and <see cref="Leaves"/> buffers.
+    /// The nodes and metanodes will be in depth traversal order.
+    /// The input source buffer is not modified.
+    /// </summary>
+    /// <param name="sourceNodes">Nodes to base the refit on.</param>
+    public void Refit2WithCacheOptimization(Buffer<Node> sourceNodes)
+    {
+        //No point in refitting a tree with no internal nodes!
+        if (LeafCount <= 2)
+            return;
+        NodeChild stub = default;
+        var context = new RefitWithCacheOptimizationContext
+        {
+            SourceNodes = sourceNodes,
+            Tree = this,
+        };
+        Refit2WithCacheOptimization(0, -1, -1, ref stub, ref context);
+
+    }
+
+    /// <summary>
+    /// 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.
+    /// </summary>
+    /// <param name="pool">Pool to allocate from. If disposeOriginals is true, this must be the same pool from which the <see cref="Nodes"/> buffer was allocated from.</param>
+    /// <param name="disposeOriginalNodes">Whether to dispose of the original nodes buffer. If false, it's up to the caller to dispose of it appropriately.</param>
+    public 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<Node>(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->SourceNodes[sourceNodeIndex];
+        var targetNodeIndex = childInParent.Index;
+        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;
+        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<Task>(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. The larger one may still be worth spawning subtasks within, though.
+            if (sourceA.Index >= 0)
+            {
+                targetA.Index = targetIndexA;
+                targetA.LeafCount = sourceA.LeafCount;
+                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
+            {
+                //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;
+                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
+            {
+                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->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)
+    {
+        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, Buffer<Node> sourceNodes)
+    {
+        //No point in refitting a tree with no internal nodes!
+        if (LeafCount <= 2)
+            return;
+        if (targetTaskCount < 0)
+            targetTaskCount = dispatcher.ThreadCount;
+
+        const int minimumTaskSize = 32;
+        var leafCountPerTask = int.Max(minimumTaskSize, (int)float.Ceiling(LeafCount / (float)targetTaskCount));
+        var refitContext = new RefitWithCacheOptimizationContext
+        {
+            SourceNodes = sourceNodes,
+            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);
+        }
+    }
+
+
+    /// <summary>
+    /// 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.
+    /// </summary>
+    /// <param name="pool">Pool used for main thread temporary allocations during execution.</param>
+    /// <param name="dispatcher">Dispatcher used during execution.</param>
+    /// <param name="disposeOriginalNodes">Whether to dispose of the original nodes buffer. If false, it's up to the caller to dispose of it appropriately.</param>
+    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<Node>(oldNodes.Length, pool);
+        Refit2WithCacheOptimization(pool, dispatcher, &taskStack, 0, -1, internallyDispatch: true, oldNodes);
+        taskStack.Dispose(pool, dispatcher);
+        if (disposeOriginalNodes)
+            oldNodes.Dispose(pool);
+    }
+
+    /// <summary>
+    /// 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.
+    /// <para/>Pushes tasks into the provided <see cref="TaskStack"/>. Does not dispatch threads internally; this is intended to be used as a part of a caller-managed dispatch.
+    /// </summary>
+    /// <param name="pool">Pool used for allocations during execution.</param>
+    /// <param name="dispatcher">Dispatcher used during execution.</param>
+    /// <param name="taskStack"><see cref="TaskStack"/> that the refit operation will push tasks onto as needed.</param>
+    /// <param name="workerIndex">Index of the worker calling the function.</param>
+    /// <param name="targetTaskCount">Number of tasks the refit should try to create during execution. If negative, uses <see cref="IThreadDispatcher.ThreadCount"/>.</param>
+    /// <param name="disposeOriginalNodes">Whether to dispose of the original nodes buffer. If false, it's up to the caller to dispose of it appropriately.</param>
+    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<Node>(oldNodes.Length, pool);
+        Refit2WithCacheOptimization(pool, dispatcher, taskStack, workerIndex, targetTaskCount, internallyDispatch: false, oldNodes);
+        if (disposeOriginalNodes)
+            oldNodes.Dispose(pool);
+    }
+
+    /// <summary>
+    /// 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 <see cref="Nodes"/>, <see cref="Metanodes"/>, and <see cref="Leaves"/> buffers.
+    /// The nodes and metanodes will be in depth traversal order.
+    /// The input source buffer is not modified.
+    /// <para/>Pushes tasks into the provided <see cref="TaskStack"/>. Does not dispatch threads internally; this is intended to be used as a part of a caller-managed dispatch.
+    /// </summary>
+    /// <param name="sourceNodes">Nodes to base the refit on.</param>
+    /// <param name="pool">Pool used for allocations during execution.</param>
+    /// <param name="dispatcher">Dispatcher used during execution.</param>
+    /// <param name="taskStack"><see cref="TaskStack"/> that the refit operation will push tasks onto as needed.</param>
+    /// <param name="workerIndex">Index of the worker calling the function.</param>
+    /// <param name="targetTaskCount">Number of tasks the refit should try to create during execution. If negative, uses <see cref="IThreadDispatcher.ThreadCount"/>.</param>
+    public unsafe void Refit2WithCacheOptimization(Buffer<Node> sourceNodes, BufferPool pool, IThreadDispatcher dispatcher, TaskStack* taskStack, int workerIndex, int targetTaskCount = -1)
+    {
+        Refit2WithCacheOptimization(pool, dispatcher, taskStack, workerIndex, targetTaskCount, internallyDispatch: false, sourceNodes);
+    }
+}
diff --git a/BepuPhysics/Trees/Tree_Remove.cs b/BepuPhysics/Trees/Tree_Remove.cs
index 16fe0d704..9f505feac 100644
--- a/BepuPhysics/Trees/Tree_Remove.cs
+++ b/BepuPhysics/Trees/Tree_Remove.cs
@@ -7,21 +7,21 @@ 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);
+            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...
@@ -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,21 +71,21 @@ unsafe void RefitForRemoval(int nodeIndex)
         /// <param name="leafIndex">Index of the leaf to remove.</param>
         /// <returns>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.</returns>
-        public unsafe int RemoveAt(int leafIndex)
+        public 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_SelfQueries.cs b/BepuPhysics/Trees/Tree_SelfQueries.cs
index bf2c4dcbf..2028b045e 100644
--- a/BepuPhysics/Trees/Tree_SelfQueries.cs
+++ b/BepuPhysics/Trees/Tree_SelfQueries.cs
@@ -1,17 +1,46 @@
 using BepuUtilities;
+using BepuUtilities.Collections;
+using BepuUtilities.Memory;
+using BepuUtilities.TaskScheduling;
+using System;
+using System.Diagnostics;
 using System.Numerics;
 using System.Runtime.CompilerServices;
+using System.Runtime.Intrinsics;
+using System.Runtime.Intrinsics.X86;
 
 namespace BepuPhysics.Trees
 {
+    /// <summary>
+    /// Overlap callback for tree overlap queries.
+    /// </summary>
     public interface IOverlapHandler
     {
+        /// <summary>
+        /// Handles an overlap between leaves.
+        /// </summary>
+        /// <param name="indexA">Index of the first leaf in the overlap.</param>
+        /// <param name="indexB">Index of the second leaf in the overlap.</param>
         void Handle(int indexA, int indexB);
     }
-
+    /// <summary>
+    /// Overlap callback for tree overlap queries. Used in multithreaded contexts.
+    /// </summary>
+    public interface IThreadedOverlapHandler
+    {
+        /// <summary>
+        /// Handles an overlap between leaves.
+        /// </summary>
+        /// <param name="indexA">Index of the first leaf in the overlap.</param>
+        /// <param name="indexB">Index of the second leaf in the overlap.</param>
+        /// <param name="workerIndex">Index of the worker reporting the overlap.</param>
+        /// <param name="managedContext">Managed context provided by the multithreaded dispatch.</param>
+        void Handle(int indexA, int indexB, int workerIndex, object managedContext);
+    }
 
     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 
@@ -19,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.
-        unsafe void DispatchTestForLeaf<TOverlapHandler>(int leafIndex, ref Vector3 leafMin, ref Vector3 leafMax, int nodeIndex, ref TOverlapHandler results) where TOverlapHandler : IOverlapHandler
+        readonly void DispatchTestForLeaf<TOverlapHandler>(int leafIndex, ref NodeChild leafChild, int nodeIndex, ref TOverlapHandler results) where TOverlapHandler : IOverlapHandler
         {
             if (nodeIndex < 0)
             {
@@ -27,10 +56,10 @@ unsafe void DispatchTestForLeaf<TOverlapHandler>(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<TOverlapHandler>(int leafIndex, ref Vector3 leafMin, ref Vector3 leafMax, int nodeIndex, ref TOverlapHandler results)
+        readonly void TestLeafAgainstNode<TOverlapHandler>(int leafIndex, ref NodeChild leafChild, int nodeIndex, ref TOverlapHandler results)
             where TOverlapHandler : IOverlapHandler
         {
             ref var node = ref Nodes[nodeIndex];
@@ -41,20 +70,20 @@ unsafe void TestLeafAgainstNode<TOverlapHandler>(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);
             }
         }
 
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        unsafe void DispatchTestForNodes<TOverlapHandler>(ref NodeChild a, ref NodeChild b, ref TOverlapHandler results) where TOverlapHandler : IOverlapHandler
+        readonly void DispatchTestForNodes<TOverlapHandler>(ref NodeChild a, ref NodeChild b, ref TOverlapHandler results) where TOverlapHandler : IOverlapHandler
         {
             if (a.Index >= 0)
             {
@@ -65,13 +94,13 @@ unsafe void DispatchTestForNodes<TOverlapHandler>(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
             {
@@ -80,23 +109,17 @@ unsafe void DispatchTestForNodes<TOverlapHandler>(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);
-        }
-
-        unsafe void GetOverlapsBetweenDifferentNodes<TOverlapHandler>(ref Node a, ref Node b, ref TOverlapHandler results) where TOverlapHandler : IOverlapHandler
+        readonly void GetOverlapsBetweenDifferentNodes<TOverlapHandler>(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;
             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)
             {
@@ -116,28 +139,24 @@ unsafe void GetOverlapsBetweenDifferentNodes<TOverlapHandler>(ref Node a, ref No
             }
         }
 
-        unsafe void GetOverlapsInNode<TOverlapHandler>(ref Node node, ref TOverlapHandler results) where TOverlapHandler : IOverlapHandler
+        readonly void GetOverlapsInNode<TOverlapHandler>(ref Node node, ref TOverlapHandler results) where TOverlapHandler : IOverlapHandler
         {
 
             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);
             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<TOverlapHandler>(ref TOverlapHandler results) where TOverlapHandler : IOverlapHandler
+        public readonly void GetSelfOverlaps<TOverlapHandler>(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.
@@ -147,5 +166,484 @@ public unsafe void GetSelfOverlaps<TOverlapHandler>(ref TOverlapHandler results)
             GetOverlapsInNode(ref Nodes[0], ref results);
         }
 
+        void GetOverlapsWithLeaf<TOverlapHandler>(ref TOverlapHandler results, NodeChild leaf, int nodeToTest, ref QuickList<int> stack) where TOverlapHandler : IOverlapHandler
+        {
+            var leafIndex = Encode(leaf.Index);
+            Debug.Assert(stack.Count == 0);
+            while (true)
+            {
+                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 (intersectedInternalA && intersectedInternalB)
+                {
+                    nodeToTest = node.A.Index;
+                    stack.AllocateUnsafely() = 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;
+                        }
+                    }
+                }
+
+                //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;
+
+
+            }
+        }
+
+        [MethodImpl(MethodImplOptions.AggressiveInlining)]
+        static uint EncodeParentIndex(int leafParentIndex, bool childIsB)
+        {
+            var encoded = (uint)leafParentIndex;
+            if (childIsB)
+                encoded |= 1u << 31;
+            return encoded;
+        }
+        [MethodImpl(MethodImplOptions.AggressiveInlining)]
+        static Vector128<int> Encode(Vector128<int> indices)
+        {
+            return Vector128<int>.AllBitsSet - indices;
+        }
+
+        /// <summary>
+        /// Gets a reference to the node child representing the leaf within the tree.
+        /// </summary>
+        /// <param name="leaf">Leaf to look up in the tree.</param>
+        /// <returns>A reference to the node child within the tree.</returns>
+        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);
+        }
+
+        /// <summary>
+        /// Gets a reference to the node child representing the leaf within the tree.
+        /// </summary>
+        /// <param name="leafIndex">Index of the leaf to look up in the tree.</param>
+        /// <returns>A reference to the node child within the tree.</returns>
+        public readonly ref NodeChild GetNodeChildForLeaf(int leafIndex)
+        {
+            return ref GetNodeChildForLeaf(Leaves[leafIndex]);
+        }
+
+        //[MethodImpl(MethodImplOptions.AggressiveInlining)]
+        //static void LeftPack(Vector128<int> mask, Vector128<int> a, Vector128<int> b, out Vector128<int> packedA, out Vector128<int> 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<int> GetLeftPackMask(Vector128<int> 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<byte> 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(in 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<int> mask, Vector128<int> a, Vector128<int> b, out Vector128<int> packedA, out Vector128<int> packedB, out int count)
+        {
+            var permuteMask = GetLeftPackMask(mask, out count);
+            packedA = Avx.PermuteVar(a.AsSingle(), permuteMask).AsInt32();
+            packedB = Avx.PermuteVar(b.AsSingle(), permuteMask).AsInt32();
+
+        }
+
+        struct IndexPair
+        {
+            public int A;
+            public int B;
+        }
+
+        unsafe struct NodeLeafPair
+        {
+            public NodeChild* LeafParent;
+            public int NodeIndex;
+        }
+
+        unsafe void AddCrossoverResult<TOverlapHandler>(ref NodeChild a, ref NodeChild b, ref QuickList<IndexPair> crossovers, ref QuickList<NodeLeafPair> nodeLeaf, ref TOverlapHandler results, BufferPool pool) where TOverlapHandler : IOverlapHandler
+        {
+            if (a.Index >= 0 && b.Index >= 0)
+            {
+                crossovers.Allocate(pool) = new IndexPair { A = a.Index, B = b.Index };
+            }
+            else if (a.Index < 0 && b.Index < 0)
+            {
+                results.Handle(Encode(a.Index), Encode(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 };
+            }
+        }
+
+        void ExecuteCrossoverBatch<TOverlapHandler>(ref QuickList<IndexPair> crossovers, ref QuickList<NodeLeafPair> nodeLeaf, ref TOverlapHandler results, BufferPool pool) where TOverlapHandler : IOverlapHandler
+        {
+            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)
+                {
+                    AddCrossoverResult(ref aa, ref ba, ref crossovers, ref nodeLeaf, ref results, pool);
+                }
+                if (abIntersects)
+                {
+                    AddCrossoverResult(ref aa, ref bb, ref crossovers, ref nodeLeaf, ref results, pool);
+                }
+                if (baIntersects)
+                {
+                    AddCrossoverResult(ref ab, ref ba, ref crossovers, ref nodeLeaf, ref results, pool);
+                }
+                if (bbIntersects)
+                {
+                    AddCrossoverResult(ref ab, ref bb, ref crossovers, ref nodeLeaf, ref results, pool);
+                }
+            }
+        }
+        unsafe void ExecuteNodeLeafBatch<TOverlapHandler>(ref QuickList<NodeLeafPair> nodeLeaf, ref TOverlapHandler results, BufferPool pool) where TOverlapHandler : IOverlapHandler
+        {
+            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)
+                {
+                    if (a.Index < 0)
+                    {
+                        results.Handle(Encode(leafChild.Index), Encode(a.Index));
+                    }
+                    else
+                    {
+                        nodeLeaf.Allocate(pool) = new NodeLeafPair { LeafParent = pair.LeafParent, NodeIndex = a.Index };
+                    }
+                }
+                if (bIntersects)
+                {
+                    if (b.Index < 0)
+                    {
+                        results.Handle(Encode(leafChild.Index), Encode(b.Index));
+                    }
+                    else
+                    {
+                        nodeLeaf.Allocate(pool) = new NodeLeafPair { LeafParent = pair.LeafParent, NodeIndex = b.Index };
+                    }
+                }
+            }
+        }
+
+        void FlushLeafLeaf<TOverlapHandler>(ref QuickList<IndexPair> 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;
+        }
+
+        public unsafe void GetSelfOverlapsBatched<TOverlapHandler>(ref TOverlapHandler results, BufferPool pool) where TOverlapHandler : IOverlapHandler
+        {
+            const int crossoverBatchSizeTarget = 16;
+            const int nodeLeafBatchSizeTarget = 16;
+            var crossovers = new QuickList<IndexPair>(crossoverBatchSizeTarget * 16, pool);
+            var nodeLeaf = new QuickList<NodeLeafPair>(nodeLeafBatchSizeTarget * 16, pool);
+            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;
+                if (BoundingBox.IntersectsUnsafe(a, b))
+                {
+                    if (a.Index >= 0 && b.Index >= 0)
+                    {
+                        crossovers.Allocate(pool) = new IndexPair { A = a.Index, B = b.Index };
+                    }
+                    else if (a.Index < 0 && b.Index < 0)
+                    {
+                        results.Handle(Encode(a.Index), Encode(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 results, pool);
+                }
+                if (nodeLeaf.Count >= nodeLeafBatchSizeTarget)
+                {
+                    ExecuteNodeLeafBatch(ref nodeLeaf, ref results, pool);
+                }
+            }
+            //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 void GetSelfOverlaps2<TOverlapHandler>(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)
+                {
+                    DispatchTestForNodes(ref a, ref b, ref results);
+                }
+            }
+        }
+
+        /// <summary>
+        /// Reports all bounding box overlaps between leaves in the tree to the given <typeparamref name="TOverlapHandler"/>.
+        /// </summary>
+        /// <param name="results">Handler to report results to.</param>
+        public readonly void GetSelfOverlaps2<TOverlapHandler>(ref TOverlapHandler results) where TOverlapHandler : IOverlapHandler
+        {
+            GetSelfOverlaps2(ref results, 0, NodeCount);
+        }
+
+        unsafe struct SelfTestContext<TOverlapHandler> where TOverlapHandler : unmanaged, IThreadedOverlapHandler
+        {
+            public Tree Tree;
+            public int LoopTaskCount;
+            public int LeafThresholdForTask;
+            public TOverlapHandler* Results;
+            public TaskStack* TaskStack;
+        }
+        unsafe struct WrappedOverlapHandler<TOverlapHandler> : 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, ManagedContext);
+            }
+        }
+
+        unsafe static void LoopEntryTask<TOverlapHandler>(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<TOverlapHandler>*)untypedContext;
+            var wrapped = new WrappedOverlapHandler<TOverlapHandler> { Inner = context.Results, WorkerIndex = workerIndex, ManagedContext = dispatcher.ManagedContext };
+            context.Tree.GetSelfOverlaps2(ref wrapped, taskStart, taskEnd);
+        }
+
+
+
+        private unsafe void GetSelfOverlaps2<TOverlapHandler>(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 (targetTaskBudget < 0)
+                targetTaskBudget = threadDispatcher.ThreadCount;
+            targetTaskBudget *= 16;
+            targetTaskBudget = int.Min(NodeCount, targetTaskBudget);
+
+            const int leafThresholdForTask = 256;
+
+            var resultsCopy = results;
+            var context = new SelfTestContext<TOverlapHandler> { 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.
+            //(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 / 4);
+            var earlyIsolatedNodesMemory = stackalloc int[isolatedNodeCapacity];
+            var earlyIsolatedNodes = new QuickList<int>(new Buffer<int>(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.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() = i;
+                    }
+                }
+                else
+                {
+                    earlyIsolatedNodeIntervalEnd = i;
+                    break;
+                }
+            }
+
+            var remainingNodeCount = NodeCount - earlyIsolatedNodeIntervalEnd;
+            var regularLoopTaskCount = targetTaskBudget - earlyIsolatedNodes.Count;
+            var nodesPerTaskBase = remainingNodeCount / regularLoopTaskCount;
+            var remainder = remainingNodeCount - nodesPerTaskBase * regularLoopTaskCount;
+            var tasks = new Buffer<Task>(targetTaskBudget, pool);
+            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(&LoopEntryTask<TOverlapHandler>, &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<TOverlapHandler>, &context, (uint)taskStart | (((long)(taskStart + 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, managedContext);
+            }
+            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;
+        }
+
+        /// <summary>
+        /// Reports all bounding box overlaps between leaves in the tree to the given <typeparamref name="TOverlapHandler"/>. Uses the thread dispatcher to parallelize overlap testing.
+        /// </summary>
+        /// <param name="results">Handler to report results to.</param>
+        /// <param name="pool">Pool used for ephemeral allocations.</param>
+        /// <param name="threadDispatcher">Thread dispatcher used during the overlap testing.</param>
+        /// <param name="managedContext">Managed context to provide to the overlap handler, if any.</param>
+        public unsafe void GetSelfOverlaps2<TOverlapHandler>(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, managedContext);
+            taskStack.Dispose(pool, threadDispatcher);
+        }
+
+        /// <summary>
+        /// Reports all bounding box overlaps between leaves in the tree to the given <typeparamref name="TOverlapHandler"/>.
+        /// <para/>Pushes tasks into the provided <see cref="TaskStack"/>. Does not dispatch threads internally; this is intended to be used as a part of a caller-managed dispatch.
+        /// </summary>
+        /// <param name="results">Handler to report results to.</param>
+        /// <param name="pool">Pool used for ephemeral allocations.</param>
+        /// <param name="threadDispatcher">Thread dispatcher used during the overlap test.</param>
+        /// <param name="taskStack"><see cref="TaskStack"/> that the overlap test will push tasks onto as needed.</param>
+        /// <param name="workerIndex">Index of the worker calling the function.</param>
+        /// <param name="targetTaskCount">Number of tasks the overlap testing should try to create during execution. If negative, uses <see cref="IThreadDispatcher.ThreadCount"/>.</param>
+        /// <remarks>This does not dispatch workers on the <see cref="IThreadDispatcher"/> directly. If the overlap handler requires managed context, that should be provided by whatever dispatched the workers.</remarks>
+        public unsafe void GetSelfOverlaps2<TOverlapHandler>(ref TOverlapHandler results, BufferPool pool,
+             IThreadDispatcher threadDispatcher, TaskStack* taskStack, int workerIndex, int targetTaskCount = -1) where TOverlapHandler : unmanaged, IThreadedOverlapHandler
+        {
+            GetSelfOverlaps2(ref results, pool, workerIndex, taskStack, threadDispatcher, false, threadDispatcher.ThreadCount, targetTaskCount);
+        }
     }
 }
diff --git a/BepuPhysics/Trees/Tree_SelfQueriesMT.cs b/BepuPhysics/Trees/Tree_SelfQueriesMT.cs
index 1f5f2ddf1..21d36181a 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;
 
@@ -73,22 +71,22 @@ 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<Job>();
                     return;
                 }
                 Debug.Assert(overlapHandlers.Length >= threadCount);
-                const float jobMultiplier = 1.5f;
+                const float jobMultiplier = 8f;
                 var targetJobCount = Math.Max(1, jobMultiplier * threadCount);
-                leafThreshold = (int)(tree.leafCount / targetJobCount);
+                leafThreshold = (int)(tree.LeafCount / targetJobCount);
                 jobs = new QuickList<Job>((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]);
             }
 
             /// <summary>
@@ -101,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)
@@ -122,7 +120,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 +129,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.
@@ -141,7 +139,7 @@ public unsafe void ExecuteJob(int jobIndex, int workerIndex)
             /// Executes a single worker of the multithreaded self test.
             /// </summary>
             /// <param name="workerIndex">Index of the worker executing this set of tests.</param>
-            public unsafe void PairTest(int workerIndex)
+            public void PairTest(int workerIndex)
             {
                 Debug.Assert(workerIndex >= 0 && workerIndex < OverlapHandlers.Length);
                 int nextNodePairIndex;
@@ -152,7 +150,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)
+            void DispatchTestForLeaf(int leafIndex, ref NodeChild leafChild, int nodeIndex, int nodeLeafCount, ref TOverlapHandler results)
             {
                 if (nodeIndex < 0)
                 {
@@ -163,11 +161,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)
+            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,20 +176,20 @@ 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);
                 }
             }
 
             [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)
                 {
@@ -206,13 +204,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
                 {
@@ -221,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.
 
@@ -229,10 +227,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)
                 {
@@ -253,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)
                 {
@@ -265,7 +263,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/BepuPhysics/Trees/Tree_Sweep.cs b/BepuPhysics/Trees/Tree_Sweep.cs
index 758f0ca28..a6e2bb5b3 100644
--- a/BepuPhysics/Trees/Tree_Sweep.cs
+++ b/BepuPhysics/Trees/Tree_Sweep.cs
@@ -1,23 +1,21 @@
 using BepuUtilities;
-using System;
-using System.Collections.Generic;
+using BepuUtilities.Memory;
 using System.Diagnostics;
 using System.Numerics;
 using System.Runtime.CompilerServices;
-using System.Text;
 
 namespace BepuPhysics.Trees
 {
     public interface ISweepLeafTester
     {
-        unsafe void TestLeaf(int leafIndex, ref float maximumT);
+        void TestLeaf(int leafIndex, ref float maximumT);
     }
     partial struct Tree
     {
-        readonly unsafe void Sweep<TLeafTester>(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<TLeafTester>(int nodeIndex, Vector3 expansion, Vector3 origin, Vector3 direction, TreeRay* treeRay, Buffer<int> 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.");
+            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)
@@ -29,7 +27,7 @@ readonly unsafe void Sweep<TLeafTester>(int nodeIndex, in Vector3 expansion, in
                     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
@@ -47,7 +45,18 @@ readonly unsafe void Sweep<TLeafTester>(int nodeIndex, in Vector3 expansion, in
                         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<int>(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;
@@ -73,20 +82,24 @@ readonly unsafe void Sweep<TLeafTester>(int nodeIndex, in Vector3 expansion, in
                     {
                         //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<TLeafTester>(in Vector3 expansion, in Vector3 origin, in Vector3 direction, TreeRay* treeRay, ref TLeafTester sweepTester) where TLeafTester : ISweepLeafTester
+        internal readonly unsafe void Sweep<TLeafTester>(Vector3 expansion, Vector3 origin, Vector3 direction, TreeRay* treeRay, BufferPool pool, 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))
@@ -96,15 +109,20 @@ internal readonly unsafe void Sweep<TLeafTester>(in Vector3 expansion, in Vector
             }
             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<int>(stack, TraversalStackCapacity), pool, ref sweepTester);
             }
         }
 
+        /// <summary>
+        /// Converts a bounding box defined by minimum and maximum corners into a centroid and half-extent representation.
+        /// </summary>
+        /// <param name="min">The minimum corner of the bounding box.</param>
+        /// <param name="max">The maximum corner of the bounding box.</param>
+        /// <param name="origin">The computed centroid of the bounding box.</param>
+        /// <param name="expansion">The computed half-extents of the bounding box.</param>
         [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,16 +130,35 @@ public static void ConvertBoxToCentroidWithExtent(in Vector3 min, in Vector3 max
             origin = halfMax + halfMin;
         }
 
-        public readonly unsafe void Sweep<TLeafTester>(in Vector3 min, in Vector3 max, in Vector3 direction, float maximumT, ref TLeafTester sweepTester) where TLeafTester : ISweepLeafTester
+        /// <summary>
+        /// Performs a sweep test of an axis-aligned bounding box against the tree and invokes the <paramref name="sweepTester"/> for each intersecting leaf.
+        /// </summary>
+        /// <typeparam name="TLeafTester">The type of the <see cref="ISweepLeafTester"/> used to process the intersecting leaves.</typeparam>
+        /// <param name="min">The minimum corner of the axis-aligned bounding box to sweep.</param>
+        /// <param name="max">The maximum corner of the axis-aligned bounding box to sweep.</param>
+        /// <param name="direction">The direction of the sweep.</param>
+        /// <param name="maximumT">The maximum parametric distance along the sweep direction to test.</param>
+        /// <param name="sweepTester">A reference to the tester that processes the indices of intersecting leaves.</param>
+        /// <param name="pool">The buffer pool used for temporary allocations during the operation. Only used if the tree is pathologically deep; stack memory is used preferentially.</param>
+        public readonly unsafe void Sweep<TLeafTester>(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);
         }
+        /// <summary>
+        /// Performs a sweep test of a bounding box against the tree and invokes the <paramref name="sweepTester"/> for each intersecting leaf.
+        /// </summary>
+        /// <typeparam name="TLeafTester">The type of the <see cref="ISweepLeafTester"/> used to process the intersecting leaves.</typeparam>
+        /// <param name="boundingBox">The bounding box to sweep.</param>
+        /// <param name="direction">The direction of the sweep.</param>
+        /// <param name="maximumT">The maximum parametric distance along the sweep direction to test.</param>
+        /// <param name="sweepTester">A reference to the tester that processes the indices of intersecting leaves.</param>
+        /// <param name="pool">The buffer pool used for temporary allocations during the operation. Only used if the tree is pathologically deep; stack memory is used preferentially.</param>
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public readonly unsafe void Sweep<TLeafTester>(in BoundingBox boundingBox, in Vector3 direction, float maximumT, ref TLeafTester sweepTester) where TLeafTester : ISweepLeafTester
+        public readonly void Sweep<TLeafTester>(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_SweepBuilder.cs b/BepuPhysics/Trees/Tree_SweepBuilder.cs
index acf8cc044..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;
 
@@ -249,19 +248,17 @@ public unsafe void SweepBuild(BufferPool pool, Buffer<BoundingBox> 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.
-            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<int>(leafBounds.Length, out var indexMap);
@@ -302,13 +299,13 @@ public unsafe void SweepBuild(BufferPool pool, Buffer<BoundingBox> 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/BepuPhysics/Trees/Tree_VolumeQuery.cs b/BepuPhysics/Trees/Tree_VolumeQuery.cs
index d301bf432..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,10 +8,10 @@ namespace BepuPhysics.Trees
 {
     partial struct Tree
     {
-        unsafe readonly void GetOverlaps<TEnumerator>(int nodeIndex, in Vector3 min, in Vector3 max, int* stack, ref TEnumerator leafEnumerator) where TEnumerator : IBreakableForEach<int>
+        unsafe readonly void GetOverlaps<TEnumerator>(int nodeIndex, BoundingBox boundingBox, Buffer<int> stack, BufferPool pool, ref TEnumerator leafEnumerator) where TEnumerator : IBreakableForEach<int>
         {
-            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)
@@ -20,25 +21,35 @@ unsafe readonly void GetOverlaps<TEnumerator>(int nodeIndex, in Vector3 min, in
                     //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
                 {
                     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)
                     {
                         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<int>(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,26 +62,38 @@ unsafe readonly void GetOverlaps<TEnumerator>(int nodeIndex, in Vector3 min, in
                     {
                         //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<TEnumerator>(in Vector3 min, in Vector3 max, ref TEnumerator leafEnumerator) where TEnumerator : IBreakableForEach<int>
+
+        /// <summary>
+        /// Finds and processes all leaves with bounding boxes that overlap the specified axis-aligned bounding box. The <paramref name="leafEnumerator"/> is invoked
+        /// for each overlapping element.
+        /// </summary>
+        /// <typeparam name="TEnumerator">The type of the <see cref="IBreakableForEach{T}"/> enumerator used to process the overlapping elements.</typeparam>
+        /// <param name="boundingBox">Query to test against the bounding volume hierarchy.</param>
+        /// <param name="pool">The buffer pool used for temporary allocations during the operation. Only used if the tree is pathologically deep; stack memory is used preferentially.</param>
+        /// <param name="leafEnumerator">A reference to the enumerator that processes the indices of overlapping elements. The enumerator can
+        /// terminate early by returning <see langword="false"/> from its iteration.</param>
+        public readonly unsafe void GetOverlaps<TEnumerator>(BoundingBox boundingBox, BufferPool pool, ref TEnumerator leafEnumerator) where TEnumerator : IBreakableForEach<int>
         {
-            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);
+                GetOverlaps(0, boundingBox, new Buffer<int>(stack, TraversalStackCapacity), pool, 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))
+                if (BoundingBox.IntersectsUnsafe(boundingBox, Nodes[0].A))
                 {
                     leafEnumerator.LoopBody(Encode(Nodes[0].A.Index));
                 }
@@ -79,10 +102,19 @@ public readonly unsafe void GetOverlaps<TEnumerator>(in Vector3 min, in Vector3
             //If the leaf count is zero, then there's nothing to test against.
         }
 
-        [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public readonly unsafe void GetOverlaps<TEnumerator>(in BoundingBox boundingBox, ref TEnumerator leafEnumerator) where TEnumerator : IBreakableForEach<int>
+        /// <summary>
+        /// Finds and processes all leaves with bounding boxes that overlap the specified axis-aligned bounding box. The <paramref name="leafEnumerator"/> is invoked
+        /// for each overlapping element.
+        /// </summary>
+        /// <typeparam name="TEnumerator">The type of the <see cref="IBreakableForEach{T}"/> enumerator used to process the overlapping elements.</typeparam>
+        /// <param name="min">The minimum corner of the axis-aligned bounding box.</param>
+        /// <param name="max">The maximum corner of the axis-aligned bounding box.</param>
+        /// <param name="pool">The buffer pool used for temporary allocations during the operation. Only used if the tree is pathologically deep; stack memory is used preferentially.</param>
+        /// <param name="leafEnumerator">A reference to the enumerator that processes the indices of overlapping elements. The enumerator can
+        /// terminate early by returning <see langword="false"/> from its iteration.</param>
+        public readonly void GetOverlaps<TEnumerator>(Vector3 min, Vector3 max, BufferPool pool, ref TEnumerator leafEnumerator) where TEnumerator : IBreakableForEach<int>
         {
-            GetOverlaps(boundingBox.Min, boundingBox.Max, ref leafEnumerator);
+            GetOverlaps(new BoundingBox(min, max), pool, ref leafEnumerator);
         }
 
 
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.
         ///</summary>
         ///<param name="translation">Translation to use in the transform.</param>
-        public AffineTransform(in Vector3 translation)
+        public AffineTransform(Vector3 translation)
         {
             LinearTransform = Matrix3x3.Identity;
             Translation = translation;
@@ -46,7 +46,7 @@ public AffineTransform(in Vector3 translation)
         ///</summary>
         ///<param name="orientation">Orientation to use as the linear transform.</param>
         ///<param name="translation">Translation to use in the transform.</param>
-        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)
         ///<param name="scaling">Scaling to apply in the linear transform.</param>
         ///<param name="orientation">Orientation to apply in the linear transform.</param>
         ///<param name="translation">Translation to apply.</param>
-        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
         ///</summary>
         ///<param name="linearTransform">The linear transform component.</param>
         ///<param name="translation">Translation component of the transform.</param>
-        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)
         ///<param name="transform">Transform to apply.</param>
         ///<param name="transformed">Transformed position.</param>
         [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/BepuUtilities.csproj b/BepuUtilities/BepuUtilities.csproj
index b6a1b7c25..5b7eb7088 100644
--- a/BepuUtilities/BepuUtilities.csproj
+++ b/BepuUtilities/BepuUtilities.csproj
@@ -1,26 +1,18 @@
-<Project Sdk="Microsoft.NET.Sdk">
+<Project Sdk="Microsoft.NET.Sdk">
   <PropertyGroup>
     <AssemblyName>BepuUtilities</AssemblyName>
     <RootNamespace>BepuUtilities</RootNamespace>
-    <TargetFramework>net5.0</TargetFramework>
-    <Version>2.4.0-beta6</Version>
-    <Company>Bepu Entertainment LLC</Company>
-    <Authors>Ross Nordby</Authors>
     <Description>Supporting utilities library for BEPUphysics v2.</Description>
-    <Copyright>© Bepu Entertainment LLC</Copyright>
-    <PackageProjectUrl>https://github.com/bepu/bepuphysics2</PackageProjectUrl>
-    <PackageLicenseExpression>Apache-2.0</PackageLicenseExpression>
-    <RepositoryUrl>https://github.com/bepu/bepuphysics2</RepositoryUrl>
-    <PackageIcon>bepuphysicslogo256.png</PackageIcon>
     <Configurations>Debug;Release</Configurations>
-    <LangVersion>latest</LangVersion>
-    <AllowUnsafeBlocks>True</AllowUnsafeBlocks>
-    
-    <SignAssembly>true</SignAssembly>
-    <DelaySign>false</DelaySign>
     <AssemblyOriginatorKeyFile>key.snk</AssemblyOriginatorKeyFile>
+
+    <!-- CS1573 Parameter has no matching param tag in the XML comment for (but other parameters do) -->
+    <!-- CS1591 Missing XML comment for publicly visible type or member -->
+    <NoWarn>1573;1591</NoWarn>
   </PropertyGroup>
 
+  <Import Project="..\CommonSettings.props" />
+
   <PropertyGroup Condition="'$(Configuration)'=='Debug'">
     <Optimize>false</Optimize>
     <DefineConstants>TRACE;DEBUG;CHECKMATH</DefineConstants>
@@ -29,15 +21,6 @@
   <PropertyGroup Condition="'$(Configuration)'=='Release'">
     <Optimize>true</Optimize>
     <DefineConstants>TRACE;RELEASE</DefineConstants>
-    <DebugSymbols>true</DebugSymbols>
-    <DebugType>embedded</DebugType>
   </PropertyGroup>
-        
-  <ItemGroup>
-    <None Include="..\Documentation\images\bepuphysicslogo256.png">
-      <Pack>True</Pack>
-      <PackagePath></PackagePath>
-    </None>
-  </ItemGroup>
 
 </Project>
\ No newline at end of file
diff --git a/BepuUtilities/BoundingBox.cs b/BepuUtilities/BoundingBox.cs
index 533039c33..0dcae8ba5 100644
--- a/BepuUtilities/BoundingBox.cs
+++ b/BepuUtilities/BoundingBox.cs
@@ -1,18 +1,47 @@
 using System;
-using System.Collections.Generic;
-using System.Linq;
+using System.Diagnostics;
 using System.Numerics;
 using System.Runtime.CompilerServices;
 using System.Runtime.InteropServices;
-using System.Text;
+using System.Runtime.Intrinsics;
+using System.Runtime.Intrinsics.X86;
 
 namespace BepuUtilities
 {
     /// <summary>
-    /// Provides XNA-like axis-aligned bounding box functionality.
+    /// Represents a bounding box as two <see cref="Vector4"/> values to to avoid complexity associated with a <see cref="Vector3"/>'s empty SIMD lane.
     /// </summary>
-    //NOTE: The explicit size avoids https://github.com/dotnet/coreclr/issues/12950
-    [StructLayout(LayoutKind.Explicit, Size = 32)] 
+    [StructLayout(LayoutKind.Explicit, Size = 32)]
+    public struct BoundingBox4
+    {
+        /// <summary>
+        /// Location with the lowest X, Y, and Z coordinates in the axis-aligned bounding box. W lane is undefined.
+        /// </summary>
+        [FieldOffset(0)]
+        public Vector4 Min;
+
+        /// <summary>
+        /// Location with the highest X, Y, and Z coordinates in the axis-aligned bounding box. W lane is undefined.
+        /// </summary>
+        [FieldOffset(16)]
+        public Vector4 Max;
+
+
+        /// <summary>
+        /// Creates a string representation of the bounding box.
+        /// </summary>
+        /// <returns>String representation of the bounding box.</returns>
+        public override string ToString()
+        {
+            return $"({Unsafe.As<Vector4, Vector3>(ref Min)}, {Unsafe.As<Vector4, Vector3>(ref Max)})";
+        }
+
+    }
+
+    /// <summary>
+    /// Provides simple axis-aligned bounding box functionality.
+    /// </summary>
+    [StructLayout(LayoutKind.Explicit, Size = 32)]
     public struct BoundingBox
     {
         /// <summary>
@@ -39,6 +68,50 @@ public BoundingBox(Vector3 min, Vector3 max)
             this.Max = max;
         }
 
+        /// <summary>
+        /// Checks if two structures with memory layouts equivalent to the <see cref="BoundingBox"/> intersect.
+        /// The referenced values must not be in unpinned managed memory.
+        /// </summary>
+        /// <param name="boundingBoxA">First bounding box to compare.</param>
+        /// <param name="boundingBoxB">Second bounding box to compare.</param>
+        /// <returns>True if the children overlap, false otherwise.</returns>
+        [MethodImpl(MethodImplOptions.AggressiveInlining)]
+        public unsafe static bool IntersectsUnsafe<TA, TB>(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!
+            Debug.Assert(Unsafe.SizeOf<TA>() == 32 && Unsafe.SizeOf<TB>() == 32);
+            //AVX codepath is not helpful in my tests.
+            //if (Vector256.IsHardwareAccelerated && Avx.IsSupported)
+            //{
+            //    var a = Vector256.LoadUnsafe(ref Unsafe.As<TA, float>(ref Unsafe.AsRef(childA)));
+            //    var b = Vector256.LoadUnsafe(ref Unsafe.As<TB, float>(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<TA, float>(ref Unsafe.AsRef(in boundingBoxA));
+                ref var b = ref Unsafe.As<TB, float>(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);
+                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(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];
+            }
+        }
 
         /// <summary>
         /// Determines if a bounding box intersects another bounding box.
@@ -46,22 +119,30 @@ public BoundingBox(Vector3 min, Vector3 max)
         /// <param name="a">First bounding box to test.</param>
         /// <param name="b">Second bounding box to test.</param>
         /// <returns>Whether the bounding boxes intersected.</returns>
+        /// <remarks>When possible, prefer using the <see cref="IntersectsUnsafe{TA, TB}(in TA, in TB)"/> variant for slightly better performance.</remarks>
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public static bool Intersects(in BoundingBox a, in BoundingBox b)
+        public 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. 
+
         /// <summary>
         /// Determines if a bounding box intersects another bounding box.
         /// </summary>
-        /// <param name="a">First bounding box to test.</param>
-        /// <param name="b">Second bounding box to test.</param>
+        /// <param name="minA">Minimum bounds of bounding box A.</param>
+        /// <param name="maxA">Maximum bounds of bounding box A.</param>
+        /// <param name="minB">Minimum bounds of bounding box B.</param>
+        /// <param name="maxB">Maximum bounds of bounding box B.</param>
         /// <returns>Whether the bounding boxes intersected.</returns>
+        /// <remarks>When possible, prefer using the <see cref="IntersectsUnsafe{TA, TB}(in TA, in TB)"/> variant for slightly better performance.</remarks>
         [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)
         {
+            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;
         }
@@ -72,7 +153,7 @@ public static bool Intersects(in Vector3 minA, in Vector3 maxA, in Vector3 minB,
         /// <param name="box">Bounding box to measure.</param>
         /// <returns>Volume of the bounding box.</returns>
         [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;
@@ -89,7 +170,7 @@ public static unsafe float ComputeVolume(ref BoundingBox box)
         /// <param name="min">Minimum of the merged bounding box.</param>
         /// <param name="max">Maximum of the merged bounding box.</param>
         [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);
@@ -107,6 +188,88 @@ 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);
         }
 
+        /// <summary>
+        /// Merges two structures with memory layouts equivalent to the <see cref="BoundingBox"/>.
+        /// The referenced values must not be in unpinned managed memory.
+        /// Any data in the empty slots is preserved.
+        /// </summary>
+        /// <param name="boundingBoxA">First bounding box to compare.</param>
+        /// <param name="boundingBoxB">Second bounding box to compare.</param>
+        /// <param name="merged">Merged bounding box.</param>
+        /// <typeparam name="TA">Type of the first bounding box-like parameter.</typeparam>
+        /// <typeparam name="TB">Type of the second bounding box-like parameter.</typeparam>
+        [MethodImpl(MethodImplOptions.AggressiveInlining)]
+        public static void CreateMergedUnsafeWithPreservation<TA, TB>(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<TA, Vector128<float>>(ref merged);
+                ref var resultMax = ref Unsafe.Add(ref Unsafe.As<TA, Vector128<float>>(ref merged), 1);
+                var min = Vector128.Min(
+                    Unsafe.As<TA, Vector128<float>>(ref Unsafe.AsRef(in boundingBoxA)),
+                    Unsafe.As<TB, Vector128<float>>(ref Unsafe.AsRef(in boundingBoxB)));
+                var max = Vector128.Max(
+                    Unsafe.Add(ref Unsafe.As<TA, Vector128<float>>(ref Unsafe.AsRef(in boundingBoxA)), 1),
+                    Unsafe.Add(ref Unsafe.As<TB, Vector128<float>>(ref Unsafe.AsRef(in 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<int, float>();
+                    resultMin = Vector128.ConditionalSelect(mask, min, resultMin);
+                    resultMax = Vector128.ConditionalSelect(mask, max, resultMax);
+                }
+            }
+            else
+            {
+                ref var a = ref Unsafe.As<TA, BoundingBox>(ref Unsafe.AsRef(in boundingBoxA));
+                ref var b = ref Unsafe.As<TB, BoundingBox>(ref Unsafe.AsRef(in boundingBoxB));
+                Unsafe.SkipInit(out merged);
+                ref var result = ref Unsafe.As<TA, BoundingBox>(ref Unsafe.AsRef(in merged));
+                result.Min = Vector3.Min(a.Min, b.Min);
+                result.Max = Vector3.Max(a.Max, b.Max);
+            }
+        }
+        /// <summary>
+        /// Merges two structures with memory layouts equivalent to the <see cref="BoundingBox"/>.
+        /// The referenced values must not be in unpinned managed memory.
+        /// Any data in the empty slots is not preserved.
+        /// </summary>
+        /// <param name="boundingBoxA">First bounding box to compare.</param>
+        /// <param name="boundingBoxB">Second bounding box to compare.</param>
+        /// <param name="merged">Merged bounding box.</param>
+        /// <typeparam name="TA">Type of the first bounding box-like parameter.</typeparam>
+        /// <typeparam name="TB">Type of the second bounding box-like parameter.</typeparam>
+        [MethodImpl(MethodImplOptions.AggressiveInlining)]
+        public static void CreateMergedUnsafe<TA, TB>(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<TA, Vector128<float>>(ref merged);
+                ref var resultMax = ref Unsafe.Add(ref Unsafe.As<TA, Vector128<float>>(ref merged), 1);
+                resultMin = Vector128.Min(
+                    Unsafe.As<TA, Vector128<float>>(ref Unsafe.AsRef(in boundingBoxA)),
+                    Unsafe.As<TB, Vector128<float>>(ref Unsafe.AsRef(in boundingBoxB)));
+                resultMax = Vector128.Max(
+                    Unsafe.Add(ref Unsafe.As<TA, Vector128<float>>(ref Unsafe.AsRef(in boundingBoxA)), 1),
+                    Unsafe.Add(ref Unsafe.As<TB, Vector128<float>>(ref Unsafe.AsRef(in boundingBoxB)), 1));
+            }
+            else
+            {
+                ref var a = ref Unsafe.As<TA, BoundingBox>(ref Unsafe.AsRef(in boundingBoxA));
+                ref var b = ref Unsafe.As<TB, BoundingBox>(ref Unsafe.AsRef(in boundingBoxB));
+                Unsafe.SkipInit(out merged);
+                ref var result = ref Unsafe.As<TA, BoundingBox>(ref Unsafe.AsRef(in merged));
+                result.Min = Vector3.Min(a.Min, b.Min);
+                result.Max = Vector3.Max(a.Max, b.Max);
+            }
+        }
+
         /// <summary>
         /// Determines if a bounding box intersects a bounding sphere.
         /// </summary>
@@ -142,14 +305,14 @@ public ContainmentType Contains(ref BoundingBox boundingBox)
         /// </summary>
         /// <param name="points">Points to enclose with a bounding box.</param>
         /// <returns>Bounding box which contains the list of points.</returns>
-        public static BoundingBox CreateFromPoints(IList<Vector3> points)
+        public static BoundingBox CreateFromPoints(ReadOnlySpan<Vector3> 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 +342,7 @@ public static void CreateFromSphere(ref BoundingSphere boundingSphere, out Bound
         /// <returns>String representation of the bounding box.</returns>
         public override string ToString()
         {
-            return $"({Min.ToString()}, {Max.ToString()})";
+            return $"({Min}, {Max})";
         }
 
     }
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 3ae5b0341..de7594251 100644
--- a/BepuUtilities/BundleIndexing.cs
+++ b/BepuUtilities/BundleIndexing.cs
@@ -1,10 +1,11 @@
-using System;
+using System.Diagnostics;
 using System.Numerics;
 using System.Runtime.CompilerServices;
-using System.Runtime.InteropServices;
+using System.Runtime.Intrinsics;
+using System.Runtime.Intrinsics.X86;
 
 namespace BepuUtilities
-{    
+{
     /// <summary>
     /// Some helpers for indexing into vector bundles.
     /// </summary>
@@ -57,5 +58,112 @@ public static int GetBundleCount(int elementCount)
         {
             return (elementCount + VectorMask) >> VectorShift;
         }
+
+        [MethodImpl(MethodImplOptions.AggressiveInlining)]
+        public static unsafe Vector<int> CreateTrailingMaskForCountInBundle(int countInBundle)
+        {
+            //TODO: Cross platform intrinsics rewrite
+            if (Avx.IsSupported && Vector<int>.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<int>.Count == 4)
+            {
+                return Sse.CompareLessThanOrEqual(Vector128.Create((float)countInBundle), Vector128.Create(0f, 1f, 2f, 3f)).AsInt32().AsVector();
+            }
+            else
+            {
+                Vector<int> mask;
+                var toReturnPointer = (int*)&mask;
+                for (int i = 0; i < Vector<int>.Count; ++i)
+                {
+                    toReturnPointer[i] = countInBundle <= i ? -1 : 0;
+                }
+                return mask;
+            }
+        }
+
+
+        [MethodImpl(MethodImplOptions.AggressiveInlining)]
+        public static unsafe Vector<int> CreateMaskForCountInBundle(int countInBundle)
+        {
+            //TODO: Cross platform intrinsics rewrite
+            if (Avx.IsSupported && Vector<int>.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<int>.Count == 4)
+            {
+                return Sse.CompareGreaterThan(Vector128.Create((float)countInBundle), Vector128.Create(0f, 1f, 2f, 3f)).AsInt32().AsVector();
+            }
+            else
+            {
+                Vector<int> mask;
+                var toReturnPointer = (int*)&mask;
+                for (int i = 0; i < Vector<int>.Count; ++i)
+                {
+                    toReturnPointer[i] = countInBundle > i ? -1 : 0;
+                }
+                return mask;
+            }
+        }
+
+
+        [MethodImpl(MethodImplOptions.AggressiveInlining)]
+        public static int GetFirstSetLaneIndex(Vector<int> v)
+        {
+            //TODO: Probable cross platform intrinsics rewrite
+            if (Avx.IsSupported && Vector<int>.Count == 8)
+            {
+                var scalarMask = Avx.MoveMask(v.AsVector256().As<int, float>());
+                return BitOperations.TrailingZeroCount(scalarMask);
+            }
+            else if (Sse.IsSupported && Vector<int>.Count == 4)
+            {
+                var scalarMask = Sse.MoveMask(v.AsVector128().As<int, float>());
+                return BitOperations.TrailingZeroCount(scalarMask);
+            }
+            else
+            {
+                Debug.Assert(Vector<int>.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<int>.Count; ++i)
+                {
+                    if (v[i] == -1)
+                        return i;
+                }
+            }
+            return -1;
+        }
+        /// <summary>
+        /// 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.
+        /// </summary>
+        /// <param name="v">Vector to examine.</param>
+        /// <returns>Number of lanes that occur at or before the last set lane.</returns>
+        [MethodImpl(MethodImplOptions.AggressiveInlining)]
+        public static int GetLastSetLaneCount(Vector<int> v)
+        {
+            //TODO: Cross platform intrinsics rewrite
+            if (Avx.IsSupported && Vector<int>.Count == 8)
+            {
+                var scalarMask = Avx.MoveMask(v.AsVector256().As<int, float>());
+                return 32 - BitOperations.LeadingZeroCount((uint)scalarMask);
+            }
+            else if (Sse.IsSupported && Vector<int>.Count == 4)
+            {
+                var scalarMask = Sse.MoveMask(v.AsVector128().As<int, float>());
+                return 32 - BitOperations.LeadingZeroCount((uint)scalarMask);
+            }
+            else
+            {
+                Debug.Assert(Vector<int>.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<int>.Count - 1; i >= 0; --i)
+                {
+                    if (v[i] == -1)
+                        return i + 1;
+                }
+            }
+            return 0;
+        }
+
     }
 }
diff --git a/BepuUtilities/Collections/IndexSet.cs b/BepuUtilities/Collections/IndexSet.cs
index b8d934df7..a3761daad 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
@@ -25,12 +26,12 @@ 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;
         }
 
-        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<ulong>();
@@ -38,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);
@@ -66,7 +67,7 @@ public bool Contains(int index)
         /// <param name="indexList">List of indices to check for in the batch.</param>
         /// <returns>True if none of the indices are present in the set, false otherwise.</returns>
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public unsafe bool CanFit(Span<int> indexList)
+        public bool CanFit(Span<int> indexList)
         {
             for (int i = 0; i < indexList.Length; ++i)
             {
@@ -79,37 +80,92 @@ public unsafe bool CanFit(Span<int> 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;
         }
+
+        /// <summary>
+        /// Sets an index in the set to contained without checking capacity or whether it is already set.
+        /// </summary>
+        /// <param name="index">Index to add.</param>
+        /// <remarks>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.</remarks>
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public void Add(int index, BufferPool pool)
+        public void SetUnsafely(int index)
+        {
+            SetUnsafely(index, index >> shift);
+        }
+
+        /// <summary>
+        /// Sets an index in the set to contained without checking whether it is already set.
+        /// </summary>
+        /// <param name="index">Index to add.</param>
+        /// <param name="pool">Pool to reuse the set if necessary.</param>
+        /// <remarks>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.</remarks>
+
+        [MethodImpl(MethodImplOptions.AggressiveInlining)]
+        public void Set(int index, IUnmanagedMemoryPool 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));
+                InternalResizeForBundleCount(pool, (int)BitOperations.RoundUpToPowerOf2((uint)(bundleIndex + 1)));
             }
-            AddUnsafely(index, bundleIndex);
+            SetUnsafely(index, index >> shift);
+        }
+
+        /// <summary>
+        /// Marks an index in the set as uncontained without checking whether it is already set.
+        /// </summary>
+        /// <param name="index">Index to add.</param>
+        /// <remarks>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.</remarks>
+
+        [MethodImpl(MethodImplOptions.AggressiveInlining)]
+        public void Unset(int index)
+        {
+            Flags[index >> shift] &= ~(1ul << (index & mask));
         }
 
+
+        /// <summary>
+        /// Adds an index to the set without checking capacity.
+        /// </summary>
+        /// <param name="index">Index to add.</param>
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
         public void AddUnsafely(int index)
         {
-            AddUnsafely(index, index >> shift);
+            Debug.Assert((Flags[index >> shift] & (1ul << (index & mask))) == 0, "Cannot add if it's already present!");
+            SetUnsafely(index, index >> shift);
         }
 
+        /// <summary>
+        /// Adds an index to the set.
+        /// </summary>
+        /// <param name="index">Index to add.</param>
+        /// <param name="pool">Pool to use to resize the set if necessary.</param>
+        [MethodImpl(MethodImplOptions.AggressiveInlining)]
+        public void Add(int index, IUnmanagedMemoryPool 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, (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);
+        }
+
+
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
         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()
@@ -117,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)
             {
@@ -126,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<ulong>(GetBundleCapacity(indexCapacity));
             if (Flags.Length > desiredBundleCount)
@@ -134,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<ulong>(GetBundleCapacity(indexCapacity));
             if (Flags.Length != desiredBundleCount)
@@ -147,7 +203,7 @@ public void Resize(int indexCapacity, BufferPool pool)
         /// </summary>
         /// <remarks>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.</remarks>
-        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/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..83257534b 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
 {
@@ -169,8 +164,6 @@ public static void SortU8<T>(ref int inputKeys, ref T inputValues, ref int outpu
         /// </summary>
         /// <remarks>Only one invocation of the sort can be running at a time on a given instance of the sorter.</remarks>
         /// <typeparam name="TValue">Type of the values to sort.</typeparam>
-        /// <typeparam name="TKeySpan">Type of the span that holds the keys to sort.</typeparam>
-        /// <typeparam name="TValueSpan">Type of the span that holds the values to sort.</typeparam>
         /// <param name="inputKeys">Span containing the keys to sort.</param>
         /// <param name="inputValues">Span containing the values to sort.</param>
         /// <param name="scratchKeys">Scratch array to write temporary results into.</param>
diff --git a/BepuUtilities/Collections/MSBRadixSort.cs b/BepuUtilities/Collections/MSBRadixSort.cs
index 1925b141c..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
 {
@@ -155,9 +151,7 @@ public unsafe static void SortU32<T>(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)
             {
diff --git a/BepuUtilities/Collections/QuickDictionary.cs b/BepuUtilities/Collections/QuickDictionary.cs
index 1a6742d74..f1f258c13 100644
--- a/BepuUtilities/Collections/QuickDictionary.cs
+++ b/BepuUtilities/Collections/QuickDictionary.cs
@@ -156,7 +156,6 @@ public QuickDictionary(ref Buffer<TKey> initialKeySpan, ref Buffer<TValue> initi
         /// <param name="initialKeySpan">Span to use as backing memory of the dictionary keys.</param>
         /// <param name="initialValueSpan">Span to use as backing memory of the dictionary values.</param>
         /// <param name="initialTableSpan">Span to use as backing memory of the table. Must be zeroed.</param>
-        /// <param name="comparer">Comparer to use for the dictionary.</param>
         /// <param name="tablePowerOffset">Target size of the table relative to the number of stored elements.</param>
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
         public QuickDictionary(ref Buffer<TKey> initialKeySpan, ref Buffer<TValue> initialValueSpan, ref Buffer<int> initialTableSpan, int tablePowerOffset = 2)
@@ -187,7 +186,6 @@ public QuickDictionary(int initialCapacity, int tableSizePower, IUnmanagedMemory
         /// </summary>
         /// <param name="initialCapacity">Initial target size of the key and value spans. The size of the initial buffer will be at least as large as the initialCapacity.</param>
         /// <param name="tableSizePower">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.</param>
-        /// <param name="comparer">Comparer to use in the dictionary.</param>
         /// <param name="pool">Pool used for spans.</param>   
         [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.
         /// </summary>
         /// <param name="initialCapacity">Initial target size of the key and value spans. The size of the initial buffer will be at least as large as the initialCapacity.</param>
-        /// <param name="comparer">Comparer to use in the dictionary.</param>
         /// <param name="pool">Pool used for spans.</param>   
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
         public QuickDictionary(int initialCapacity, IUnmanagedMemoryPool pool)
@@ -239,7 +236,7 @@ public void Resize(ref Buffer<TKey> newKeySpan, ref Buffer<TValue> 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<T>, 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;
@@ -273,12 +270,6 @@ public void Resize(int newSize, IUnmanagedMemoryPool pool)
         /// <summary>
         /// Returns the resources associated with the dictionary to pools.
         /// </summary>
-        /// <param name="keyPool">Pool used for key spans.</param>   
-        /// <param name="valuePool">Pool used for value spans.</param>   
-        /// <param name="tablePool">Pool used for table spans.</param>
-        /// <typeparam name="TKeyPool">Type of the pool used for key spans.</typeparam>
-        /// <typeparam name="TValuePool">Type of the pool used for value spans.</typeparam>
-        /// <typeparam name="TTablePool">Type of the pool used for table spans.</typeparam>
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
         public void Dispose(IUnmanagedMemoryPool pool)
         {
@@ -304,8 +295,7 @@ public void EnsureCapacity(int count, IUnmanagedMemoryPool pool)
         /// <summary>
         /// Shrinks the internal buffers to the smallest acceptable size and releases the old buffers to the pools.
         /// </summary>
-        /// <param name="pool">Pool used for spans.</param>   
-        /// <param name="element">Element to add.</param>
+        /// <param name="pool">Pool used for spans.</param>
         public void Compact(IUnmanagedMemoryPool pool)
         {
             Validate();
@@ -364,7 +354,7 @@ public int IndexOf(TKey key)
         /// </summary>
         /// <param name="key">Key to get the index of.</param>
         /// <returns>The index of the key if the key exists in the dictionary, -1 otherwise.</returns>
-        public int IndexOfRef(ref TKey key)
+        public int IndexOf(ref TKey key)
         {
             Validate();
             GetTableIndices(ref key, out int tableIndex, out int objectIndex);
@@ -387,7 +377,7 @@ public bool ContainsKey(TKey key)
         /// </summary>
         /// <param name="key">Key to test for.</param>
         /// <returns>True if the key already belongs to the dictionary, false otherwise.</returns>
-        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 +407,7 @@ public bool TryGetValue(TKey key, out TValue value)
         /// <param name="key">Key to look up.</param>
         /// <param name="value">Value associated with the specified key.</param>
         /// <returns>True if a value was found, false otherwise.</returns>
-        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 +419,83 @@ public bool TryGetValueRef(ref TKey key, out TValue value)
             return false;
         }
 
+        /// <summary>
+        /// 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.
+        /// </summary>
+        /// <param name="key">Key to find or allocate a slot for.</param>
+        /// <param name="slotIndex">Index of the found or allocated slot.</param>
+        /// <returns>True if the key was already present in the dictionary, false otherwise.</returns>
+        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;
+        }
+
+        /// <summary>
+        /// 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.
+        /// </summary>
+        /// <param name="key">Key to find or allocate a slot for.</param>
+        /// <param name="slotIndex">Index of the found or allocated slot.</param>
+        /// <returns>True if the key was already present in the dictionary, false otherwise.</returns>
+        [MethodImpl(MethodImplOptions.AggressiveInlining)]
+        public bool FindOrAllocateSlotUnsafely(TKey key, out int slotIndex)
+        {
+            return FindOrAllocateSlotUnsafely(ref key, out slotIndex);
+        }
+
+        /// <summary>
+        /// 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.
+        /// </summary>
+        /// <param name="key">Key to find or allocate a slot for.</param>
+        /// <param name="pool">Pool used to resize the container if necessary to allocate.</param>
+        /// <param name="slotIndex">Index of the found or allocated slot.</param>
+        /// <returns>True if the key was already present in the dictionary, false otherwise.</returns>
+        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;
+        }
+
+        /// <summary>
+        /// 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.
+        /// </summary>
+        /// <param name="key">Key to find or allocate a slot for.</param>
+        /// <param name="pool">Pool used to resize the container if necessary to allocate.</param>
+        /// <param name="slotIndex">Index of the found or allocated slot.</param>
+        /// <returns>True if the key was already present in the dictionary, false otherwise.</returns>
+        [MethodImpl(MethodImplOptions.AggressiveInlining)]
+        public bool FindOrAllocateSlot(TKey key, BufferPool pool, out int slotIndex)
+        {
+            return FindOrAllocateSlot(ref key, pool, out slotIndex);
+        }
+
         /// <summary>
         /// 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 +504,7 @@ public bool TryGetValueRef(ref TKey key, out TValue value)
         /// <param name="value">Value of the pair to add.</param>
         /// <returns>True if the pair was added to the dictionary, false if the key was already present and its pair was replaced.</returns>
         //[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 +535,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);
         }
 
         /// <summary>
@@ -478,7 +545,7 @@ public bool AddAndReplaceUnsafely(TKey key, in TValue value)
         /// <param name="value">Value of the pair to add.</param>
         /// <returns>True if the pair was added to the dictionary, false if the key was already present.</returns>
         //[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 +572,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);
         }
 
         /// <summary>
@@ -517,7 +584,7 @@ public bool AddUnsafely(TKey key, in TValue value)
         /// <param name="pool">Pool used for spans.</param>   
         /// <returns>True if the pair was added to the dictionary, false if the key was already present and its pair was replaced.</returns>
         [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 +595,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);
         }
 
         /// <summary>
@@ -542,7 +609,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);
         }
 
         /// <summary>
@@ -551,10 +618,9 @@ public bool AddAndReplace(TKey key, in TValue value, IUnmanagedMemoryPool pool)
         /// <param name="key">Key of the pair to add.</param>
         /// <param name="value">Value of the pair to add.</param>
         /// <param name="pool">Pool used for spans.</param>   
-        /// <typeparam name="TPool">Type of the pool used for spans.</typeparam>
         /// <returns>True if the pair was added to the dictionary, false if the key was already present.</returns>
         [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 +633,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);
         }
 
         /// <summary>
@@ -576,12 +642,11 @@ public bool AddRef(ref TKey key, in TValue value, IUnmanagedMemoryPool pool)
         /// <param name="key">Key of the pair to add.</param>
         /// <param name="value">Value of the pair to add.</param>
         /// <param name="pool">Pool to pull resources from and to return resources to.</param>   
-        /// <typeparam name="TPool">Type of the pool to use.</typeparam>
         /// <returns>True if the pair was added to the dictionary, false if the key was already present.</returns>
         [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 +711,7 @@ public void FastRemove(int tableIndex, int elementIndex)
         /// </summary>
         /// <param name="key">Key of the pair to remove.</param>
         /// <returns>True if the key was found and removed, false otherwise.</returns>
-        public bool FastRemoveRef(ref TKey key)
+        public bool FastRemove(ref TKey key)
         {
             Validate();
             //Find it.
@@ -668,7 +733,7 @@ public bool FastRemoveRef(ref TKey key)
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
         public bool FastRemove(TKey key)
         {
-            return FastRemoveRef(ref key);
+            return FastRemove(ref key);
         }
 
         /// <summary>
diff --git a/BepuUtilities/Collections/QuickList.cs b/BepuUtilities/Collections/QuickList.cs
index dd6f0b3f9..0baafd3e0 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.
     /// </remarks>
     /// <typeparam name="T">Type of the elements in the list.</typeparam>
+    [StructLayout(LayoutKind.Sequential, Pack = 8)]
     public struct QuickList<T> where T : unmanaged
     {
         /// <summary>
@@ -174,7 +176,7 @@ public void Compact(IUnmanagedMemoryPool pool)
         /// <param name="start">Start index of the added range.</param>
         /// <param name="count">Number of elements in the added range.</param>
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public void AddRangeUnsafely(in Buffer<T> span, int start, int count)
+        public void AddRangeUnsafely(Buffer<T> span, int start, int count)
         {
             Validate();
             ValidateUnsafeAdd(count);
@@ -190,7 +192,7 @@ public void AddRangeUnsafely(in Buffer<T> span, int start, int count)
         /// <param name="count">Number of elements in the added range.</param>
         /// <param name="pool">Pool used to obtain a new span if needed.</param>
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public void AddRange(in Buffer<T> span, int start, int count, IUnmanagedMemoryPool pool)
+        public void AddRange(Buffer<T> span, int start, int count, IUnmanagedMemoryPool pool)
         {
             EnsureCapacity(Count + count, pool);
             AddRangeUnsafely(span, start, count);
@@ -200,10 +202,8 @@ public void AddRange(in Buffer<T> span, int start, int count, IUnmanagedMemoryPo
         /// Adds the elements of a buffer to the QuickList without checking capacity.
         /// </summary>
         /// <param name="span">Span of elements to add.</param>
-        /// <param name="start">Start index of the added range.</param>
-        /// <param name="count">Number of elements in the added range.</param>
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public void AddRangeUnsafely(in Buffer<T> span)
+        public void AddRangeUnsafely(Buffer<T> span)
         {
             AddRangeUnsafely(span, 0, span.Length);
         }
@@ -212,11 +212,9 @@ public void AddRangeUnsafely(in Buffer<T> span)
         /// Adds the elements of a buffer to the QuickList.
         /// </summary>
         /// <param name="span">Span of elements to add.</param>
-        /// <param name="start">Start index of the added range.</param>
-        /// <param name="count">Number of elements in the added range.</param>
         /// <param name="pool">Pool used to obtain a new span if needed.</param>
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public void AddRange(in Buffer<T> span, IUnmanagedMemoryPool pool)
+        public void AddRange(Buffer<T> span, IUnmanagedMemoryPool pool)
         {
             EnsureCapacity(Count + span.Length, pool);
             AddRangeUnsafely(span, 0, span.Length);
@@ -229,7 +227,7 @@ public void AddRange(in Buffer<T> span, IUnmanagedMemoryPool pool)
         /// <param name="start">Start index of the added range.</param>
         /// <param name="count">Number of elements in the added range.</param>
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public void AddRangeUnsafely(in Span<T> span, int start, int count)
+        public void AddRangeUnsafely(Span<T> span, int start, int count)
         {
             Validate();
             ValidateUnsafeAdd(count);
@@ -245,7 +243,7 @@ public void AddRangeUnsafely(in Span<T> span, int start, int count)
         /// <param name="count">Number of elements in the added range.</param>
         /// <param name="pool">Pool used to obtain a new span if needed.</param>
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public void AddRange(in Span<T> span, int start, int count, IUnmanagedMemoryPool pool)
+        public void AddRange(Span<T> span, int start, int count, IUnmanagedMemoryPool pool)
         {
             EnsureCapacity(Count + count, pool);
             AddRangeUnsafely(span, start, count);
@@ -255,10 +253,8 @@ public void AddRange(in Span<T> span, int start, int count, IUnmanagedMemoryPool
         /// Adds the elements of a span to the QuickList without checking capacity.
         /// </summary>
         /// <param name="span">Span of elements to add.</param>
-        /// <param name="start">Start index of the added range.</param>
-        /// <param name="count">Number of elements in the added range.</param>
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public void AddRangeUnsafely(in Span<T> span)
+        public void AddRangeUnsafely(Span<T> span)
         {
             AddRangeUnsafely(span, 0, span.Length);
         }
@@ -267,11 +263,9 @@ public void AddRangeUnsafely(in Span<T> span)
         /// Adds the elements of a span to the QuickList.
         /// </summary>
         /// <param name="span">Span of elements to add.</param>
-        /// <param name="start">Start index of the added range.</param>
-        /// <param name="count">Number of elements in the added range.</param>
         /// <param name="pool">Pool used to obtain a new span if needed.</param>
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public void AddRange(in Span<T> span, IUnmanagedMemoryPool pool)
+        public void AddRange(Span<T> span, IUnmanagedMemoryPool pool)
         {
             EnsureCapacity(Count + span.Length, pool);
             AddRangeUnsafely(span, 0, span.Length);
@@ -284,7 +278,7 @@ public void AddRange(in Span<T> span, IUnmanagedMemoryPool pool)
         /// <param name="start">Start index of the added range.</param>
         /// <param name="count">Number of elements in the added range.</param>
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public void AddRangeUnsafely(in ReadOnlySpan<T> span, int start, int count)
+        public void AddRangeUnsafely(ReadOnlySpan<T> span, int start, int count)
         {
             Validate();
             ValidateUnsafeAdd(count);
@@ -300,7 +294,7 @@ public void AddRangeUnsafely(in ReadOnlySpan<T> span, int start, int count)
         /// <param name="count">Number of elements in the added range.</param>
         /// <param name="pool">Pool used to obtain a new span if needed.</param>
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public void AddRange(in ReadOnlySpan<T> span, int start, int count, IUnmanagedMemoryPool pool)
+        public void AddRange(ReadOnlySpan<T> span, int start, int count, IUnmanagedMemoryPool pool)
         {
             EnsureCapacity(Count + count, pool);
             AddRangeUnsafely(span, start, count);
@@ -310,10 +304,8 @@ public void AddRange(in ReadOnlySpan<T> span, int start, int count, IUnmanagedMe
         /// Adds the elements of a span to the QuickList without checking capacity.
         /// </summary>
         /// <param name="span">Span of elements to add.</param>
-        /// <param name="start">Start index of the added range.</param>
-        /// <param name="count">Number of elements in the added range.</param>
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public void AddRangeUnsafely(in ReadOnlySpan<T> span)
+        public void AddRangeUnsafely(ReadOnlySpan<T> span)
         {
             AddRangeUnsafely(span, 0, span.Length);
         }
@@ -322,11 +314,9 @@ public void AddRangeUnsafely(in ReadOnlySpan<T> span)
         /// Adds the elements of a span to the QuickList.
         /// </summary>
         /// <param name="span">Span of elements to add.</param>
-        /// <param name="start">Start index of the added range.</param>
-        /// <param name="count">Number of elements in the added range.</param>
         /// <param name="pool">Pool used to obtain a new span if needed.</param>
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public void AddRange(in ReadOnlySpan<T> span, IUnmanagedMemoryPool pool)
+        public void AddRange(ReadOnlySpan<T> span, IUnmanagedMemoryPool pool)
         {
             EnsureCapacity(Count + span.Length, pool);
             AddRangeUnsafely(span, 0, span.Length);
@@ -382,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);
@@ -435,7 +426,7 @@ public int IndexOf(T element)
         /// <param name="element">Element to find.</param>
         /// <returns>Index of the element in the list if present, -1 otherwise.</returns>
         [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 +453,7 @@ public int IndexOf<TPredicate>(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 +499,7 @@ public bool Remove<TPredicate>(ref TPredicate predicate) where TPredicate : IPre
         /// <param name="element">Element to remove from the list.</param>
         /// <returns>True if the element was present and was removed, false otherwise.</returns>
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public bool FastRemoveRef(ref T element)
+        public bool FastRemove(ref T element)
         {
             Validate();
             var index = IndexOf(element);
@@ -528,7 +519,7 @@ public bool FastRemoveRef(ref T element)
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
         public bool FastRemove(T element)
         {
-            return FastRemoveRef(ref element);
+            return FastRemove(ref element);
         }
 
         /// <summary>
@@ -640,9 +631,9 @@ public bool Contains(T element)
         /// </returns>
         /// <param name="element">The object to locate in the collection.</param>
         [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;
         }
 
         /// <summary>
@@ -676,14 +667,14 @@ public unsafe static implicit operator Span<T>(in QuickList<T> list)
             return new Span<T>(list.Span.Memory, list.Count);
         }
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public unsafe static implicit operator ReadOnlySpan<T>(in QuickList<T> list)
+        public unsafe static implicit operator ReadOnlySpan<T>(QuickList<T> list)
         {
             return new ReadOnlySpan<T>(list.Span.Memory, list.Count);
         }
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public unsafe static implicit operator Buffer<T>(in QuickList<T> list)
+        public unsafe static implicit operator Buffer<T>(QuickList<T> list)
         {
-            return new Buffer<T>(list.Span.Memory, list.Count);
+            return new Buffer<T>(list.Span.Memory, list.Count, list.Span.Id);
         }
 
 
diff --git a/BepuUtilities/Collections/QuickQueue.cs b/BepuUtilities/Collections/QuickQueue.cs
index ec62172b7..8f7947291 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;
         }
 
         /// <summary>
@@ -90,7 +91,7 @@ static int GetCapacityMask(int spanLength)
         /// </summary>
         /// <param name="initialSpan">Span to use as backing memory to begin with.</param>
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public QuickQueue(ref Buffer<T> initialSpan)
+        public QuickQueue(Buffer<T> initialSpan)
         {
             Span = initialSpan;
             Count = 0;
@@ -212,55 +213,100 @@ public void Compact(IUnmanagedMemoryPool pool)
         }
 
         /// <summary>
-        /// 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.
         /// </summary>
-        /// <param name="element">Item to enqueue.</param>
+        /// <returns>Reference to the enqueued slot.</returns>
         [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))];
         }
 
         /// <summary>
-        /// 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.
         /// </summary>
-        /// <param name="element">Item to enqueue.</param>
+        /// <returns>Reference to the enqueued slot.</returns>
         [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))];
         }
-        
         /// <summary>
-        /// 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.
         /// </summary>
-        /// <param name="element">Item to enqueue.</param>
+        /// <param name="pool">Pool to use to resize the queue's internal buffer if necessary.</param>
         [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();
         }
 
         /// <summary>
-        /// 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.
         /// </summary>
-        /// <param name="element">Item to enqueue.</param>
+        /// <param name="pool">Pool to use to resize the queue's internal buffer if necessary.</param>
         [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();
+        }
+
+        /// <summary>
+        /// 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.
+        /// </summary>
+        /// <param name="element">Item to enqueue.</param>
+        [MethodImpl(MethodImplOptions.AggressiveInlining)]
+        public void EnqueueUnsafely(in T element)
+        {
+            EnqueueUnsafely() = element;
+        }
+
+        /// <summary>
+        /// 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.
+        /// </summary>
+        /// <param name="element">Item to enqueue.</param>
+        [MethodImpl(MethodImplOptions.AggressiveInlining)]
+        public void EnqueueFirstUnsafely(in T element)
+        {
+            EnqueueFirstUnsafely() = element;
+        }
+
+        /// <summary>
+        /// Enqueues the element to the end of the queue, incrementing the last index.
+        /// </summary>
+        /// <param name="element">Item to enqueue.</param>
+        /// <param name="pool">Pool to use to resize the queue's internal buffer if necessary.</param>
+        [MethodImpl(MethodImplOptions.AggressiveInlining)]
+        public void Enqueue(in T element, IUnmanagedMemoryPool pool)
+        {
+            Enqueue(pool) = element;
+        }
+
+        /// <summary>
+        /// Enqueues the element to the start of the queue, decrementing the first index.
+        /// </summary>
+        /// <param name="element">Item to enqueue.</param>
+        /// <param name="pool">Pool to use to resize the queue's internal buffer if necessary.</param>
+        [MethodImpl(MethodImplOptions.AggressiveInlining)]
+        public void EnqueueFirst(in T element, IUnmanagedMemoryPool pool)
+        {
+            EnqueueFirst(pool) = element;
         }
 
         /// <summary>
@@ -274,7 +320,7 @@ public T Dequeue()
             if (Count == 0)
                 throw new InvalidOperationException("The queue is empty.");
             var element = Span[FirstIndex];
-            DeleteFirst();
+            IncrementFirst();
             return element;
 
         }
@@ -290,9 +336,8 @@ public T DequeueLast()
             if (Count == 0)
                 throw new InvalidOperationException("The queue is empty.");
             var element = Span[LastIndex];
-            DeleteLast();
+            DecrementLast();
             return element;
-
         }
 
         /// <summary>
@@ -307,7 +352,7 @@ public bool TryDequeue(out T element)
             if (Count > 0)
             {
                 element = Span[FirstIndex];
-                DeleteFirst();
+                IncrementFirst();
                 return true;
             }
             element = default;
@@ -327,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;
+        }
+
+        /// <summary>
+        /// 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.
+        /// </summary>
+        /// <returns>Reference to the slot removed from the queue.</returns>
+        /// <remarks>Be very careful with this function; it is easy to accidentally destroy your foot in sneaky ways.
+        /// Consider what happens if you call <see cref="EnqueueFirstUnsafely()"/> after this function- both functions will return references to the same slot.</remarks>
+        [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;
+        }
 
+        /// <summary>
+        /// 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.
+        /// </summary>
+        /// <returns>Reference to the slot removed from the queue.</returns>
+        /// <remarks>Be very careful with this function; it is easy to accidentally destroy your foot in sneaky ways.
+        /// Consider what happens if you call <see cref="EnqueueUnsafely()"/> after this function- both functions will return references to the same slot.</remarks>
+        [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;
         }
@@ -360,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.
@@ -382,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();
             }
         }
 
@@ -419,7 +494,7 @@ public void Clear()
         }
 
         /// <summary>
-        /// 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.
         /// </summary>
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
         public void FastClear()
@@ -485,7 +560,7 @@ void ValidateIndex(int index)
         [Conditional("DEBUG")]
         static void ValidateSpanCapacity(ref Buffer<T> 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/Collections/QuickSet.cs b/BepuUtilities/Collections/QuickSet.cs
index fa8656fde..65f435ec0 100644
--- a/BepuUtilities/Collections/QuickSet.cs
+++ b/BepuUtilities/Collections/QuickSet.cs
@@ -175,7 +175,7 @@ public void Resize(ref Buffer<T> newSpan, ref Buffer<int> 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<T>, 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;
@@ -232,7 +232,6 @@ public void EnsureCapacity(int count, IUnmanagedMemoryPool pool)
         /// <summary>
         /// Shrinks the internal buffers to the smallest acceptable size and releases the old buffers to the pools.
         /// </summary>
-        /// <param name="element">Element to add.</param>
         /// <param name="pool">Pool used for spans.</param>   
         public void Compact(IUnmanagedMemoryPool pool)
         {
@@ -289,7 +288,7 @@ public int IndexOf(T element)
         /// <param name="element">Element to get the index of.</param>
         /// <returns>The index of the element if the element exists in the set, -1 otherwise.</returns>
         [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 +311,7 @@ public bool Contains(T element)
         /// <param name="element">Element to test for.</param>
         /// <returns>True if the element already belongs to the set, false otherwise.</returns>
         [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 +324,7 @@ public bool ContainsRef(ref T element)
         /// <param name="element">Element to add.</param>
         /// <returns>True if the element was added to the set, false if the element was already present and was instead replaced.</returns>
         //[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 +352,7 @@ public bool AddAndReplaceUnsafelyRef(ref T element)
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
         public bool AddAndReplaceUnsafely(T element)
         {
-            return AddAndReplaceUnsafelyRef(ref element);
+            return AddAndReplaceUnsafely(ref element);
         }
 
         /// <summary>
@@ -363,7 +362,7 @@ public bool AddAndReplaceUnsafely(T element)
         /// <param name="element">Element to add.</param>
         /// <returns>True if the element was added to the set, false if the element was already present.</returns>
         //[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 +388,7 @@ public bool AddUnsafelyRef(ref T element)
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
         public bool AddUnsafely(T element)
         {
-            return AddUnsafelyRef(ref element);
+            return AddUnsafely(ref element);
         }
 
         /// <summary>
@@ -400,7 +399,7 @@ public bool AddUnsafely(T element)
         /// <param name="pool">Pool used for spans.</param>   
         /// <returns>True if the element was added to the set, false if the element was already present and was instead replaced.</returns>
         [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 +410,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 +421,7 @@ public bool AddAndReplaceRef(ref T element, IUnmanagedMemoryPool pool)
         /// <param name="pool">Pool used for spans.</param>   
         /// <returns>True if the element was added to the set, false if the element was already present.</returns>
         [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 +432,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 +446,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 +459,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 +523,7 @@ public void FastRemove(int tableIndex, int elementIndex)
         /// </summary>
         /// <param name="element">Element to remove.</param>
         /// <returns>True if the element was found and removed, false otherwise.</returns>
-        public bool FastRemoveRef(ref T element)
+        public bool FastRemove(ref T element)
         {
             Validate();
             //Find it.
@@ -546,7 +545,7 @@ public bool FastRemoveRef(ref T element)
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
         public bool FastRemove(T element)
         {
-            return FastRemoveRef(ref element);
+            return FastRemove(ref element);
         }
 
 
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
new file mode 100644
index 000000000..63d6cac7d
--- /dev/null
+++ b/BepuUtilities/Collections/VectorizedSorts.cs
@@ -0,0 +1,122 @@
+using BepuUtilities.Memory;
+using System;
+using System.Diagnostics;
+using System.Runtime.Intrinsics;
+
+namespace BepuUtilities.Collections
+{
+    /// <summary>
+    /// Specialized sorts that can go fast sometimes, but often come with special requirements.
+    /// </summary>
+    public static class VectorizedSorts
+    {
+        /// <summary>
+        /// Computes the mapping from current index to ascending sorted index for all elements in the paddedKeys. The paddedKeys buffer is not actually sorted.
+        /// <para>This function requires that <see cref="Vector256.IsHardwareAccelerated"/> or <see cref="Vector128.IsHardwareAccelerated"/>.</para>
+        /// </summary>
+        /// <param name="paddedKeys">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 <see cref="Vector256.IsHardwareAccelerated"/> or 4 elements if only <see cref="Vector128.IsHardwareAccelerated"/>.</param>
+        /// <param name="paddedTargetIndices">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 <see cref="Vector256.IsHardwareAccelerated"/> or 4 elements if only <see cref="Vector128.IsHardwareAccelerated"/>.</param>
+        /// <param name="elementCount">Number of actual elements in the paddedKeys buffer.</param>
+        public unsafe static void VectorCountingSort(Buffer<float> paddedKeys, Buffer<int> 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<int>.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<int>.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/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/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!
         /// </summary>
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public static unsafe ref T Get<T>(ref Vector<T> vector, int index) where T : struct
+        public static ref T Get<T>(ref Vector<T> 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<Vector<T>, T>(ref vector), index);
diff --git a/BepuUtilities/IThreadDispatcher.cs b/BepuUtilities/IThreadDispatcher.cs
index 0c5d01ad9..03efc0bcc 100644
--- a/BepuUtilities/IThreadDispatcher.cs
+++ b/BepuUtilities/IThreadDispatcher.cs
@@ -3,6 +3,14 @@
 
 namespace BepuUtilities
 {
+    /// <summary>
+    /// Function to be invoked on a worker thread in an <see cref="IThreadDispatcher"/>. Provides the unmanaged context of the dispatch.
+    /// </summary>
+    /// <param name="workerIndex">Index of the worker in the dispatcher executing this function.</param>
+    /// <param name="context">Pointer to the context of this dispatch, if any.</param>
+    /// 
+    public unsafe delegate void ThreadDispatcherWorker(int workerIndex, void* context);
+
     /// <summary>
     /// Provides multithreading dispatch primitives, a thread count, and per thread resource pools for the simulation to use.
     /// </summary>
@@ -14,7 +22,7 @@ namespace BepuUtilities
     /// <para>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.</para>
     /// </remarks>
-    public interface IThreadDispatcher
+    public unsafe interface IThreadDispatcher
     {
         /// <summary>
         /// Gets the number of workers available in the thread dispatcher.
@@ -24,20 +32,42 @@ public interface IThreadDispatcher
         /// </remarks>
         int ThreadCount { get; }
 
+        /// <summary>
+        /// Gets the unmanaged context associated with the current dispatch, if any.
+        /// </summary>
+        /// <remarks>This is intended to help pass information to workers, especially those defined with function pointers that can't just include extra state in closures.</remarks>
+        public void* UnmanagedContext { get; }
+
+        /// <summary>
+        /// Gets the managed context associated with the current dispatch, if any.
+        /// </summary>
+        /// <remarks>This is intended to help pass information to workers, especially those defined with function pointers that can't just include extra state in closures.</remarks>
+        public object ManagedContext { get; }
+
         /// <summary>
         /// Dispatches all the available workers.
         /// </summary>
-        /// <param name="loopBody">Delegate to be invoked on for every worker.</param>
-        void DispatchWorkers(Action<int> workerBody);
+        /// <param name="workerBody">Function pointer to be invoked on every worker. Matches the signature of the <see cref="ThreadDispatcherWorker"/>.</param>
+        /// <param name="maximumWorkerCount">Maximum number of workers to dispatch.</param>
+        /// <param name="unmanagedContext">Unmanaged context of the dispatch.</param>
+        /// <param name="managedContext">Managed context of the dispatch.</param>
+        void DispatchWorkers(delegate*<int, IThreadDispatcher, void> workerBody, int maximumWorkerCount = int.MaxValue, void* unmanagedContext = null, object managedContext = null);
+
+        /// <summary>
+        /// Dispatches all the available workers with a null context.
+        /// </summary>
+        /// <param name="workerBody">Delegate to be invoked on every worker.</param>
+        /// <param name="maximumWorkerCount">Maximum number of workers to dispatch.</param>
+        /// <param name="unmanagedContext">Unmanaged context of the dispatch.</param>
+        /// <param name="managedContext">Managed context of the dispatch.</param>
+        void DispatchWorkers(Action<int> workerBody, int maximumWorkerCount = int.MaxValue, void* unmanagedContext = null, object managedContext = null);
 
         /// <summary>
-        /// 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.
         /// </summary>
-        /// <remarks>All usages of the memory pool within the simulation are guaranteed to return thread pool memory before the function returns. In other words,
+        /// <remarks>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.</remarks>
-        /// <param name="workerIndex">Index of the worker to grab the pool for.</param>
-        /// <returns>The memory pool for the specified worker index.</returns>
-        BufferPool GetThreadMemoryPool(int workerIndex);
+        WorkerBufferPools WorkerPools { get; }
     }
 }
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);
+        }
     }
 }
diff --git a/BepuUtilities/Int3.cs b/BepuUtilities/Int3.cs
index 8888e7117..68f76d719 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
@@ -14,7 +13,7 @@ public struct Int3 : IEquatable<Int3>, IEqualityComparerRef<Int3>
         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
new file mode 100644
index 000000000..583310786
--- /dev/null
+++ b/BepuUtilities/Int4.cs
@@ -0,0 +1,70 @@
+using BepuUtilities.Collections;
+using System;
+using System.Runtime.CompilerServices;
+
+namespace BepuUtilities
+{
+    /// <summary>
+    /// A set of 4 integers, useful for spatial hashing.
+    /// </summary>
+    public struct Int4 : IEquatable<Int4>, IEqualityComparerRef<Int4>
+    {
+        public int X;
+        public int Y;
+        public int Z;
+        public int W;
+
+        public 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/BepuUtilities/MathChecker.cs b/BepuUtilities/MathChecker.cs
index 4b41a3623..897933e64 100644
--- a/BepuUtilities/MathChecker.cs
+++ b/BepuUtilities/MathChecker.cs
@@ -22,7 +22,7 @@ public static bool IsInvalid(float f)
 
 
         /// <summary>
-        /// 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.
         /// </summary>
         [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.");
             }
         }
 
         /// <summary>
-        /// 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.
         /// </summary>
         [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.");
             }
         }
 
         /// <summary>
-        /// 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.
         /// </summary>
         [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.");
             }
         }
 
         /// <summary>
-        /// 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.
         /// </summary>
         [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.");
             }
         }
 
 
         /// <summary>
-        /// 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.
         /// </summary>
         [Conditional("CHECKMATH")]
@@ -87,7 +87,7 @@ public static void Validate(this Matrix3x3 m)
         }
 
         /// <summary>
-        /// 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.
         /// </summary>
         [Conditional("CHECKMATH")]
@@ -100,7 +100,7 @@ public static void Validate(this Matrix m)
         }
 
         /// <summary>
-        /// 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.
         /// </summary>
         [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.");
             }
         }
 
         /// <summary>
-        /// 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.
         /// </summary>
         [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.");
             }
         }
 
         /// <summary>
-        /// 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.
         /// </summary>
         [Conditional("CHECKMATH")]
@@ -142,7 +142,7 @@ public static void Validate(this Symmetric3x3 m)
         }
 
         /// <summary>
-        /// 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.
         /// </summary>
         [Conditional("CHECKMATH")]
@@ -153,7 +153,7 @@ public static void Validate(this AffineTransform a)
         }
 
         /// <summary>
-        /// 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.
         /// </summary>
         [Conditional("CHECKMATH")]
@@ -164,7 +164,7 @@ public static void Validate(this BoundingBox b)
         }
 
         /// <summary>
-        /// 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.
         /// </summary>
         [Conditional("CHECKMATH")]
@@ -178,7 +178,7 @@ public static void Validate(this BoundingSphere b)
         public static void Validate(this Vector<float> f, int laneCount = -1)
         {
             if (laneCount < -1 || laneCount > Vector<float>.Count)
-                throw new ArgumentException("Invalid lane count.");
+                Debug.Fail("Invalid lane count.");
             if (laneCount == -1)
                 laneCount = Vector<float>.Count;
             ref var casted = ref Unsafe.As<Vector<float>, float>(ref f);
@@ -187,7 +187,7 @@ public static void Validate(this Vector<float> 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<float> f, Vector<int> 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/BepuUtilities/MathHelper.cs b/BepuUtilities/MathHelper.cs
index e8c588fe8..f1cb16a45 100644
--- a/BepuUtilities/MathHelper.cs
+++ b/BepuUtilities/MathHelper.cs
@@ -1,6 +1,8 @@
 using System;
 using System.Numerics;
 using System.Runtime.CompilerServices;
+using System.Runtime.Intrinsics;
+using System.Runtime.Intrinsics.X86;
 
 namespace BepuUtilities
 {
@@ -192,147 +194,171 @@ 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).
 
         /// <summary>
-        /// 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.
         /// </summary>
         /// <param name="x">Value to take the cosine of.</param>
         /// <returns>Approximate cosine of the input value.</returns>
-        [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;
 
         }
         /// <summary>
-        /// 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.
         /// </summary>
         /// <param name="x">Value to take the sine of.</param>
         /// <returns>Approximate sine of the input value.</returns>
-        [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;
+        }
+
+        /// <summary>
+        /// Computes an approximation of arccos. Inputs outside of [-1, 1] are clamped. Maximum error less than 5.17e-07.
+        /// </summary>
+        /// <param name="x">Input value to the arccos function.</param>
+        /// <returns>Result of the arccos function.</returns>
+        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;
         }
 
         /// <summary>
-        /// 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.
         /// </summary>
         /// <param name="x">Values to take the cosine of.</param>
         /// <returns>Approximate cosine of the input values.</returns>
-        [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public static void Cos(in Vector<float> x, out Vector<float> result)
+        public static Vector<float> Cos(Vector<float> 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<float>(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<float> y;
-            y = Vector.ConditionalSelect(Vector.GreaterThan(periodX, new Vector<float>(PiOver2)), new Vector<float>(Pi) - periodX, periodX);
-            y = Vector.ConditionalSelect(Vector.GreaterThan(periodX, new Vector<float>(Pi)), new Vector<float>(-Pi) + periodX, y);
-            y = Vector.ConditionalSelect(Vector.GreaterThan(periodX, new Vector<float>(3 * PiOver2)), new Vector<float>(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<float>.One - 0.24f * y - 0.4266f * y2 + 0.110838f * y3;
-            var denominator = Vector<float>.One - 0.240082f * y + 0.0741637f * y2 - 0.0118786f * y3;
-            result = numerator / denominator;
-            result = Vector.ConditionalSelect(
-                Vector.BitwiseAnd(
-                    Vector.GreaterThan(periodX, new Vector<float>(PiOver2)),
-                    Vector.LessThan(periodX, new Vector<float>(3 * PiOver2))), -result, result);
+            var piOver2 = new Vector<float>(PiOver2);
+            var pi = new Vector<float>(Pi);
+            var pi3Over2 = new Vector<float>(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<float>(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<float>.One - 0.15082367674208508f * y - 0.4578088075324152f * y2 + 0.06955843390178032f * y3 + 0.021317031205957775f * y4 - 0.003436308368583229f * y5;
+            //var denominator = Vector<float>.One - 0.15082367538305258f * y + 0.04219116713777847f * y2 - 0.00585321045829395f * y3 + 0.0007451378206294365f * y4 - 0.00007650398834677185f * y5;
+            var numerator = ((((new Vector<float>(-0.003436308368583229f) * y + new Vector<float>(0.021317031205957775f)) * y + new Vector<float>(0.06955843390178032f)) * y - new Vector<float>(0.4578088075324152f)) * y - new Vector<float>(0.15082367674208508f)) * y + Vector<float>.One;
+            var denominator = ((((new Vector<float>(-0.00007650398834677185f) * y + new Vector<float>(0.0007451378206294365f)) * y - new Vector<float>(0.00585321045829395f)) * y + new Vector<float>(0.04219116713777847f)) * y - new Vector<float>(0.15082367538305258f)) * y + Vector<float>.One;
+            var result = numerator / denominator;
+            return Vector.ConditionalSelect(Vector.BitwiseAnd(Vector.GreaterThan(periodX, piOver2), Vector.LessThan(periodX, pi3Over2)), -result, result);
         }
         /// <summary>
-        /// 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.
         /// </summary>
         /// <param name="x">Value to take the sine of.</param>
         /// <returns>Approximate sine of the input value.</returns>
-        [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public static void Sin(in Vector<float> x, out Vector<float> result)
+        public static Vector<float> Sin(Vector<float> x)
         {
-            Cos(x - new Vector<float>(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<float>(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<float> y;
+            var pi = new Vector<float>(Pi);
+            var piOver2 = new Vector<float>(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<float>(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<float>.One + 0.061745651499203795f * y + 0.02672943625500751f * y2 + 0.003606014457152456f * y3 + 0.0001700784176413186f * y4 + 0.00009018370615921334f * y5;
+            var numerator = ((((0.0040507708755727605f * y - new Vector<float>(0.006685815219853882f)) * y - new Vector<float>(0.13993701695343166f)) * y + new Vector<float>(0.06174562337697123f)) * y + new Vector<float>(1.00000000151466040f)) * y;
+            var denominator = ((((new Vector<float>(0.00009018370615921334f) * y + new Vector<float>(0.0001700784176413186f)) * y + new Vector<float>(0.003606014457152456f)) * y + new Vector<float>(0.02672943625500751f)) * y + new Vector<float>(0.061745651499203795f)) * y + Vector<float>.One;
+            var result = numerator / denominator;
+            return Vector.ConditionalSelect(inSecondHalf, -result, result);
         }
 
-
         /// <summary>
-        /// 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.
         /// </summary>
         /// <param name="x">Input value to the arccos function.</param>
-        /// <param name="acos">Result of the arccos function.</param>
-        [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public static void ApproximateAcos(Vector<float> x, out Vector<float> acos)
-        {
-            //Adapted from Handbook of Mathematical Functions by Milton Abramowitz and Irene A. Stegun.
-            var negate = Vector.ConditionalSelect(Vector.LessThan(x, Vector<float>.Zero), Vector<float>.One, Vector<float>.Zero);
-            x = Vector.Abs(x);
-            acos = new Vector<float>(-0.0187293f) * x + new Vector<float>(0.0742610f);
-            acos = (acos * x - new Vector<float>(0.2121144f)) * x + new Vector<float>(1.5707288f);
-            acos *= Vector.SquareRoot(Vector.Max(Vector<float>.Zero, Vector<float>.One - x));
-            acos -= new Vector<float>(2) * negate * acos;
-            acos = negate * new Vector<float>(3.14159265358979f) + acos;
-        }
-
-
-        [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public static void Floor(in Vector<float> x, out Vector<float> result)
+        /// <returns>Result of the arccos function.</returns>
+        public static Vector<float> Acos(Vector<float> x)
         {
-            //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<float>.Zero), intX - Vector<int>.One, intX));
+            var negativeInput = Vector.LessThan(x, Vector<float>.Zero);
+            x = Vector.Min(Vector<float>.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<float>.One - x) * (new Vector<float>(62.95741097600742f) + x * (new Vector<float>(69.6550664543659f) + x * (new Vector<float>(17.54512349463405f) + x * 0.6022076120669532f)));
+            var denominator = new Vector<float>(40.07993264439811f) + x * (new Vector<float>(49.81949855726789f) + x * (new Vector<float>(15.703851745284796f) + x));
+            var result = numerator / denominator;
+            return Vector.ConditionalSelect(negativeInput, new Vector<float>(Pi) - result, result);
         }
 
         /// <summary>
@@ -344,11 +370,45 @@ public static void Floor(in Vector<float> x, out Vector<float> result)
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
         public static void GetSignedAngleDifference(in Vector<float> a, in Vector<float> b, out Vector<float> difference)
         {
-            var pi = new Vector<float>(Pi);
             var half = new Vector<float>(0.5f);
             var x = (b - a) * new Vector<float>(1f / TwoPi) + half;
-            Floor(x, out var flooredX);
-            difference = (x - flooredX - half) * TwoPi;
+            difference = (x - Vector.Floor(x) - half) * new Vector<float>(TwoPi);
+        }
+
+
+        [MethodImpl(MethodImplOptions.AggressiveInlining)]
+        public static Vector<float> FastReciprocal(Vector<float> v)
+        {
+            if (Avx.IsSupported && Vector<float>.Count == 8)
+            {
+                return Avx.Reciprocal(v.AsVector256()).AsVector();
+            }
+            else if (Sse.IsSupported && Vector<float>.Count == 4)
+            {
+                return Sse.Reciprocal(v.AsVector128()).AsVector();
+            }
+            else
+            {
+                return Vector<float>.One / v;
+            }
+            //TODO: Arm!
+        }
+        [MethodImpl(MethodImplOptions.AggressiveInlining)]
+        public static Vector<float> FastReciprocalSquareRoot(Vector<float> v)
+        {
+            if (Avx.IsSupported && Vector<float>.Count == 8)
+            {
+                return Avx.ReciprocalSqrt(v.AsVector256()).AsVector();
+            }
+            else if (Sse.IsSupported && Vector<float>.Count == 4)
+            {
+                return Sse.ReciprocalSqrt(v.AsVector128()).AsVector();
+            }
+            else
+            {
+                return Vector<float>.One / Vector.SquareRoot(v);
+            }
+            //TODO: Arm!
         }
     }
 }
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)
         /// <param name="m">Matrix to apply to the vector.</param>
         /// <param name="result">Transformed vector.</param>
         [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)
         /// <param name="target">Target of the camera.</param>
         /// <param name="upVector">Up vector of the camera.</param>
         /// <param name="viewMatrix">Look at matrix.</param>
-        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
         /// <param name="target">Target of the camera.</param>
         /// <param name="upVector">Up vector of the camera.</param>
         /// <returns>Look at matrix.</returns>
-        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
         /// <param name="forward">Forward direction of the camera.</param>
         /// <param name="upVector">Up vector of the camera.</param>
         /// <param name="viewMatrix">Look at matrix.</param>
-        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
         /// <param name="forward">Forward direction of the camera.</param>
         /// <param name="upVector">Up vector of the camera.</param>
         /// <returns>Look at matrix.</returns>
-        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
         /// <param name="position">Position of the transform.</param>
         /// <param name="world">4x4 matrix representing the combined transform.</param>
         [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
         /// <param name="position">Position of the transform.</param>
         /// <param name="world">4x4 matrix representing the combined transform.</param>
         [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/Matrix2x2Wide.cs b/BepuUtilities/Matrix2x2Wide.cs
index 5ef6e96e0..6b122b689 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
@@ -94,8 +93,8 @@ public static void Subtract(in Matrix2x2Wide a, in Matrix2x2Wide b, out Matrix2x
         /// <summary>
         /// Inverts the given matix.
         /// </summary>
-        /// <param name="matrix">Matrix to be inverted.</param>
-        /// <param name="result">Inverted matrix.</param>
+        /// <param name="m">Matrix to be inverted.</param>
+        /// <param name="inverse">Inverted matrix.</param>
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
         public static void InvertWithoutOverlap(in Matrix2x2Wide m, out Matrix2x2Wide inverse)
         {
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/Matrix3x3.cs b/BepuUtilities/Matrix3x3.cs
index ed6f37c84..c2d2e7754 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;
@@ -197,7 +197,7 @@ public unsafe static void Invert(Matrix3x3* m, Matrix3x3* inverse)
         /// <param name="m">Matrix to use as the transformation.</param>
         /// <param name="result">Product of the transformation.</param>
         [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)
         /// <param name="m">Matrix to use as the transformation transpose.</param>
         /// <param name="result">Product of the transformation.</param>
         [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)
         /// <param name="scale">Scale to represent.</param>
         /// <param name="linearTransform">Matrix representing a scale.</param>
         [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)
         /// <param name="angle">Angle of the rotation.</param>
         /// <param name="result">Resulting rotation matrix.</param>
         [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
         /// <param name="angle">Angle of the rotation.</param>
         /// <returns>Resulting rotation matrix.</returns>
         [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)
         /// <param name="v">Vector to build the skew symmetric matrix from.</param>
         /// <param name="result">Skew symmetric matrix representing the cross product.</param>
         [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/Matrix3x3Wide.cs b/BepuUtilities/Matrix3x3Wide.cs
index a4afd9fca..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
@@ -113,6 +112,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)
         {
@@ -169,6 +179,22 @@ public static void CreateCrossProduct(in Vector3Wide v, out Matrix3x3Wide skew)
             skew.Z.Z = Vector<float>.Zero;
         }
 
+        [MethodImpl(MethodImplOptions.AggressiveInlining)]
+        public static Matrix3x3Wide CreateCrossProduct(in Vector3Wide v) //TODO: this has some weird codegen on .NET 6 preview 5.
+        {
+            Matrix3x3Wide skew;
+            skew.X.X = Vector<float>.Zero;
+            skew.X.Y = -v.Z;
+            skew.X.Z = v.Y;
+            skew.Y.X = v.Z;
+            skew.Y.Y = Vector<float>.Zero;
+            skew.Y.Z = -v.X;
+            skew.Z.X = -v.Y;
+            skew.Z.Y = v.X;
+            skew.Z.Z = Vector<float>.Zero;
+            return skew;
+        }
+
         /// <summary>
         /// Negates the components of a matrix.
         /// </summary>
@@ -237,6 +263,35 @@ public static void CreateFromQuaternion(in QuaternionWide quaternion, out Matrix
             result.Z.Z = Vector<float>.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;
+        }
+
         /// <summary>
         /// Pulls one lane out of the wide representation.
         /// </summary>
diff --git a/BepuUtilities/Memory/Buffer.cs b/BepuUtilities/Memory/Buffer.cs
index 801b14227..7734fa070 100644
--- a/BepuUtilities/Memory/Buffer.cs
+++ b/BepuUtilities/Memory/Buffer.cs
@@ -14,19 +14,28 @@ namespace BepuUtilities.Memory
     /// Span over an unmanaged memory region.
     /// </summary>
     /// <typeparam name="T">Type of the memory exposed by the span.</typeparam>
+    [StructLayout(LayoutKind.Sequential)]
     public unsafe struct Buffer<T> where T : unmanaged
     {
+        /// <summary>
+        /// Pointer to the beginning of the memory backing this buffer.
+        /// </summary>
         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.
 
         /// <summary>
-        /// 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.
         /// </summary>
         public int Id;
 
-        [MethodImpl(MethodImplOptions.AggressiveInlining)]
+        /// <summary>
+        /// Creates a new buffer.
+        /// </summary>
+        /// <param name="memory">Memory to back the buffer.</param>
+        /// <param name="length">Length of the buffer in terms of the specified type.</param>
+        /// <param name="id">Id of the buffer.</param>
         public Buffer(void* memory, int length, int id = -1)
         {
             Memory = (T*)memory;
@@ -34,17 +43,36 @@ public Buffer(void* memory, int length, int id = -1)
             Id = id;
         }
 
-        [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public static ref T Get(byte* memory, int index)
+        /// <summary>
+        /// Allocates a new buffer from a pool.
+        /// </summary>
+        /// <param name="length">Length of the buffer in terms of elements of type T</param>
+        /// <param name="pool">Pool to allocate from.</param>
+        public Buffer(int length, IUnmanagedMemoryPool pool)
         {
-            return ref Unsafe.Add(ref Unsafe.As<byte, T>(ref *memory), index);
+            pool.Take(length, out this);
         }
 
+        /// <summary>
+        /// 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.
+        /// </summary>
+        /// <param name="pool">Pool to return the buffer to.</param>
+        public void Dispose(IUnmanagedMemoryPool pool)
+        {
+            pool.Return(ref this);
+        }
+
+        /// <summary>
+        /// Gets a typed reference from a byte buffer by an index in terms of the type.
+        /// </summary>
+        /// <param name="buffer">Byte buffer to interpret.</param>
+        /// <param name="index">Index into the buffer in terms of the specified type instead of bytes.</param>
+        /// <returns>Reference to the instance at the specified index.</returns>
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public static ref T Get(ref RawBuffer buffer, int index)
+        public static ref T Get(ref Buffer<byte> buffer, int index)
         {
             Debug.Assert(index >= 0 && index * Unsafe.SizeOf<T>() < buffer.Length, "Index out of range.");
-            return ref Get(buffer.Memory, index);
+            return ref ((T*)buffer.Memory)[index];
         }
 
         /// <summary>
@@ -62,6 +90,21 @@ public ref T this[int index]
             }
         }
 
+        /// <summary>
+        /// Gets a reference to the element at the given index.
+        /// </summary>
+        /// <param name="index">Index of the element to grab a reference of.</param>
+        /// <returns>Reference to the element at the given index.</returns>
+        public ref T this[uint index]
+        {
+            [MethodImpl(MethodImplOptions.AggressiveInlining)]
+            get
+            {
+                Debug.Assert(index >= 0 && index < length, "Index out of range.");
+                return ref Memory[index];
+            }
+        }
+
         /// <summary>
         /// Gets a pointer to the element at the given index.
         /// </summary>
@@ -74,6 +117,18 @@ public ref T this[int index]
             return Memory + index;
         }
 
+        /// <summary>
+        /// Gets a pointer to the element at the given index.
+        /// </summary>
+        /// <param name="index">Index of the element to retrieve a pointer for.</param>
+        /// <returns>Pointer to the element at the given index.</returns>
+        [MethodImpl(MethodImplOptions.AggressiveInlining)]
+        public T* GetPointer(uint index)
+        {
+            Debug.Assert(index >= 0 && index < Length, "Index out of range.");
+            return Memory + index;
+        }
+
         /// <summary>
         /// Creates a view of a subset of the buffer's memory.
         /// </summary>
@@ -272,17 +327,15 @@ public int IndexOf<TPredicate>(ref TPredicate predicate, int start, int count) w
         }
 
         /// <summary>
-        /// Creates an untyped buffer containing the same data as the Buffer<typeparamref name="T"/>.
+        /// Creates a typed region from the raw buffer with the largest capacity that can fit within the allocated bytes.
         /// </summary>
-        /// <returns>Untyped buffer containing the same data as the source buffer.</returns>
+        /// <typeparam name="TCast">Type of the buffer.</typeparam>
+        /// <returns>Typed buffer of maximum extent within the current buffer.</returns>
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public RawBuffer AsRaw()
+        public Buffer<TCast> As<TCast>() where TCast : unmanaged
         {
-            RawBuffer buffer;
-            buffer.Memory = (byte*)Memory;
-            buffer.Length = length * Unsafe.SizeOf<T>();
-            buffer.Id = Id;
-            return buffer;
+            var count = Length * Unsafe.SizeOf<T>() / Unsafe.SizeOf<TCast>();
+            return new Buffer<TCast>(Memory, count, Id);
         }
 
         [Conditional("DEBUG")]
@@ -304,5 +357,4 @@ public static implicit operator ReadOnlySpan<T>(in Buffer<T> buffer)
             return new ReadOnlySpan<T>(buffer.Memory, buffer.Length);
         }
     }
-
 }
\ No newline at end of file
diff --git a/BepuUtilities/Memory/BufferPool.cs b/BepuUtilities/Memory/BufferPool.cs
index 03b6d6f69..47eff016a 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;
@@ -9,95 +8,13 @@
 namespace BepuUtilities.Memory
 {
     /// <summary>
-    /// 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.
     /// </summary>
-    /// <remarks>This currently works by allocating large managed arrays and pinning them under the assumption that they'll end up in the large object heap.</remarks>
-    public class BufferPool : IUnmanagedMemoryPool, IDisposable
+    public class BufferPool : IUnmanagedMemoryPool
     {
-        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. 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.
-                //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<float>.Count * sizeof(float);
-                Array = new byte[blockSize + alignment];
-                Handle = GCHandle.Alloc(Array, GCHandleType.Pinned);
-                Pointer = (byte*)Handle.AddrOfPinnedObject();
-                var mask = alignment - 1;
-                var offset = (uint)Pointer & mask;
-                Pointer += alignment - 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;
-                    }
-                }
-            }
-
-            /// <summary>
-            /// Unpins and drops the reference to the underlying array.
-            /// </summary>
-            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;
             /// <summary>
             /// Pool of slots available to this power level.
             /// </summary>
@@ -118,6 +35,11 @@ struct PowerPool
             public int BlockCount;
 
             internal const int IdPowerShift = 26;
+            /// <summary>
+            /// Byte alignment to enforce for all block allocations within the buffer pool.
+            /// </summary>
+            /// <remarks>Since this only applies at the level of blocks, we can use a pretty beefy value without much concern.</remarks>
+            public const int BlockAlignment = 128;
 
             public PowerPool(int power, int minimumBlockSize, int expectedPooledCount)
             {
@@ -128,7 +50,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
@@ -139,52 +61,69 @@ 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.
+                Blocks[blockIndex] = (byte*)NativeMemory.AlignedAlloc((nuint)BlockSize, BlockAlignment);
+                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;
                 }
 
             }
 
-            public unsafe readonly byte* GetStartPointerForSlot(int slot)
+            public readonly byte* GetStartPointerForSlot(int slot)
             {
                 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)
+            public void Take(out Buffer<byte> buffer)
             {
                 var slot = Slots.Take();
                 var blockIndex = slot >> SuballocationsPerBlockShift;
                 if (blockIndex >= Blocks.Length)
                 {
-                    Array.Resize(ref Blocks, 1 << SpanHelper.GetContainingPowerOf2(blockIndex + 1));
+                    Resize((int)BitOperations.RoundUpToPowerOf2((uint)(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 Buffer<byte>(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;
@@ -198,15 +137,17 @@ public unsafe void Take(out RawBuffer buffer)
                     idsForAllocator = new HashSet<int>();
                     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
             }
 
             [Conditional("DEBUG")]
-            internal unsafe void ValidateBufferIsContained(ref RawBuffer buffer)
+            internal void ValidateBufferIsContained<T>(ref Buffer<T> typedBuffer) where T : unmanaged
             {
+                var buffer = typedBuffer.As<byte>();
                 //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;
@@ -215,16 +156,16 @@ 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.");
             }
 
-            public readonly unsafe void Return(int slotIndex)
+            public readonly void Return(int slotIndex)
             {
 #if DEBUG 
                 Debug.Assert(outstandingIds.Remove(slotIndex),
@@ -260,7 +201,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;
@@ -276,9 +218,7 @@ public void Clear()
         /// </summary>
         /// <param name="minimumBlockAllocationSize">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.</param>
+        /// Buffers will be suballocated from blocks.</param>
         /// <param name="expectedPooledResourceCount">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.</param>
         public BufferPool(int minimumBlockAllocationSize = 131072, int expectedPooledResourceCount = 16)
@@ -300,7 +240,6 @@ public BufferPool(int minimumBlockAllocationSize = 131072, int expectedPooledRes
         public void EnsureCapacityForPower(int byteCount, int power)
         {
             SpanHelper.ValidatePower(power);
-            ValidatePinnedState(true);
             pools[power].EnsureCapacity(byteCount);
         }
 
@@ -316,26 +255,18 @@ public int GetCapacityForPower(int power)
         }
 
         /// <summary>
-        /// Takes a buffer large enough to contain a number of bytes. Capacity may be larger than requested.
-        /// </summary>
-        /// <param name="count">Desired minimum capacity of the buffer in bytes.</param>
-        /// <param name="buffer">Buffer that can hold the bytes.</param>
-        [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public void TakeAtLeast(int count, out RawBuffer buffer)
-        {
-            TakeForPower(SpanHelper.GetContainingPowerOf2(count), out buffer);
-        }
-
-        /// <summary>
-        /// Takes a buffer of the requested size from the pool.
+        /// 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.
         /// </summary>
-        /// <param name="count">Desired capacity of the buffer in bytes.</param>
-        /// <param name="buffer">Buffer of the requested size.</param>
-        [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public void Take(int count, out RawBuffer buffer)
+        /// <returns>Total number of bytes allocated from native memory in this buffer pool.</returns>
+        public ulong GetTotalAllocatedByteCount()
         {
-            TakeAtLeast(count, out buffer);
-            buffer.Length = count;
+            ulong sum = 0;
+            for (int i = 0; i < pools.Length; ++i)
+            {
+                sum += (ulong)pools[i].BlockCount * (ulong)pools[i].BlockSize;
+            }
+            return sum;
         }
 
         /// <summary>
@@ -350,7 +281,7 @@ public void TakeAtLeast<T>(int count, out Buffer<T> buffer) where T : unmanaged
             //Avoid returning a zero length span because 1 byte / Unsafe.SizeOf<T>() happens to be zero.
             if (count == 0)
                 count = 1;
-            TakeAtLeast(count * Unsafe.SizeOf<T>(), out var rawBuffer);
+            TakeForPower(SpanHelper.GetContainingPowerOf2(count * Unsafe.SizeOf<T>()), out var rawBuffer);
             buffer = rawBuffer.As<T>();
         }
 
@@ -370,12 +301,11 @@ public void Take<T>(int count, out Buffer<T> buffer) where T : unmanaged
         /// <summary>
         /// Takes a buffer large enough to contain a number of bytes given by a power, where the number of bytes is 2^power.
         /// </summary>
-        /// <param name="count">Number of bytes that should fit within the buffer as an exponent, where the number of bytes is 2^power.</param>
+        /// <param name="power">Number of bytes that should fit within the buffer as an exponent, where the number of bytes is 2^power.</param>
         /// <param name="buffer">Buffer that can hold the bytes.</param>
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public void TakeForPower(int power, out RawBuffer buffer)
+        public void TakeForPower(int power, out Buffer<byte> buffer)
         {
-            ValidatePinnedState(true);
             Debug.Assert(power >= 0 && power <= SpanHelper.MaximumSpanSizePower);
             pools[power].Take(out buffer);
         }
@@ -387,27 +317,17 @@ internal static void DecomposeId(int bufferId, out int powerIndex, out int slotI
             slotIndex = bufferId & ((1 << PowerPool.IdPowerShift) - 1);
         }
 
-        /// <summary>
-        /// Returns a buffer to the pool by id.
-        /// </summary>
-        /// <param name="buffer">Buffer to return to the pool.</param>
-        /// <remarks>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.</remarks>
+        /// <inheritdoc/>       
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public unsafe void ReturnUnsafely(int id)
+        public void ReturnUnsafely(int id)
         {
-            ValidatePinnedState(true);
             DecomposeId(id, out var powerIndex, out var slotIndex);
             pools[powerIndex].Return(slotIndex);
         }
 
-        /// <summary>
-        /// Returns a buffer to the pool.
-        /// </summary>
-        /// <param name="buffer">Buffer to return to the pool.</param>
+        /// <inheritdoc/>  
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public unsafe void Return(ref RawBuffer buffer)
+        public void Return<T>(ref Buffer<T> buffer) where T : unmanaged
         {
 #if DEBUG
             DecomposeId(buffer.Id, out var powerIndex, out var slotIndex);
@@ -416,132 +336,52 @@ public unsafe void Return(ref RawBuffer buffer)
             ReturnUnsafely(buffer.Id);
             buffer = default;
         }
-        /// <summary>
-        /// Returns a buffer to the pool.
-        /// </summary>
-        /// <param name="buffer">Buffer to return to the pool.</param>
-        [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public unsafe void Return<T>(ref Buffer<T> 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.
-        /// <summary>
-        /// 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.
-        /// </summary>
-        /// <param name="buffer">Buffer reference to resize.</param>
-        /// <param name="targetSize">Number of bytes to resize the buffer for.</param>
-        /// <param name="copyCount">Number of bytes to copy into the new buffer from the old buffer.</param>
-        [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);
-            }
-        }
 
-        /// <summary>
-        /// Resizes a buffer to the target size. Copies a subset of elements into the new buffer.
-        /// </summary>
-        /// <param name="buffer">Buffer reference to resize.</param>
-        /// <param name="targetSize">Number of bytes to resize the buffer for.</param>
-        /// <param name="copyCount">Number of bytes to copy into the new buffer from the old buffer.</param>
-        [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public unsafe void Resize(ref RawBuffer buffer, int targetSize, int copyCount)
-        {
-            ResizeToAtLeast(ref buffer, targetSize, copyCount);
-            buffer.Length = targetSize;
-        }
-
-        /// <summary>
-        /// 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.
-        /// </summary>
-        /// <typeparam name="T">Type of the buffer to resize.</typeparam>
-        /// <param name="buffer">Buffer reference to resize.</param>
-        /// <param name="targetSize">Number of elements to resize the buffer for.</param>
-        /// <param name="copyCount">Number of elements to copy into the new buffer from the old buffer.</param>
-        [MethodImpl(MethodImplOptions.AggressiveInlining)]
+        /// <inheritdoc/>  
         public void ResizeToAtLeast<T>(ref Buffer<T> 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<T>(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)
             {
-                TakeAtLeast(targetSize, out Buffer<T> newBuffer);
-                if (buffer.Length > 0)
+                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
+            {
+                var originalAllocatedSizeInBytes = 1 << (buffer.Id >> PowerPool.IdPowerShift);
+                var originalAllocatedSize = originalAllocatedSizeInBytes / Unsafe.SizeOf<T>();
+                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<T> 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;
             }
         }
 
-
-        /// <summary>
-        /// Resizes a buffer to the target size. Copies a subset of elements into the new buffer.
-        /// </summary>
-        /// <typeparam name="T">Type of the buffer to resize.</typeparam>
-        /// <param name="buffer">Buffer reference to resize.</param>
-        /// <param name="targetSize">Number of elements to resize the buffer for.</param>
-        /// <param name="copyCount">Number of elements to copy into the new buffer from the old buffer.</param>
-        [MethodImpl(MethodImplOptions.AggressiveInlining)]
+        /// <inheritdoc/>  
         public void Resize<T>(ref Buffer<T> buffer, int targetSize, int copyCount) where T : unmanaged
         {
             ResizeToAtLeast(ref buffer, targetSize, copyCount);
             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}.");
-                }
-            }
-        }
-
+        /// <summary>
+        /// Issues debug assertions that all pools are empty.
+        /// </summary>
         [Conditional("DEBUG")]
         public void AssertEmpty()
         {
@@ -565,59 +405,8 @@ public void AssertEmpty()
         }
 
         /// <summary>
-        /// 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.
-        /// </summary>
-        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);
-                    }
-                }
-            }
-        }
-
-        /// <summary>
-        /// 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.
         /// </summary>
         public void Clear()
         {
@@ -627,6 +416,10 @@ public void Clear()
             }
         }
 
+        /// <summary>
+        /// Returns all allocations in the pool to sources. Any outstanding buffers will be invalidated silently.
+        /// Equivalent to <see cref="Clear"/> for <see cref="BufferPool"/>.
+        /// </summary>
         void IDisposable.Dispose()
         {
             Clear();
@@ -637,7 +430,8 @@ public static int GetCapacityForCount<T>(int count)
         {
             if (count == 0)
                 count = 1;
-            return (1 << SpanHelper.GetContainingPowerOf2(count * Unsafe.SizeOf<T>())) / Unsafe.SizeOf<T>();
+            Debug.Assert(BitOperations.RoundUpToPowerOf2((ulong)(count * Unsafe.SizeOf<T>())) < 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<T>()))) / Unsafe.SizeOf<T>();
         }
 
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
@@ -656,7 +450,7 @@ int IUnmanagedMemoryPool.GetCapacityForCount<T>(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..db28eb90a 100644
--- a/BepuUtilities/Memory/IUnmanagedMemoryPool.cs
+++ b/BepuUtilities/Memory/IUnmanagedMemoryPool.cs
@@ -1,9 +1,11 @@
-namespace BepuUtilities.Memory
+using System;
+
+namespace BepuUtilities.Memory
 {
     /// <summary>
-    /// 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.
     /// </summary>
-    public interface IUnmanagedMemoryPool
+    public interface IUnmanagedMemoryPool : IDisposable
     {
         /// <summary>
         /// Takes a buffer large enough to contain a number of elements of a given type. Capacity may be larger than requested.
@@ -23,7 +25,7 @@ public interface IUnmanagedMemoryPool
         /// Returns a buffer to the pool.
         /// </summary>
         /// <typeparam name="T">Type of the buffer's elements.</typeparam>
-        /// <param name="buffer">Buffer to return to the pool.</param>
+        /// <param name="buffer">Buffer to return to the pool. The reference will be cleared.</param>
         void Return<T>(ref Buffer<T> buffer) where T : unmanaged;
         /// <summary>
         /// 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,45 @@ public interface IUnmanagedMemoryPool
         /// <param name="count">Number of elements to request.</param>
         /// <returns>Capacity of a buffer that would be returned if the given element count was requested.</returns>
         int GetCapacityForCount<T>(int count) where T : unmanaged;
+
+        /// <summary>
+        /// Returns a buffer to the pool by id.
+        /// </summary>
+        /// <param name="id">Id of the buffer to return to the pool.</param>
+        /// <remarks>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.</remarks>
+        void ReturnUnsafely(int id);
+
+        /// <summary>
+        /// 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.
+        /// </summary>
+        /// <typeparam name="T">Type of the buffer to resize.</typeparam>
+        /// <param name="buffer">Buffer reference to resize.</param>
+        /// <param name="targetSize">Number of elements to resize the buffer for.</param>
+        /// <param name="copyCount">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.</param>
+        void ResizeToAtLeast<T>(ref Buffer<T> buffer, int targetSize, int copyCount) where T : unmanaged;
+
+        /// <summary>
+        /// Resizes a buffer to the target size. Copies a subset of elements into the new buffer.
+        /// </summary>
+        /// <typeparam name="T">Type of the buffer to resize.</typeparam>
+        /// <param name="buffer">Buffer reference to resize.</param>
+        /// <param name="targetSize">Number of elements to resize the buffer for.</param>
+        /// <param name="copyCount">Number of elements to copy into the new buffer from the old buffer.</param>
+        void Resize<T>(ref Buffer<T> buffer, int targetSize, int copyCount) where T : unmanaged;
+
+        /// <summary>
+        /// 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.
+        /// </summary>
+        void Clear();
+
+        /// <summary>
+        /// 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.
+        /// </summary>
+        /// <returns>Total number of bytes allocated from the backing memory source in this pool.</returns>
+        ulong GetTotalAllocatedByteCount();
     }
 }
\ No newline at end of file
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/Memory/RawBuffer.cs b/BepuUtilities/Memory/RawBuffer.cs
deleted file mode 100644
index f7abefa7d..000000000
--- a/BepuUtilities/Memory/RawBuffer.cs
+++ /dev/null
@@ -1,163 +0,0 @@
-using System;
-using System.Diagnostics;
-using System.Runtime.CompilerServices;
-
-namespace BepuUtilities.Memory
-{
-    /// <summary>
-    /// Raw byte buffer with some helpers for interoperating with typed spans.
-    /// </summary>
-    public unsafe struct RawBuffer : IEquatable<RawBuffer>
-    {
-        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.
-
-        /// <summary>
-        /// 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.
-        /// </summary>
-        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);
-            }
-        }
-
-        /// <summary>
-        /// Interprets the bytes at the memory location as a given type.
-        /// </summary>
-        /// <typeparam name="T">Type to interpret the memory as.</typeparam>
-        /// <param name="byteIndex">Memory location to interpret.</param>
-        /// <returns>Reference to the memory as a given type.</returns>
-        [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public ref T Interpret<T>(int byteIndex)
-        {
-            ValidateRegion(byteIndex, Unsafe.SizeOf<T>());
-            return ref Unsafe.As<byte, T>(ref *(Memory + byteIndex));
-        }
-
-        [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public RawBuffer Slice(int start, int count)
-        {
-            ValidateRegion(start, count);
-            return new RawBuffer(Memory + start, count, Id);
-        }
-
-        /// <summary>
-        /// Takes a region of the raw buffer as a typed buffer.
-        /// </summary>
-        /// <typeparam name="T">Type to interpret the region as.</typeparam>
-        /// <param name="start">Start of the region in terms of the type's size.</param>
-        /// <param name="count">Number of elements in the region in terms of the type.</param>
-        /// <returns>A typed buffer.</returns>
-        [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public Buffer<T> Slice<T>(int start, int count) where T : unmanaged
-        {
-            ValidateRegion<T>(start, count);
-            return new Buffer<T>(Memory + start * Unsafe.SizeOf<T>(), count * Unsafe.SizeOf<T>(), Id);
-        }
-
-        /// <summary>
-        /// Creates a typed region from the raw buffer with the largest capacity that can fit within the allocated bytes.
-        /// </summary>
-        /// <typeparam name="T">Type of the buffer.</typeparam>
-        /// <returns>Typed buffer of maximum extent within the current raw buffer.</returns>
-        [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public Buffer<T> As<T>() where T : unmanaged
-        {
-            var count = Length / Unsafe.SizeOf<T>();
-            return new Buffer<T>(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<T>(int startInElements, int countInElements)
-        {
-            Debug.Assert(startInElements * Unsafe.SizeOf<T>() >= 0, "The start of a region must be within the buffer's extent.");
-            Debug.Assert((startInElements + countInElements) * Unsafe.SizeOf<T>() <= 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<byte>(in RawBuffer buffer)
-        {
-            return new Span<byte>(buffer.Memory, buffer.Length);
-        }
-
-        [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public static implicit operator ReadOnlySpan<byte>(in RawBuffer buffer)
-        {
-            return new ReadOnlySpan<byte>(buffer.Memory, buffer.Length);
-        }
-
-        [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public static implicit operator Buffer<byte>(in RawBuffer buffer)
-        {
-            return new Buffer<byte>(buffer.Memory, buffer.Length);
-        }
-
-    }
-}
\ No newline at end of file
diff --git a/BepuUtilities/Memory/SpanHelper.cs b/BepuUtilities/Memory/SpanHelper.cs
index eae243181..80d32e4ae 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.");
         }
-        /// <summary>
-        /// Computes the largest integer N such that 2^N is less than or equal to i.
-        /// </summary>
-        /// <param name="i">Integer to compute the power of.</param>
-        /// <returns>Lowest integer N such that 2^N is less than or equal to i.</returns>
-        [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;
-        }
+
         /// <summary>
         /// Computes the lowest integer N such that 2^N >= i.
         /// </summary>
@@ -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);
+
         }
 
         /// <summary>
@@ -96,7 +60,7 @@ public static bool IsPrimitive<T>()
         /// <summary>
         /// Tests if a type is primitive. Slow path; unspecialized compilation.
         /// </summary>
-        /// <param name="type">Type to check for primitiveness.</typeparam>
+        /// <param name="type">Type to check for primitiveness.</param>
         /// <returns>True if the type is one of the primitive types, false otherwise.</returns>
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
         public static bool IsPrimitive(Type type)
diff --git a/BepuUtilities/Memory/WorkerBufferPools.cs b/BepuUtilities/Memory/WorkerBufferPools.cs
new file mode 100644
index 000000000..47e26622e
--- /dev/null
+++ b/BepuUtilities/Memory/WorkerBufferPools.cs
@@ -0,0 +1,76 @@
+using System;
+
+namespace BepuUtilities.Memory;
+
+/// <summary>
+/// Collection of pools used by worker threads.
+/// </summary> 
+public class WorkerBufferPools : IDisposable
+{
+    BufferPool[] pools;
+
+    /// <summary>
+    /// Gets the pool associated with this worker.
+    /// </summary>
+    /// <param name="workerIndex">Worker index of the pool to look up.</param>
+    /// <returns>Pool associated with the given worker.</returns>
+    public BufferPool this[int workerIndex] => pools[workerIndex];
+
+    /// <summary>
+    /// Gets or sets the default block capacity for any newly created arena subpools.
+    /// </summary>
+    public int DefaultBlockCapacity { get; set; }
+
+
+    /// <summary>
+    /// Creates a new set of worker pools.
+    /// </summary>
+   /// <param name="initialWorkerCount">Initial number of workers to allocate space for.</param>
+    /// <param name="defaultBlockCapacity">Default block capacity in thread pools.</param>
+    public WorkerBufferPools(int initialWorkerCount, int defaultBlockCapacity = 16384)
+    {
+        pools = new BufferPool[initialWorkerCount];
+        DefaultBlockCapacity = defaultBlockCapacity;
+        for (int i = 0; i < pools.Length; ++i)
+        {
+            pools[i] = new BufferPool(defaultBlockCapacity);
+        }
+    }
+
+    /// <summary>
+    /// Clears all allocations from worker pools. Pools can still be used after being cleared.
+    /// </summary>
+    /// <remarks>This does not take any locks and should not be called if any other threads may be using any of the involved pools.</remarks>
+    public void Clear()
+    {
+        for (int i = 0; i < pools.Length; ++i)
+        {
+            pools[i].Clear();
+        }
+    }
+
+    /// <summary>
+    /// Disposes all worker pools. Pools cannot be used after being disposed.
+    /// </summary>
+    public void Dispose()
+    {
+        for (int i = 0; i < pools.Length; ++i)
+        {
+            pools[i].Clear();
+        }
+    }
+
+    /// <summary>
+    /// 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.
+    /// </summary>
+    /// <returns>Total number of bytes allocated from native memory by all worker pools. Includes memory that is not currently in use by external allocators.</returns>
+    public ulong GetTotalAllocatedByteCount()
+    {
+        ulong sum = 0;
+        for (int i = 0; i < pools.Length; ++i)
+        {
+            sum += pools[i].GetTotalAllocatedByteCount();
+        }
+        return sum;
+    }
+}
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
         /// <param name="b">Second quaternion to add.</param>
         /// <param name="result">Sum of the addition.</param>
         [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)
         /// <param name="scale">Amount to multiply each component of the quaternion by.</param>
         /// <param name="result">Scaled quaternion.</param>
         [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)
         /// <param name="b">Second quaternion to concatenate.</param>
         /// <param name="result">Product of the concatenation.</param>
         [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
         /// <param name="b">Second quaternion to concatenate.</param>
         /// <param name="result">Product of the concatenation.</param>
         [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
         /// <param name="b">Second quaternion to multiply.</param>
         /// <returns>Product of the multiplication.</returns>
         [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)
         /// <param name="interpolationAmount">Amount of the end point to use.</param>
         /// <param name="result">Interpolated intermediate quaternion.</param>
         [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
         /// <param name="interpolationAmount">Amount of the end point to use.</param>
         /// <returns>Interpolated intermediate quaternion.</returns>
         [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
         /// <param name="quaternion">Quaternion to conjugate.</param>
         /// <param name="result">Conjugated quaternion.</param>
         [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)
         /// <param name="quaternion">Quaternion to conjugate.</param>
         /// <returns>Conjugated quaternion.</returns>
         [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)
         /// <param name="quaternion">Quaternion to invert.</param>
         /// <param name="result">Result of the inversion.</param>
         [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)
         /// <param name="quaternion">Quaternion to invert.</param>
         /// <returns>Result of the inversion.</returns>
         [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)
         /// <param name="a">Quaternion to negate.</param>
         /// <param name="b">Negated result.</param>
         [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)
         /// <param name="q">Quaternion to negate.</param>
         /// <returns>Negated result.</returns>
         [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)
         /// <param name="rotation">Rotation to apply to the vector.</param>
         /// <param name="result">Transformed vector.</param>
         [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,
         /// <param name="rotation">Rotation to apply to the vector.</param>
         /// <param name="result">Transformed vector.</param>
         [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
         /// <param name="rotation">Rotation to apply to the vector.</param>
         /// <returns>Transformed vector.</returns>
         [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)
         /// <param name="rotation">Rotation to apply to the vector.</param>
         /// <param name="result">Transformed vector.</param>
         [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)
         /// <param name="rotation">Rotation to apply to the vector.</param>
         /// <param name="result">Transformed vector.</param>
         [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)
         /// <param name="rotation">Rotation to apply to the vector.</param>
         /// <param name="result">Transformed vector.</param>
         [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)
         /// <param name="angle">Angle to rotate around the axis.</param>
         /// <returns>Quaternion representing the axis and angle rotation.</returns>
         [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)
         /// <param name="angle">Angle to rotate around the axis.</param>
         /// <param name="q">Quaternion representing the axis and angle rotation.</param>
         [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
         /// <param name="q">Quaternion to be converted.</param>
         /// <returns>Angle around the axis represented by the quaternion.</returns>
         [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)
         /// <param name="axis">Axis represented by the quaternion.</param>
         /// <param name="angle">Angle around the axis represented by the quaternion.</param>
         [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,
         /// <param name="v2">Second unit-length vector.</param>
         /// <param name="q">Quaternion representing the rotation from v1 to v2.</param>
         [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
         /// <param name="end">Ending orientation.</param>
         /// <param name="relative">Relative rotation from the start to the end orientation.</param>
         [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
         /// <param name="targetBasis">Basis in the original frame of reference to transform the rotation into.</param>
         /// <param name="localRotation">Rotation in the local space of the target basis.</param>
         [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 58e901ae5..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
@@ -12,7 +11,7 @@ public struct QuaternionWide
         public Vector<float> 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<float>(source.X);
             broadcasted.Y = new Vector<float>(source.Y);
@@ -28,7 +27,7 @@ public static void Broadcast(in Quaternion source, out QuaternionWide broadcaste
         /// <param name="broadcasted">Target quaternion to be filled with the selected data.</param>
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
         public static void Rebroadcast(in QuaternionWide source, int slotIndex, out QuaternionWide broadcasted)
-        { 
+        {
             broadcasted.X = new Vector<float>(source.X[slotIndex]);
             broadcasted.Y = new Vector<float>(source.Y[slotIndex]);
             broadcasted.Z = new Vector<float>(source.Z[slotIndex]);
@@ -110,23 +109,28 @@ public static void Scale(in QuaternionWide q, in Vector<float> scale, out Quater
         }
 
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public static void GetLengthSquared(in QuaternionWide q, out Vector<float> lengthSquared)
+        public Vector<float> 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<float> length)
+        public Vector<float> 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 void Normalize(in QuaternionWide q, out QuaternionWide normalized)
+        public static QuaternionWide Normalize(QuaternionWide q)
         {
+            //TODO: fast path is possible with intrinsics.
             var inverseNorm = Vector<float>.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 +142,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;
+        }
+
         /// <summary>
         /// Computes the quaternion rotation between two normalized vectors.
         /// </summary>
@@ -167,17 +182,49 @@ 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<float>.Zero)));
             q.W = Vector.ConditionalSelect(useNormalCase, dot + Vector<float>.One, Vector<float>.Zero);
 
-            Normalize(q, out q);
+            q = Normalize(q);
+        }
+
+        /// <summary>
+        /// Computes the quaternion rotation between two normalized vectors.
+        /// </summary>
+        /// <param name="v1">First unit-length vector.</param>
+        /// <param name="v2">Second unit-length vector.</param>
+        /// <returns>Quaternion representing the rotation from v1 to v2.</returns>
+        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<float>(-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<float>.Zero, Vector.ConditionalSelect(yIsSmaller, -v1.Z, -v1.Y)));
+            q.Y = Vector.ConditionalSelect(useNormalCase, cross.Y, Vector.ConditionalSelect(xIsSmallest, -v1.Z, Vector.ConditionalSelect(yIsSmaller, Vector<float>.Zero, v1.X)));
+            q.Z = Vector.ConditionalSelect(useNormalCase, cross.Z, Vector.ConditionalSelect(xIsSmallest, v1.Y, Vector.ConditionalSelect(yIsSmaller, v1.X, Vector<float>.Zero)));
+            q.W = Vector.ConditionalSelect(useNormalCase, dot + Vector<float>.One, Vector<float>.Zero);
+            return Normalize(q);
         }
 
         /// <summary>
-        /// 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.
         /// </summary>
         /// <param name="q">Quaternion to extract an axis-angle representation from.</param>
         /// <param name="axis">Axis of rotation extracted from the quaternion.</param>
-        /// <param name="angle">Approximated angle of rotation extracted from the quaternion.</param>
+        /// <param name="angle">Angle of rotation extracted from the quaternion.</param>
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public static void GetApproximateAxisAngleFromQuaternion(in QuaternionWide q, out Vector3Wide axis, out Vector<float> angle)
+        public static void GetAxisAngleFromQuaternion(in QuaternionWide q, out Vector3Wide axis, out Vector<float> angle)
         {
             var shouldNegate = Vector.LessThan(q.W, Vector<float>.Zero);
             axis.X = Vector.ConditionalSelect(shouldNegate, -q.X, q.X);
@@ -191,8 +238,8 @@ public static void GetApproximateAxisAngleFromQuaternion(in QuaternionWide q, ou
             axis.X = Vector.ConditionalSelect(useFallback, Vector<float>.One, axis.X);
             axis.Y = Vector.ConditionalSelect(useFallback, Vector<float>.Zero, axis.Y);
             axis.Z = Vector.ConditionalSelect(useFallback, Vector<float>.Zero, axis.Z);
-            MathHelper.ApproximateAcos(qw, out var halfAngle);
-            angle = 2 * halfAngle;
+            var halfAngle = MathHelper.Acos(qw);
+            angle = new Vector<float>(2) * halfAngle;
         }
 
         /// <summary>
@@ -239,13 +286,84 @@ public static void Transform(in Vector3Wide v, in QuaternionWide rotation, out V
             result = temp;
         }
 
+        /// <summary>
+        /// Transforms the vector using a quaternion.
+        /// </summary>
+        /// <param name="v">Vector to transform.</param>
+        /// <param name="rotation">Rotation to apply to the vector.</param>
+        /// <returns>Transformed vector.</returns>
+        [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<float>.One - yy2 - zz2) + v.Y * (xy2 - wz2) + v.Z * (xz2 + wy2);
+            result.Y = v.X * (xy2 + wz2) + v.Y * (Vector<float>.One - xx2 - zz2) + v.Z * (yz2 - wx2);
+            result.Z = v.X * (xz2 - wy2) + v.Y * (yz2 + wx2) + v.Z * (Vector<float>.One - xx2 - yy2);
+            return result;
+        }
+
+        /// <summary>
+        /// Transforms the vector using a quaternion.
+        /// </summary>
+        /// <param name="v">Vector to transform.</param>
+        /// <param name="rotation">Rotation to apply to the vector.</param>
+        /// <returns>Transformed vector.</returns>
+        [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<float>.One - yy2 - zz2) + v.Y * (xy2 - wz2) + v.Z * (xz2 + wy2);
+            result.Y = v.X * (xy2 + wz2) + v.Y * (Vector<float>.One - xx2 - zz2) + v.Z * (yz2 - wx2);
+            result.Z = v.X * (xz2 - wy2) + v.Y * (yz2 + wx2) + v.Z * (Vector<float>.One - xx2 - yy2);
+            return result;
+        }
+
+        [MethodImpl(MethodImplOptions.AggressiveInlining)]
+        public static Vector3Wide operator *(Vector3Wide v, QuaternionWide rotation)
+        {
+            return Transform(v, rotation);
+        }
+
         /// <summary>
         /// Transforms the unit X direction using a quaternion.
         /// </summary>
         /// <param name="rotation">Rotation to apply to the vector.</param>
-        /// <param name="result">Transformed vector.</param>
+        /// <returns>Transformed vector.</returns>
         [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 +373,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<float>.One - yy2 - zz2;
             result.Y = xy2 + wz2;
             result.Z = xz2 - wy2;
+            return result;
         }
 
         /// <summary>
         /// Transforms the unit Y vector using a quaternion.
         /// </summary>
         /// <param name="rotation">Rotation to apply to the vector.</param>
-        /// <param name="result">Transformed vector.</param>
+        /// <returns>Transformed vector.</returns>
         [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 +397,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<float>.One - xx2 - zz2;
             result.Z = yz2 + wx2;
+            return result;
         }
 
         /// <summary>
         /// Transforms the unit Z vector using a quaternion.
         /// </summary>
         /// <param name="rotation">Rotation to apply to the vector.</param>
-        /// <param name="result">Transformed vector.</param>
+        /// <returns>Transformed vector.</returns>
         [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 +421,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<float>.One - xx2 - yy2;
+            return result;
         }
 
         /// <summary>
@@ -395,6 +519,24 @@ public static void Concatenate(in QuaternionWide a, in QuaternionWide b, out Qua
             result = tempResult;
         }
 
+        /// <summary>
+        /// 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.
+        /// </summary>
+        /// <param name="a">First quaternion to concatenate.</param>
+        /// <param name="b">Second quaternion to concatenate.</param>
+        /// <returns>Product of the concatenation.</returns>
+        [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;
+        }
+
         /// <summary>
         /// Computes the conjugate of the quaternion.
         /// </summary>
@@ -409,6 +551,42 @@ public static void Conjugate(in QuaternionWide quaternion, out QuaternionWide re
             result.W = -quaternion.W;
         }
 
+        /// <summary>
+        /// Computes the conjugate of the quaternion.
+        /// </summary>
+        /// <param name="quaternion">Quaternion to conjugate.</param>
+        /// <returns>Conjugated quaternion.</returns>
+        [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;
+        }
+
+        [MethodImpl(MethodImplOptions.AggressiveInlining)]
+        public static void ConditionalSelect(in Vector<int> 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<int> 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;
+        }
+
         /// <summary>
         /// Gathers values from the first slot of a wide quaternion and puts them into a narrow representation.
         /// </summary>
@@ -430,12 +608,37 @@ public static void ReadFirst(in QuaternionWide source, out Quaternion target)
         /// <param name="source">Quaternion to copy values from.</param>
         /// <param name="targetSlot">Wide quaternion to place values into.</param>
         [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;
             GatherScatter.GetFirst(ref targetSlot.Z) = source.Z;
             GatherScatter.GetFirst(ref targetSlot.W) = source.W;
+        }       
+
+        /// <summary>
+        /// Writes a value into a slot of the target bundle.
+        /// </summary>
+        /// <param name="source">Source of the value to write.</param>
+        /// <param name="slotIndex">Index of the slot to write into.</param>
+        /// <param name="target">Bundle to write the value into.</param>
+        [MethodImpl(MethodImplOptions.AggressiveInlining)]
+        public static void WriteSlot(Quaternion source, int slotIndex, ref QuaternionWide target)
+        {
+            WriteFirst(source, ref GatherScatter.GetOffsetInstance(ref target, slotIndex));
+        }
+
+        /// <summary>
+        /// Pulls one lane out of the wide representation.
+        /// </summary>
+        /// <param name="wide">Source of the lane.</param>
+        /// <param name="slotIndex">Index of the lane within the wide representation to read.</param>
+        /// <param name="narrow">Non-SIMD type to store the lane in.</param>
+        [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/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 822866d1e..11a25396d 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
 {
@@ -37,12 +33,16 @@ public struct Symmetric3x3
         /// </summary>
         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.
+
         /// <summary>
-        /// 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.
         /// </summary>
         /// <param name="r">Rotation matrix to use as the sandwich bread.</param>
         /// <param name="m">Succulent interior symmetric matrix.</param>
-        /// <param name="sandwich">Result of v * m * transpose(v) for a symmetric matrix m.</param>
+        /// <param name="sandwich">Result of transpose(r) * m * r.</param>
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
         public static void RotationSandwich(in Matrix3x3 r, in Symmetric3x3 m, out Symmetric3x3 sandwich)
         {
@@ -83,13 +83,25 @@ public static float Determinant(in Symmetric3x3 m)
             return m11 * m.XX + m21 * m.YX + m31 * m.ZX;
         }
 
+        /// <summary>
+        /// Inverts the given matix.
+        /// </summary>
+        /// <param name="m">Matrix to be inverted.</param>
+        /// <returns>Inverted matrix.</returns>
+        [MethodImpl(MethodImplOptions.AggressiveInlining)]
+        public static Symmetric3x3 Invert(Symmetric3x3 m)
+        {
+            Invert(m, out var inverse);
+            return inverse;
+        }
+
         /// <summary>
         /// Inverts the given matix.
         /// </summary>
         /// <param name="m">Matrix to be inverted.</param>
         /// <param name="inverse">Inverted matrix.</param>
         [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;
@@ -127,11 +139,30 @@ public static void Add(in Symmetric3x3 a, in Symmetric3x3 b, out Symmetric3x3 re
         }
 
         /// <summary>
-        /// Subtracts the components of b from a.
+        /// Adds the components of two matrices together.
         /// </summary>
-        /// <param name="a">Matrix to be subtracted from.</param>
-        /// <param name="b">Matrix to subtract from the first matrix..</param>
-        /// <param name="result">Matrix with subtracted components.</param>
+        /// <param name="a">First matrix to add.</param>
+        /// <param name="b">Second matrix to add.</param>
+        /// <returns>Matrix with components equal to the components of the two input matrices added together.</returns>
+        [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;
+        }
+
+        /// <summary>
+        /// Subtracts the components of matrix b from matrix a.
+        /// </summary>
+        /// <param name="a">Matrix to be subtracted from</param>
+        /// <param name="b">Matrix to subtract from matrix a.</param>
+        /// <param name="result">Matrix with components equal to the components of the matrix a minus the components of matrix b.</param>
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
         public static void Subtract(in Symmetric3x3 a, in Symmetric3x3 b, out Symmetric3x3 result)
         {
@@ -143,6 +174,25 @@ public static void Subtract(in Symmetric3x3 a, in Symmetric3x3 b, out Symmetric3
             result.ZZ = a.ZZ - b.ZZ;
         }
 
+        /// <summary>
+        /// Subtracts the components of matrix b from matrix a.
+        /// </summary>
+        /// <param name="a">Matrix to be subtracted from</param>
+        /// <param name="b">Matrix to subtract from matrix a.</param>
+        /// <returns>Matrix with components equal to the components of the matrix a minus the components of matrix b.</returns>
+        [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;
+        }
+
         /// <summary>
         /// Adds the components of two matrices together.
         /// </summary>
@@ -160,6 +210,44 @@ public static void Add(in Matrix3x3 a, in Symmetric3x3 b, out Matrix3x3 result)
             result.Z = a.Z + bZ;
         }
 
+        /// <summary>
+        /// Adds the components of two matrices together.
+        /// </summary>
+        /// <param name="a">First matrix to add.</param>
+        /// <param name="b">Second matrix to add.</param>
+        /// <returns>Matrix with components equal to the components of the two input matrices added together.</returns>
+        [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;
+        }
+
+        /// <summary>
+        /// Adds the components of two matrices together.
+        /// </summary>
+        /// <param name="a">First matrix to add.</param>
+        /// <param name="b">Second matrix to add.</param>
+        /// <returns>Matrix with components equal to the components of the two input matrices added together.</returns>
+        [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;
+        }
+
         /// <summary>
         /// Subtracts the components of one matrix from another.
         /// </summary>
@@ -177,6 +265,44 @@ public static void Subtract(in Matrix3x3 a, in Symmetric3x3 b, out Matrix3x3 res
             result.Z = a.Z - bZ;
         }
 
+        /// <summary>
+        /// Subtracts the components of matrix b from matrix a.
+        /// </summary>
+        /// <param name="a">Matrix to be subtracted from</param>
+        /// <param name="b">Matrix to subtract from matrix a.</param>
+        /// <returns>Matrix with components equal to the components of the matrix a minus the components of matrix b.</returns>
+        [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;
+        }
+
+        /// <summary>
+        /// Subtracts the components of matrix b from matrix a.
+        /// </summary>
+        /// <param name="b">Matrix to be subtracted from</param>
+        /// <param name="a">Matrix to subtract from matrix a.</param>
+        /// <returns>Matrix with components equal to the components of the matrix a minus the components of matrix b.</returns>
+        [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;
+        }
+
         /// <summary>
         /// Multiplies every component in the matrix by the given scale.
         /// </summary>
@@ -194,6 +320,25 @@ public static void Scale(in Symmetric3x3 m, float scale, out Symmetric3x3 scaled
             scaled.ZZ = m.ZZ * scale;
         }
 
+        /// <summary>
+        /// Multiplies every component in the matrix by the given scale.
+        /// </summary>
+        /// <param name="m">Matrix to be scaled.</param>
+        /// <param name="scale">Scale to apply to every component of the original matrix.</param>
+        /// <returns>Scaled result.</returns>
+        [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;
+        }
+
         /// <summary>
         /// Multiplies the two matrices as if they were symmetric.
         /// </summary>
@@ -216,6 +361,29 @@ public static void MultiplyWithoutOverlap(in Symmetric3x3 a, in Symmetric3x3 b,
             result.ZZ = azxbzx + azybzy + a.ZZ * b.ZZ;
         }
 
+        /// <summary>
+        /// Multiplies the two matrices as if they were symmetric.
+        /// </summary>
+        /// <param name="a">First matrix to multiply.</param>
+        /// <param name="b">Second matrix to multiply.</param>
+        /// <returns>Product of the multiplication.</returns>
+        [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;
+        }
 
         /// <summary>
         /// Multiplies the two matrices.
@@ -251,6 +419,67 @@ public static void Multiply(in Matrix3x3 a, in Symmetric3x3 b, out Matrix3x3 res
             }
         }
 
+
+        /// <summary>
+        /// Multiplies the two matrices.
+        /// </summary>
+        /// <param name="a">First matrix to multiply.</param>
+        /// <param name="b">Second matrix to multiply.</param>
+        /// <returns>Product of the multiplication.</returns>
+        [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;
+        }
+
+
+
+        /// <summary>
+        /// Multiplies the two matrices.
+        /// </summary>
+        /// <param name="a">First matrix to multiply.</param>
+        /// <param name="b">Second matrix to multiply.</param>
+        /// <returns>Product of the multiplication.</returns>
+        [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;
+        }
+
         /// <summary>
         /// Transforms a vector by a symmetric matrix.
         /// </summary>
@@ -258,11 +487,27 @@ public static void Multiply(in Matrix3x3 a, in Symmetric3x3 b, out Matrix3x3 res
         /// <param name="m">Matrix to interpret as symmetric transform.</param>
         /// <param name="result">Result of transforming the vector by the given symmetric matrix.</param>
         [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;
+            result.Z = v.X * m.ZX + v.Y * m.ZY + v.Z * m.ZZ;
+        }
+
+        /// <summary>
+        /// Transforms a vector by a symmetric matrix.
+        /// </summary>
+        /// <param name="v">Vector to transform.</param>
+        /// <param name="m">Matrix to interpret as symmetric transform.</param>
+        /// <returns>Result of transforming the vector by the given symmetric matrix.</returns>
+        [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()
diff --git a/BepuUtilities/Symmetric3x3Wide.cs b/BepuUtilities/Symmetric3x3Wide.cs
index f9ea5307c..05ff1c7a5 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
 {
@@ -41,7 +38,7 @@ public struct Symmetric3x3Wide
         /// </summary>
         /// <param name="m">Symmetric matrix to invert.</param>
         /// <param name="inverse">Inverse of the symmetric matrix.</param>
-        [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;
@@ -78,6 +75,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;
         }
+        /// <summary>
+        /// Adds the components of two symmetric matrices together.
+        /// </summary>
+        /// <param name="a">First matrix to add.</param>
+        /// <param name="b">Second matrix to add.</param>
+        /// <returns>Sum of the two input matrices.</returns>
+        [MethodImpl(MethodImplOptions.AggressiveInlining)]
+        public static Symmetric3x3Wide operator +(in Symmetric3x3Wide a, in Symmetric3x3Wide b)
+        {
+            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;
+        }
+
 
         /// <summary>
         /// Subtracts one symmetric matrix's components from another.
@@ -96,6 +112,24 @@ public static void Subtract(in Symmetric3x3Wide a, in Symmetric3x3Wide b, out Sy
             result.ZZ = a.ZZ - b.ZZ;
         }
 
+        /// <summary>
+        /// Subtracts one symmetric matrix's components from another.
+        /// </summary>
+        /// <param name="a">Matrix to be subtracted from.</param>
+        /// <param name="b">Matrix to subtract from the first matrix.</param>
+        /// <returns>Result of a - b.</returns>
+        [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<float> scale, out Symmetric3x3Wide result)
@@ -107,7 +141,20 @@ public static void Scale(in Symmetric3x3Wide m, in Vector<float> 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<float> 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 +267,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;
         }
 
+        /// <summary>
+        /// Computes result = a * b, assuming that b represents a symmetric 3x3 matrix. Assumes that input parameters and output result do not overlap.
+        /// </summary>
+        /// <param name="a">First matrix of the pair to multiply.</param>
+        /// <param name="b">Matrix to be reinterpreted as symmetric for the multiply.</param>
+        /// <returns>Result of multiplying a * b.</returns>
+        [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;
+        }
+
         /// <summary>
         /// Computes result = a * b, assuming that b represents a symmetric 3x3 matrix. Assumes that input parameters and output result do not overlap.
         /// </summary>
@@ -242,6 +308,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;
         }
 
+
+        /// <summary>
+        /// Computes result = a * b, assuming that b represents a symmetric 3x3 matrix. Assumes that input parameters and output result do not overlap.
+        /// </summary>
+        /// <param name="a">First matrix of the pair to multiply.</param>
+        /// <param name="b">Matrix to be reinterpreted as symmetric for the multiply.</param>
+        /// <returns>Result of multiplying a * b.</returns>
+        [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;
+        }
+
         /// <summary>
         /// Computes result = a * b, assuming that a represents a symmetric 3x3 matrix. Assumes that input parameters and output result do not overlap.
         /// </summary>
@@ -264,6 +355,29 @@ 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;
         }
 
+        /// <summary>
+        /// Computes result = a * b, assuming that a represents a symmetric 3x3 matrix. Assumes that input parameters and output result do not overlap.
+        /// </summary>
+        /// <param name="a">Matrix to be reinterpreted as symmetric for the multiply.</param>
+        /// <param name="b">Second matrix of the pair to multiply.</param>
+        [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;
+        }
+
         /// <summary>
         /// Computes result = a * transpose(b).
         /// </summary>
@@ -310,7 +424,7 @@ public static void MultiplyByTransposed(in Symmetric3x3Wide a, in Matrix2x3Wide
         /// </summary>
         /// <param name="m">Matrix to use as the sandwich bread.</param>
         /// <param name="t">Succulent interior symmetric matrix.</param>
-        /// <param name="sandwich">Result of m * t * mT for a symmetric matrix t.</param>
+        /// <param name="result">Result of m * t * mT for a symmetric matrix t.</param>
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
         public static void MatrixSandwich(in Matrix2x3Wide m, in Symmetric3x3Wide t, out Symmetric2x2Wide result)
         {
@@ -401,7 +515,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 +525,49 @@ 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 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)
         {
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 d2c11f954..bb2375196 100644
--- a/BepuUtilities/Symmetric6x6Wide.cs
+++ b/BepuUtilities/Symmetric6x6Wide.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
 {
@@ -46,7 +43,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);
@@ -71,5 +68,64 @@ 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;
         }
+
+        /// <summary>
+        /// Solves [vLower, vUpper] = [resultLower, resultUpper] * [[a, b], [bT, d]] for [resultLower, resultUpper] using LDLT decomposition.
+        /// [[a, b], [bT, d]] should be positive semidefinite.
+        /// </summary>
+        /// <param name="v0">First 3 values of the 6 component input vector.</param>
+        /// <param name="v1">Second 3 values of the 6 component input vector.</param>
+        /// <param name="a">Upper left 3x3 region of the matrix.</param>
+        /// <param name="b">Upper right 3x3 region of the matrix. Also the lower left 3x3 region of the matrix, transposed.</param>
+        /// <param name="d">Lower right 3x3 region of the matrix.</param>
+        /// <param name="result0">First 3 values of the result vector.</param>
+        /// <param name="result1">Second 3 values of the result vector.</param>
+        //[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<float>.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<float>.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<float>.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<float>.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<float>.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<float>.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;
+        }
     }
 }
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;
+
+/// <summary>
+/// Stores a block of task continuations that maintains a pointer to previous blocks. 
+/// </summary>
+internal unsafe struct ContinuationBlock
+{
+    public ContinuationBlock* Previous;
+
+    public int Count;
+    public Buffer<TaskContinuation> Continuations;
+
+    public static ContinuationBlock* Create(int continuationCapacity, BufferPool pool)
+    {
+        pool.Take<byte>(sizeof(TaskContinuation) * continuationCapacity + sizeof(ContinuationBlock), out var rawBuffer);
+        ContinuationBlock* block = (ContinuationBlock*)rawBuffer.Memory;
+        block->Continuations = new Buffer<TaskContinuation>(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;
+
+/// <summary>
+/// Refers to a continuation within a <see cref="TaskStack"/>.
+/// </summary>
+public unsafe struct ContinuationHandle : IEquatable<ContinuationHandle>
+{
+    //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;
+    }
+
+    /// <summary>
+    /// Gets whether the tasks associated with this continuation have completed. If the continuation has not been initialized, this will always return false.
+    /// </summary>
+    public bool Completed
+    {
+        get
+        {
+            return Initialized && continuation->RemainingTaskCounter <= 0;
+        }
+    }
+
+    /// <summary>
+    /// Retrieves a pointer to the continuation data for <see cref="ContinuationHandle"/>.
+    /// </summary>
+    /// <returns>Pointer to the continuation backing the given handle.</returns>
+    /// <remarks>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.</remarks>
+    public TaskContinuation* Continuation => continuation;
+
+    /// <summary>
+    /// Gets a null continuation handle.
+    /// </summary>
+    public static ContinuationHandle Null => default;
+
+    /// <summary>
+    /// Gets whether this handle ever represented an allocated handle. This does not guarantee that the continuation's associated tasks are active in the <see cref="TaskStack"/> that it was allocated from.
+    /// </summary>
+    public bool Initialized => continuation != null;
+
+    /// <summary>
+    /// Notifies the continuation that one task was completed.
+    /// </summary>
+    /// <param name="workerIndex">Worker index to pass to the continuation's delegate, if any.</param>
+    /// <param name="dispatcher">Dispatcher to pass to the continuation's delegate, if any.</param>
+    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;
+
+/// <summary>
+/// Determines which jobs are allowed to serve a <see cref="TaskStack.TryPop{TJobFilter}(ref TJobFilter, out Task)"/> request.
+/// </summary>
+public interface IJobFilter
+{
+    /// <summary>
+    /// Determines whether a job with the given tag should be allowed to serve a <see cref="TaskStack.TryPop{TJobFilter}(ref TJobFilter, out Task)"/> request.
+    /// </summary>
+    /// <param name="jobTag">Tag of the candidate job.</param>
+    /// <returns>True if the job should be allowed to serve a request, false otherwise.</returns>
+    bool AllowJob(ulong jobTag);
+}
+
+
+/// <summary>
+/// A filter that will allow pops from any jobs.
+/// </summary>
+public struct AllowAllJobs : IJobFilter
+{
+    /// <inheritdoc/>
+    public readonly bool AllowJob(ulong jobTag)
+    {
+        return true;
+    }
+}
+
+/// <summary>
+/// A job filter that wraps a managed delegate.
+/// </summary>
+public struct DelegateJobFilter : IJobFilter
+{
+    /// <summary>
+    /// Delegate to use as the filter.
+    /// </summary>
+    public Func<ulong, bool> Filter;
+    /// <summary>
+    /// Creates a job filter that wraps a delegate.
+    /// </summary>
+    /// <param name="filter">Delegate to use as the filter.</param>
+    public DelegateJobFilter(Func<ulong, bool> filter)
+    {
+        Filter = filter;
+    }
+    /// <inheritdoc/>
+    public readonly bool AllowJob(ulong jobTag)
+    {
+        return Filter(jobTag);
+    }
+}
+
+/// <summary>
+/// A job filter that wraps a function pointer.
+/// </summary>
+public unsafe struct FunctionPointerJobFilter : IJobFilter
+{
+    /// <summary>
+    /// Delegate to use as the filter.
+    /// </summary>
+    public delegate*<ulong, bool> Filter;
+    /// <summary>
+    /// Creates a job filter that wraps a delegate.
+    /// </summary>
+    /// <param name="filter">Delegate to use as the filter.</param>
+    public FunctionPointerJobFilter(delegate*<ulong, bool> filter)
+    {
+        Filter = filter;
+    }
+    /// <inheritdoc/>
+    public readonly bool AllowJob(ulong jobTag)
+    {
+        return Filter(jobTag);
+    }
+}
+/// <summary>
+/// A job filter that requires the job tag to meet or exceed a threshold value.
+/// </summary>
+public struct MinimumTagFilter : IJobFilter
+{
+    /// <summary>
+    /// Value that a job must match or exceed to be allowed.
+    /// </summary>
+    public ulong MinimumTagValue;
+    /// <summary>
+    /// Creates a job filter that requires the job tag to meet or exceed a threshold value.
+    /// </summary>
+    /// <param name="minimumTagValue">Value that a job must match or exceed to be allowed.</param>
+    public MinimumTagFilter(ulong minimumTagValue)
+    {
+        MinimumTagValue = minimumTagValue;
+    }
+    /// <inheritdoc/>
+    public bool AllowJob(ulong jobTag)
+    {
+        return jobTag >= MinimumTagValue;
+    }
+}
+
+/// <summary>
+/// A job filter that requires the job tag to match a specific value.
+/// </summary>
+public struct EqualTagFilter : IJobFilter
+{
+    /// <summary>
+    /// Tag value required to allow a job.
+    /// </summary>
+    public ulong RequiredTag;
+
+    /// <summary>
+    /// Creates a job filter that requires the job tag to match a specific value.
+    /// </summary>
+    public EqualTagFilter(ulong requiredTag)
+    {
+        RequiredTag = requiredTag;
+    }
+    /// <inheritdoc/>
+    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..09535d79d
--- /dev/null
+++ b/BepuUtilities/TaskScheduling/Job.cs
@@ -0,0 +1,64 @@
+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<Task> Tasks;
+    [FieldOffset(16)]
+    public Job* Previous;
+    [FieldOffset(24)]
+    public ulong Tag;
+
+    [FieldOffset(160)]
+    public int Counter;
+
+
+    public static Job* Create(Span<Task> 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<byte>(sizeToAllocate, out var rawBuffer);
+        Job* job = (Job*)rawBuffer.Memory;
+        job->Tasks = new Buffer<Task>(rawBuffer.Memory + sizeof(Job), sourceTasks.Length, rawBuffer.Id);
+        job->Tag = tag;
+        sourceTasks.CopyTo(job->Tasks);
+        job->Counter = sourceTasks.Length;
+        job->Previous = null;
+        return job;
+    }
+
+
+    /// <summary>
+    /// Attempts to pop a task from the job.
+    /// </summary>
+    /// <param name="task">Task popped from the job, if any.</param>
+    /// <returns>True if a task was available to pop, false otherwise.</returns>
+    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;
+
+/// <summary>
+/// Describes the result status of a pop attempt.
+/// </summary>
+public enum PopTaskResult
+{
+    /// <summary>
+    /// A task was successfully popped.
+    /// </summary>
+    Success,
+    /// <summary>
+    /// The stack was empty, but may have more tasks in the future.
+    /// </summary>
+    Empty,
+    /// <summary>
+    /// The stack has been terminated and all threads seeking work should stop.
+    /// </summary>
+    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;
+
+/// <summary>
+/// Description of a task to be submitted to a <see cref="TaskStack"/>.
+/// </summary>
+public unsafe struct Task
+{
+    /// <summary>
+    /// Function to be executed by the task. Takes as arguments the <see cref="Id"/>, <see cref="Context"/> pointer, and executing worker index.
+    /// </summary>
+    public delegate*<long, void*, int, IThreadDispatcher, void> Function;
+    /// <summary>
+    /// Context to be passed into the <see cref="Function"/>.
+    /// </summary>
+    public void* Context;
+    /// <summary>
+    /// Continuation to be notified after this task completes, if any.
+    /// </summary>
+    public ContinuationHandle Continuation;
+    /// <summary>
+    /// User-provided identifier of this task.
+    /// </summary>
+    public long Id;
+
+    /// <summary>
+    /// Creates a new task.
+    /// </summary>
+    /// <param name="function">Function to be executed by the task. Takes as arguments the <see cref="Id"/>, <see cref="Context"/> pointer, executing worker index, and executing <see cref="IThreadDispatcher"/>.</param>
+    /// <param name="context">Context pointer to pass to the <see cref="Function"/>.</param>
+    /// <param name="taskId">Id of this task to be passed into the <see cref="Function"/>.</param>
+    /// <param name="continuation">Continuation to notify after the completion of this task, if any.</param>
+    public Task(delegate*<long, void*, int, IThreadDispatcher, void> function, void* context = null, long taskId = 0, ContinuationHandle continuation = default)
+    {
+        Function = function;
+        Context = context;
+        Continuation = continuation;
+        Id = taskId;
+    }
+
+    /// <summary>
+    /// Creates a task from a function.
+    /// </summary>
+    /// <param name="function">Function to turn into a task.</param>
+    public static implicit operator Task(delegate*<long, void*, int, IThreadDispatcher, void> function) => new(function);
+
+    /// <summary>
+    /// Runs the task and, if necessary, notifies the associated continuation of its completion.
+    /// </summary>
+    /// <param name="workerIndex">Worker index to pass to the function.</param> 
+    /// <param name="dispatcher">Dispatcher running this task.</param>
+    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..281db2cb8
--- /dev/null
+++ b/BepuUtilities/TaskScheduling/TaskContinuation.cs
@@ -0,0 +1,16 @@
+namespace BepuUtilities.TaskScheduling;
+
+/// <summary>
+/// Stores data relevant to tracking task completion and reporting completion for a continuation.
+/// </summary>
+public struct TaskContinuation
+{
+    /// <summary>
+    /// Task to run upon completion of the associated task.
+    /// </summary>
+    public Task OnCompleted;
+    /// <summary>
+    /// Number of tasks not yet reported as complete in the continuation.
+    /// </summary>
+    public int RemainingTaskCounter;
+}
diff --git a/BepuUtilities/TaskScheduling/TaskStack.cs b/BepuUtilities/TaskScheduling/TaskStack.cs
new file mode 100644
index 000000000..ef4810fe2
--- /dev/null
+++ b/BepuUtilities/TaskScheduling/TaskStack.cs
@@ -0,0 +1,596 @@
+using System;
+using System.Diagnostics;
+using System.Runtime.InteropServices;
+using System.Threading;
+using BepuUtilities.Memory;
+
+namespace BepuUtilities.TaskScheduling;
+
+/// <summary>
+/// Manages a linked stack of tasks.
+/// </summary>
+public unsafe struct TaskStack
+{
+    Buffer<Worker> workers;
+
+    [StructLayout(LayoutKind.Explicit, Size = 256 + 16)]
+    struct StopPad
+    {
+        [FieldOffset(128)]
+        public volatile bool Stop;
+    }
+    StopPad padded;
+
+    /// <summary>
+    /// Most recently pushed job on the stack. May be null if the stack is empty.
+    /// </summary>
+    /// <remarks>Pointers and generics don't play well, alas.</remarks>
+    volatile nuint head;
+
+    /// <summary>
+    /// Constructs a new parallel task stack.
+    /// </summary>
+    /// <param name="pool">Buffer pool to allocate non-thread allocated resources from.</param>
+    /// <param name="dispatcher">Thread dispatcher to pull thread pools from for thread allocations.</param>
+    /// <param name="workerCount">Number of workers to allocate space for.</param>
+    /// <param name="initialWorkerJobCapacity">Initial number of jobs (groups of tasks submitted together) to allocate space for in each worker.</param>
+    /// <param name="continuationBlockCapacity">Number of slots to allocate in each block of continuations in each worker.</param>
+    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)
+        {
+            workers[i] = new Worker(i, dispatcher, initialWorkerJobCapacity, continuationBlockCapacity);
+        }
+        Reset(dispatcher);
+    }
+
+    /// <summary>
+    /// Returns the stack to a fresh state without reallocating.
+    /// </summary>
+    /// <param name="dispatcher">Dispatcher whose thread pools should be used to return any thread allocated resources.</param>
+    public void Reset(IThreadDispatcher dispatcher)
+    {
+        for (int i = 0; i < workers.Length; ++i)
+        {
+            workers[i].Reset(dispatcher.WorkerPools[workers[i].WorkerIndex]);
+        }
+        padded.Stop = false;
+        head = (nuint)null;
+    }
+
+    /// <summary>
+    /// Returns unmanaged resources held by the <see cref="TaskStack"/> to a pool.
+    /// </summary>
+    /// <param name="pool">Buffer pool to return resources to.</param>
+    /// <param name="dispatcher">Dispatcher whose thread pools should be used to return any thread allocated resources.</param>
+    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);
+    }
+
+    /// <summary>
+    /// 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.
+    /// </summary>
+    public int ApproximateTaskCount
+    {
+        get
+        {
+            int sum = 0;
+            var job = (Job*)head;
+            while (true)
+            {
+                if (job == null)
+                    break;
+                sum += int.Max(0, job->Counter);
+                job = job->Previous;
+            }
+            return sum;
+        }
+    }
+    /// <summary>
+    /// 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.
+    /// </summary>
+    public int ApproximateContinuationCount
+    {
+        get
+        {
+            int sum = 0;
+            for (int i = 0; i < workers.Length; ++i)
+            {
+                var block = workers[i].ContinuationHead;
+                while (block != null)
+                {
+                    sum += block->Count;
+                    block = block->Previous;
+                }
+            }
+            return sum;
+        }
+    }
+
+
+    /// <summary>
+    /// Attempts to allocate a continuation for a set of tasks.
+    /// </summary>
+    /// <param name="taskCount">Number of tasks associated with the continuation.</param>
+    /// <param name="workerIndex">Worker index to allocate the continuation on.</param>
+    /// <param name="dispatcher">Dispatcher to use for any per-thread allocations if necessary.</param>
+    /// <param name="onCompleted">Function to execute upon completing all associated tasks, if any. Any task with a null <see cref="Task.Function"/> will not be executed.</param>
+    /// <returns>Handle of the continuation.</returns>
+    public ContinuationHandle AllocateContinuation(int taskCount, int workerIndex, IThreadDispatcher dispatcher, Task onCompleted = default)
+    {
+        return workers[workerIndex].AllocateContinuation(taskCount, dispatcher, onCompleted);
+    }
+
+    /// <summary>
+    /// Attempts to pop a task.
+    /// </summary>
+    /// <param name="filter">Filter to apply to jobs. Only allowed jobs can have tasks popped from them.</param>
+    /// <param name="task">Popped task, if any.</param>
+    /// <typeparam name="TJobFilter">Type of the job filter used in the pop.</typeparam>
+    /// <returns>Result status of the pop attempt.</returns>
+    public PopTaskResult TryPop<TJobFilter>(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)
+        {
+            if (job == null)
+            {
+                //There is no job to pop from.
+                task = default;
+                return padded.Stop ? PopTaskResult.Stop : PopTaskResult.Empty;
+            }
+            //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);
+                return PopTaskResult.Success;
+            }
+            else
+            {
+                //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.
+                //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;
+            }
+        }
+    }
+
+    /// <summary>
+    /// Attempts to pop a task.
+    /// </summary>
+    /// <param name="task">Popped task, if any.</param>
+    /// <returns>Result status of the pop attempt.</returns>
+    public PopTaskResult TryPop(out Task task)
+    {
+        AllowAllJobs filter = default;
+        return TryPop(ref filter, out task);
+    }
+
+    /// <summary>
+    /// Attempts to pop a task and run it.
+    /// </summary>
+    /// <param name="filter">Filter to apply to jobs. Only allowed jobs can have tasks popped from them.</param>
+    /// <param name="workerIndex">Index of the worker to pass into the task function.</param>
+    /// <param name="dispatcher">Thread dispatcher running this task stack.</param>
+    /// <typeparam name="TJobFilter">Type of the job filter used in the pop.</typeparam>
+    /// <returns>Result status of the pop attempt.</returns>
+    public PopTaskResult TryPopAndRun<TJobFilter>(ref TJobFilter filter, int workerIndex, IThreadDispatcher dispatcher) where TJobFilter : IJobFilter
+    {
+        var result = TryPop(ref filter, out var task);
+        if (result == PopTaskResult.Success)
+        {
+            task.Run(workerIndex, dispatcher);
+        }
+        return result;
+    }
+
+    /// <summary>
+    /// Attempts to pop a task and run it.
+    /// </summary>
+    /// <param name="workerIndex">Index of the worker to pass into the task function.</param>
+    /// <param name="dispatcher">Thread dispatcher running this task stack.</param>
+    /// <returns>Result status of the pop attempt.</returns>
+    public PopTaskResult TryPopAndRun(int workerIndex, IThreadDispatcher dispatcher)
+    {
+        AllowAllJobs filter = default;
+        return TryPopAndRun(ref filter, workerIndex, dispatcher);
+    }
+
+
+    /// <summary>
+    /// Pushes a set of tasks onto the task stack. This function is not thread safe.
+    /// </summary>
+    /// <param name="tasks">Tasks composing the job.</param>
+    /// <param name="dispatcher">Thread dispatcher to allocate thread data from if necessary.</param>
+    /// <param name="workerIndex">Index of the worker stack to push the tasks onto.</param>
+    /// <param name="tag">User-defined tag data for the submitted job.</param>
+    /// <remarks>This must not be used while other threads could be performing task pushes or pops that could affect the specified worker.</remarks>
+    public void PushUnsafely(Span<Task> tasks, int workerIndex, IThreadDispatcher dispatcher, ulong tag = 0)
+    {
+        Job* job = workers[workerIndex].AllocateJob(tasks, tag, dispatcher);
+        job->Previous = (Job*)head;
+        head = (nuint)job;
+    }
+    /// <summary>
+    /// Pushes a task onto the task stack. This function is not thread safe.
+    /// </summary>
+    /// <param name="task">Task to push.</param>
+    /// <param name="dispatcher">Thread dispatcher to allocate thread data from if necessary.</param>
+    /// <param name="workerIndex">Index of the worker stack to push the tasks onto.</param>
+    /// <param name="tag">User tag associated with the job spanning the submitted tasks.</param>
+    /// <remarks>This must not be used while other threads could be performing task pushes or pops that could affect the specified worker.</remarks>
+    public void PushUnsafely(Task task, int workerIndex, IThreadDispatcher dispatcher, ulong tag = 0)
+    {
+        PushUnsafely(new Span<Task>(&task, 1), workerIndex, dispatcher, tag);
+    }
+
+    /// <summary>
+    /// Pushes a set of tasks onto the task stack.
+    /// </summary>
+    /// <param name="tasks">Tasks composing the job.</param>
+    /// <param name="dispatcher">Thread dispatcher to allocate thread data from if necessary.</param>
+    /// <param name="workerIndex">Index of the worker stack to push the tasks onto.</param>
+    /// <param name="tag">User-defined tag data for the submitted job.</param>
+    /// <returns>True if the push succeeded, false if it was contested.</returns>
+    public void Push(Span<Task> tasks, int workerIndex, IThreadDispatcher dispatcher, ulong tag = 0)
+    {
+        Job* job = workers[workerIndex].AllocateJob(tasks, tag, 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;
+        }
+    }
+
+    /// <summary>
+    /// Pushes a task onto the task stack.
+    /// </summary>
+    /// <param name="task">Task composing the job.</param>
+    /// <param name="dispatcher">Thread dispatcher to allocate thread data from if necessary.</param>
+    /// <param name="workerIndex">Index of the worker stack to push the task onto.</param>
+    /// <param name="tag">User-defined tag data for the submitted job.</param>
+    /// <returns>True if the push succeeded, false if it was contested.</returns>
+    public void Push(Task task, int workerIndex, IThreadDispatcher dispatcher, ulong tag = 0)
+    {
+        Push(new Span<Task>(ref task), workerIndex, dispatcher, tag);
+    }
+
+    /// <summary>
+    /// Pushes a set of tasks to the stack with a created continuation.
+    /// </summary>
+    /// <param name="tasks">Tasks composing the job. A continuation will be assigned internally; no continuation should be present on any of the provided tasks.</param>
+    /// <param name="dispatcher">Thread dispatcher to allocate thread data from if necessary.</param>
+    /// <param name="workerIndex">Index of the worker stack to push the tasks onto.</param>
+    /// <param name="tag">User tag associated with the job spanning the submitted tasks.</param>
+    /// <param name="onComplete">Task to run upon completion of all the submitted tasks, if any.</param>
+    /// <returns>Handle of the continuation created for these tasks.</returns>
+    public ContinuationHandle AllocateContinuationAndPush(Span<Task> 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)
+        {
+            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, tag);
+        return continuationHandle;
+    }
+
+    /// <summary>
+    /// Pushes a task to the stack with a created continuation.
+    /// </summary>
+    /// <param name="task">Task composing the job. A continuation will be assigned internally; no continuation should be present on any of the provided task.</param>
+    /// <param name="dispatcher">Thread dispatcher to allocate thread data from if necessary.</param>
+    /// <param name="workerIndex">Index of the worker stack to push the task onto.</param>
+    /// <param name="tag">User tag associated with the task's job.</param>
+    /// <param name="onComplete">Task to run upon completion of all the submitted task, if any.</param>
+    /// <returns>Handle of the continuation created for these task.</returns>
+    public ContinuationHandle AllocateContinuationAndPush(Task task, int workerIndex, IThreadDispatcher dispatcher, ulong tag = 0, Task onComplete = default)
+    {
+        return AllocateContinuationAndPush(new Span<Task>(ref task), workerIndex, dispatcher, tag, onComplete);
+    }
+
+    /// <summary>
+    /// Waits for a continuation to be completed.
+    /// </summary>
+    /// <remarks>Instead of spinning the entire time, this may pop and execute pending tasks to fill the gap.</remarks>
+    /// <param name="filter">Filter to apply to jobs. Only allowed jobs can have tasks popped from them.</param>
+    /// <param name="continuation">Continuation to wait on.</param>
+    /// <param name="dispatcher">Thread dispatcher to allocate thread data from if necessary.</param>
+    /// <param name="workerIndex">Index of the executing worker.</param>
+    /// <typeparam name="TJobFilter">Type of the job filter used in the pop.</typeparam>
+    public void WaitForCompletion<TJobFilter>(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(ref filter, out var fillerTask);
+            if (result == PopTaskResult.Stop)
+            {
+                return;
+            }
+            if (result == PopTaskResult.Success)
+            {
+                fillerTask.Run(workerIndex, dispatcher);
+                waiter.Reset();
+            }
+            else
+            {
+                waiter.SpinOnce(-1);
+            }
+        }
+    }
+
+    /// <summary>
+    /// Waits for a continuation to be completed.
+    /// </summary>
+    /// <remarks>Instead of spinning the entire time, this may pop and execute pending tasks to fill the gap.</remarks>
+    /// <param name="continuation">Continuation to wait on.</param>
+    /// <param name="dispatcher">Thread dispatcher to allocate thread data from if necessary.</param>
+    /// <param name="workerIndex">Index of the executing worker.</param>
+    public void WaitForCompletion(ContinuationHandle continuation, int workerIndex, IThreadDispatcher dispatcher)
+    {
+        AllowAllJobs filter = default;
+        WaitForCompletion(ref filter, continuation, workerIndex, dispatcher);
+    }
+
+    /// <summary>
+    /// Pushes a set of tasks to the worker stack and returns when all tasks are complete.
+    /// </summary>
+    /// <param name="tasks">Tasks composing the job. A continuation will be assigned internally; no continuation should be present on any of the provided tasks.</param>
+    /// <param name="workerIndex">Index of the worker executing this function.</param>
+    /// <param name="dispatcher">Thread dispatcher to allocate thread data from if necessary.</param>
+    /// <param name="filter">Filter applied to jobs considered for filling the calling thread's wait for other threads to complete.</param>
+    /// <param name="tag">User tag associated with the job spanning the submitted tasks.</param>
+    /// <typeparam name="TJobFilter">Type of the job filter used in the pop.</typeparam>
+    /// <remarks>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.</remarks>
+    public void RunTasks<TJobFilter>(Span<Task> tasks, int workerIndex, IThreadDispatcher dispatcher, ref TJobFilter filter, ulong tag = 0) where TJobFilter : IJobFilter
+    {
+        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<Task> 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(!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, 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.
+        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(ref filter, continuationHandle, workerIndex, dispatcher);
+        }
+    }
+
+
+    /// <summary>
+    /// Pushes a set of tasks to the worker stack and returns when all tasks are complete.
+    /// </summary>
+    /// <param name="tasks">Tasks composing the job. A continuation will be assigned internally; no continuation should be present on any of the provided tasks.</param>
+    /// <param name="workerIndex">Index of the worker executing this function.</param>
+    /// <param name="dispatcher">Thread dispatcher to allocate thread data from if necessary.</param>
+    /// <param name="tag">User tag associated with the job spanning the submitted tasks.</param>
+    /// <remarks>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.</remarks>
+    public void RunTasks(Span<Task> tasks, int workerIndex, IThreadDispatcher dispatcher, ulong tag = 0)
+    {
+        AllowAllJobs filter = default;
+        RunTasks(tasks, workerIndex, dispatcher, ref filter, tag);
+    }
+
+    /// <summary>
+    /// Pushes a task to the worker stack and returns when it completes.
+    /// </summary>
+    /// <param name="task">Task composing the job. A continuation will be assigned internally; no continuation should be present on the task.</param>
+    /// <param name="workerIndex">Index of the worker executing this function.</param>
+    /// <param name="dispatcher">Thread dispatcher to allocate thread data from if necessary.</param>
+    /// <param name="tag">User tag associated with the job spanning the submitted task.</param>
+    /// <remarks>Note that this will keep working until the task completes. It may execute tasks unrelated to the requested task while waiting on other workers.</remarks>
+    public void RunTask(Task task, int workerIndex, IThreadDispatcher dispatcher, ulong tag = 0)
+    {
+        RunTasks(new Span<Task>(ref task), workerIndex, dispatcher, tag);
+    }
+
+    /// <summary>
+    /// Pushes a task to the worker stack and returns when all tasks are complete.
+    /// </summary>
+    /// <param name="task">Task composing the job. A continuation will be assigned internally; no continuation should be present on the task.</param>
+    /// <param name="workerIndex">Index of the worker executing this function.</param>
+    /// <param name="dispatcher">Thread dispatcher to allocate thread data from if necessary.</param>
+    /// <param name="filter">Filter applied to jobs considered for filling the calling thread's wait for other threads to complete.</param>
+    /// <param name="tag">User tag associated with the job spanning the submitted task.</param>
+    /// <typeparam name="TJobFilter">Type of the job filter used in the pop.</typeparam>
+    /// <remarks>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.</remarks>
+    public void RunTask<TJobFilter>(Task task, int workerIndex, IThreadDispatcher dispatcher, ref TJobFilter filter, ulong tag = 0) where TJobFilter : IJobFilter
+    {
+        RunTasks(new Span<Task>(ref task), workerIndex, dispatcher, ref filter, tag);
+    }
+
+    /// <summary>
+    /// 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.
+    /// </summary>
+    public void RequestStop()
+    {
+        padded.Stop = true;
+    }
+
+    /// <summary>
+    /// Convenience function for requesting a stop. Requires the context to be a pointer to the expected <see cref="TaskStack"/>.
+    /// </summary>
+    /// <param name="id">Id of the task.</param>
+    /// <param name="untypedContext"><see cref="TaskStack"/> to be stopped.</param>
+    /// <param name="workerIndex">Index of the worker executing this task.</param>
+    /// <param name="dispatcher">Dispatcher associated with the execution.</param>
+    public static void RequestStopTaskFunction(long id, void* untypedContext, int workerIndex, IThreadDispatcher dispatcher)
+    {
+        ((TaskStack*)untypedContext)->RequestStop();
+    }
+
+    /// <summary>
+    /// Convenience function for getting a task representing a stop request.
+    /// </summary>
+    /// <param name="stack">Stack to be stopped.</param>
+    /// <returns>Task representing a stop request.</returns>
+    public static Task GetRequestStopTask(TaskStack* stack) => new(&RequestStopTaskFunction, stack);
+
+    /// <summary>
+    /// Pushes a for loop onto the task stack. Does not take a lock.
+    /// </summary>
+    /// <param name="function">Function to execute on each iteration of the loop.</param>
+    /// <param name="context">Context pointer to pass into each task execution.</param>
+    /// <param name="inclusiveStartIndex">Inclusive start index of the loop range.</param>
+    /// <param name="iterationCount">Number of iterations to perform.</param>
+    /// <param name="dispatcher">Thread dispatcher to allocate thread data from if necessary.</param>
+    /// <param name="workerIndex">Index of the worker stack to push the tasks onto.</param>
+    /// <param name="tag">User tag associated with the job spanning the submitted tasks.</param>
+    /// <param name="continuation">Continuation associated with the loop tasks, if any.</param>
+    /// <remarks>This must not be used while other threads could be performing task pushes or pops that could affect the specified worker.</remarks>
+    public void PushForUnsafely(delegate*<long, void*, int, IThreadDispatcher, void> function, void* context, int inclusiveStartIndex, int iterationCount, int workerIndex, IThreadDispatcher dispatcher, ulong tag = 0, ContinuationHandle continuation = default)
+    {
+        Span<Task> 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, tag);
+    }
+
+    /// <summary>
+    /// Pushes a for loop onto the task stack.
+    /// </summary>
+    /// <param name="function">Function to execute on each iteration of the loop.</param>
+    /// <param name="context">Context pointer to pass into each task execution.</param>
+    /// <param name="inclusiveStartIndex">Inclusive start index of the loop range.</param>
+    /// <param name="iterationCount">Number of iterations to perform.</param>
+    /// <param name="dispatcher">Thread dispatcher to allocate thread data from if necessary.</param>
+    /// <param name="workerIndex">Index of the worker stack to push the tasks onto.</param> 
+    /// <param name="continuation">Continuation associated with the loop tasks, if any.</param>
+    /// <param name="tag">User tag associated with the job spanning the submitted tasks.</param>
+    /// <remarks>This function will not attempt to run any iterations of the loop itself.</remarks>
+    public void PushFor(delegate*<long, void*, int, IThreadDispatcher, void> function, void* context, int inclusiveStartIndex, int iterationCount, int workerIndex, IThreadDispatcher dispatcher, ulong tag = 0, ContinuationHandle continuation = default)
+    {
+        Span<Task> 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, tag);
+    }
+
+    /// <summary>
+    /// Submits a set of tasks representing a for loop over the given indices and returns when all loop iterations are complete.
+    /// </summary>
+    /// <param name="function">Function to execute on each iteration of the loop.</param>
+    /// <param name="context">Context pointer to pass into each iteration of the loop.</param>
+    /// <param name="inclusiveStartIndex">Inclusive start index of the loop range.</param>
+    /// <param name="iterationCount">Number of iterations to perform.</param>
+    /// <param name="workerIndex">Index of the worker stack to push the tasks onto.</param>
+    /// <param name="dispatcher">Thread dispatcher to allocate thread data from if necessary.</param>
+    /// <param name="filter">Filter applied to jobs considered for filling the calling thread's wait for other threads to complete.</param>
+    /// <param name="tag">User tag associated with the job spanning the submitted tasks.</param>
+    /// <typeparam name="TJobFilter">Type of the job filter used in the pop.</typeparam>
+    public void For<TJobFilter>(delegate*<long, void*, int, IThreadDispatcher, void> function, void* context, int inclusiveStartIndex, int iterationCount, int workerIndex, IThreadDispatcher dispatcher,
+        ref TJobFilter filter, ulong tag = 0) where TJobFilter : IJobFilter
+    {
+        if (iterationCount <= 0)
+            return;
+        Span<Task> tasks = stackalloc Task[iterationCount];
+        for (int i = 0; i < tasks.Length; ++i)
+        {
+            tasks[i] = new Task(function, context, inclusiveStartIndex + i);
+        }
+        RunTasks(tasks, workerIndex, dispatcher, ref filter, tag);
+    }
+
+    /// <summary>
+    /// Submits a set of tasks representing a for loop over the given indices and returns when all loop iterations are complete.
+    /// </summary>
+    /// <param name="function">Function to execute on each iteration of the loop.</param>
+    /// <param name="context">Context pointer to pass into each iteration of the loop.</param>
+    /// <param name="inclusiveStartIndex">Inclusive start index of the loop range.</param>
+    /// <param name="iterationCount">Number of iterations to perform.</param>
+    /// <param name="workerIndex">Index of the worker stack to push the tasks onto.</param>
+    /// <param name="dispatcher">Thread dispatcher to allocate thread data from if necessary.</param>
+    /// <param name="tag">User tag associated with the job spanning the submitted tasks.</param>
+    public void For(delegate*<long, void*, int, IThreadDispatcher, void> 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, tag);
+    }
+
+    /// <summary>
+    /// Worker function that pops tasks from the stack and executes them.
+    /// </summary>
+    /// <param name="workerIndex">Index of the worker calling this function.</param>
+    /// <param name="dispatcher">Thread dispatcher responsible for the invocation.</param>
+    public static void DispatchWorkerFunction(int workerIndex, IThreadDispatcher dispatcher)
+    {
+        var taskStack = (TaskStack*)dispatcher.UnmanagedContext;
+        var waiter = new SpinWait();
+        while (true)
+        {
+            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;
+            }
+        }
+    }
+
+    /// <summary>
+    /// Dispatches workers to execute tasks from the given stack.
+    /// </summary>
+    /// <param name="taskStack">Task stack to pull work from.</param>
+    /// <param name="dispatcher">Thread dispatcher to dispatch workers with.</param>
+    /// <param name="maximumWorkerCount">Maximum number of workers to spin up for the dispatch.</param>
+    /// <param name="managedContext">Managed context to include in this dispatch, if any.</param>
+    public static void DispatchWorkers(IThreadDispatcher dispatcher, TaskStack* taskStack, int maximumWorkerCount = int.MaxValue, object managedContext = null)
+    {
+        dispatcher.DispatchWorkers(&DispatchWorkerFunction, maximumWorkerCount, taskStack, managedContext);
+    }
+}
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<nuint> 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<nuint>(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);
+    }
+
+    /// <summary>
+    /// Pushes a set of tasks onto the stack.
+    /// </summary>
+    /// <param name="tasks">Tasks composing the job.</param>
+    /// <param name="tag">User tag associated with the job.</param>
+    /// <param name="dispatcher">Dispatcher used to pull thread allocations if necessary.</param>
+    /// <remarks>If the worker associated with this stack might be active, this function can only be called by the worker.</remarks>
+    internal Job* AllocateJob(Span<Task> 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;
+    }
+
+
+    /// <summary>
+    /// Allocates a continuation for a set of tasks.
+    /// </summary>
+    /// <param name="taskCount">Number of tasks associated with the continuation.</param>
+    /// <param name="dispatcher">Dispatcher from which to pull a buffer pool if needed for resizing.</param>
+    /// <param name="onCompleted">Function to execute upon completing all associated tasks, if any. Any task with a null <see cref="Task.Function"/> will not be executed.</param>
+    /// <returns>Handle of the continuation.</returns>
+    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/BepuUtilities/ThreadDispatcher.cs b/BepuUtilities/ThreadDispatcher.cs
new file mode 100644
index 000000000..d065f6546
--- /dev/null
+++ b/BepuUtilities/ThreadDispatcher.cs
@@ -0,0 +1,195 @@
+using System;
+using System.Diagnostics;
+using System.Runtime.InteropServices;
+using System.Threading;
+using BepuUtilities.Memory;
+
+namespace BepuUtilities
+{
+    /// <summary>
+    /// Provides a <see cref="IThreadDispatcher"/> implementation. Not reentrant.
+    /// </summary>
+    public unsafe class ThreadDispatcher : IThreadDispatcher, IDisposable
+    {
+        int threadCount;
+        /// <summary>
+        /// Gets the number of threads to dispatch work on.
+        /// </summary>
+        public int ThreadCount => threadCount;
+        struct Worker
+        {
+            public Thread Thread;
+            public AutoResetEvent Signal;
+        }
+
+        Worker[] workers;
+        AutoResetEvent finished;
+
+        /// <inheritdoc/>
+        public WorkerBufferPools WorkerPools { get; private set; }
+        /// <inheritdoc/>
+        public void* UnmanagedContext => unmanagedContext;
+        /// <inheritdoc/>
+        public object ManagedContext => managedContext;
+
+        /// <summary>
+        /// Creates a new thread dispatcher with the given number of threads.
+        /// </summary>
+        /// <param name="threadCount">Number of threads to dispatch on each invocation.</param>
+        /// <param name="threadPoolBlockAllocationSize">Size of memory blocks to allocate for thread pools.</param>
+        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)
+            {
+                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);
+            WorkerPools = new WorkerBufferPools(threadCount, threadPoolBlockAllocationSize);
+        }
+
+        void DispatchThread(int workerIndex)
+        {
+            switch (workerType)
+            {
+                case WorkerType.Managed: managedWorker(workerIndex); break;
+                case WorkerType.Unmanaged: unmanagedWorker(workerIndex, this); break;
+            }
+
+            if (Interlocked.Decrement(ref remainingWorkerCounter.Value) == -1)
+            {
+                finished.Set();
+            }
+        }
+
+        enum WorkerType
+        {
+            //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,
+            Unmanaged,
+        }
+
+        volatile WorkerType workerType;
+        volatile Action<int> managedWorker;
+        volatile delegate*<int, IThreadDispatcher, void> unmanagedWorker;
+        volatile void* unmanagedContext;
+        volatile object managedContext;
+
+        //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)
+        {
+            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.Value = workersToSignal;
+            for (int i = 0; i < workersToSignal; ++i)
+            {
+                workers[i].Signal.Set();
+            }
+        }
+
+        /// <inheritdoc/>
+        public void DispatchWorkers(delegate*<int, IThreadDispatcher, void> 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)
+            {
+                workerType = WorkerType.Unmanaged;
+                this.unmanagedWorker = workerBody;
+                SignalThreads(maximumWorkerCount);
+                //Calling thread does work. No reason to spin up another worker and block this one!
+                DispatchThread(0);
+                finished.WaitOne();
+                this.unmanagedWorker = null;
+            }
+            else if (maximumWorkerCount == 1)
+            {
+                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.)
+        /// <inheritdoc/>
+        public void DispatchWorkers(Action<int> 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)
+            {
+                workerType = WorkerType.Managed;
+                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);
+            }
+            this.unmanagedContext = null;
+            this.managedContext = null;
+        }
+
+
+        volatile bool disposed;
+
+        /// <summary>
+        /// Waits for all pending work to complete and then disposes all workers.
+        /// </summary>
+        public void Dispose()
+        {
+            if (!disposed)
+            {
+                disposed = true;
+                SignalThreads(threadCount);
+                foreach (var worker in workers)
+                {
+                    worker.Thread.Join();
+                    worker.Signal.Dispose();
+                }
+                WorkerPools.Dispose();
+            }
+        }
+
+    }
+
+}
diff --git a/BepuUtilities/Vector2Wide.cs b/BepuUtilities/Vector2Wide.cs
index a0de2b2c4..1f3a2c611 100644
--- a/BepuUtilities/Vector2Wide.cs
+++ b/BepuUtilities/Vector2Wide.cs
@@ -1,27 +1,83 @@
-using System;
-using System.Numerics;
+using System.Numerics;
 using System.Runtime.CompilerServices;
 
 namespace BepuUtilities
 {
+    /// <summary>
+    /// Two dimensional vector with <see cref="Vector{T}.Count"/> (with generic type argument of <see cref="float"/>) SIMD lanes.
+    /// </summary>
     public struct Vector2Wide
     {
+        /// <summary>
+        /// First component of the vector.
+        /// </summary>
         public Vector<float> X;
+        /// <summary>
+        /// Second component of the vector.
+        /// </summary>
         public Vector<float> Y;
 
+        /// <summary>
+        /// Performs a componentwise add between two vectors.
+        /// </summary>
+        /// <param name="a">First vector to add.</param>
+        /// <param name="b">Second vector to add.</param>
+        /// <param name="result">Sum of a and b.</param>
         [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;
         }
+        /// <summary>
+        /// Performs a componentwise add between two vectors.
+        /// </summary>
+        /// <param name="a">First vector to add.</param>
+        /// <param name="b">Second vector to add.</param>
+        /// <returns>Sum of a and b.</returns>
+        [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;
+        }
+        /// <summary>
+        /// Finds the result of adding a scalar to every component of a vector.
+        /// </summary>
+        /// <param name="v">Vector to add to.</param>
+        /// <param name="s">Scalar to add to every component of the vector.</param>
+        /// <returns>Vector with components equal to the input vector added to the input scalar.</returns>
+        [MethodImpl(MethodImplOptions.AggressiveInlining)]
+        public static Vector2Wide operator +(Vector2Wide v, Vector<float> s)
+        {
+            Vector2Wide result;
+            result.X = v.X + s;
+            result.Y = v.Y + s;
+            return result;
+        }
+        /// <summary>
+        /// Finds the result of adding a scalar to every component of a vector.
+        /// </summary>
+        /// <param name="v">Vector to add to.</param>
+        /// <param name="s">Scalar to add to every component of the vector.</param>
+        /// <returns>Vector with components equal to the input vector added to the input scalar.</returns>
+        [MethodImpl(MethodImplOptions.AggressiveInlining)]
+        public static Vector2Wide operator +(Vector<float> 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)
         {
             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<float> result)
@@ -36,6 +92,23 @@ public static void Scale(in Vector2Wide vector, in Vector<float> scalar, out Vec
             result.Y = vector.Y * scalar;
         }
 
+        [MethodImpl(MethodImplOptions.AggressiveInlining)]
+        public static Vector2Wide operator *(Vector2Wide vector, Vector<float> scalar)
+        {
+            Vector2Wide result;
+            result.X = vector.X * scalar;
+            result.Y = vector.Y * scalar;
+            return result;
+        }
+        [MethodImpl(MethodImplOptions.AggressiveInlining)]
+        public static Vector2Wide operator *(Vector<float> 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 +215,6 @@ public override string ToString()
         {
             return $"<{X}, {Y}>";
         }
-  
+
     }
 }
diff --git a/BepuUtilities/Vector3Wide.cs b/BepuUtilities/Vector3Wide.cs
index 30f530eb3..44f90efe9 100644
--- a/BepuUtilities/Vector3Wide.cs
+++ b/BepuUtilities/Vector3Wide.cs
@@ -1,15 +1,30 @@
-using System;
-using System.Numerics;
+using System.Numerics;
 using System.Runtime.CompilerServices;
 
 namespace BepuUtilities
 {
+    /// <summary>
+    /// Three dimensional vector with <see cref="Vector{T}.Count"/> (with generic type argument of <see cref="float"/>) SIMD lanes.
+    /// </summary>
     public struct Vector3Wide
     {
+        /// <summary>
+        /// First component of the vector.
+        /// </summary>
         public Vector<float> X;
+        /// <summary>
+        /// Second component of the vector.
+        /// </summary>
         public Vector<float> Y;
+        /// <summary>
+        /// Third component of the vector.
+        /// </summary>
         public Vector<float> Z;
 
+        /// <summary>
+        /// Creates a vector by populating each component with the given scalar.
+        /// </summary>
+        /// <param name="s">Scalar to copy into all lanes of the vector.</param>
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
         public Vector3Wide(ref Vector<float> s)
         {
@@ -18,6 +33,24 @@ public Vector3Wide(ref Vector<float> s)
             Z = s;
         }
 
+        /// <summary>
+        /// Creates a vector by populating each component with the given scalar.
+        /// </summary>
+        /// <param name="s">Scalar to copy into all lanes of the vector.</param>
+        [MethodImpl(MethodImplOptions.AggressiveInlining)]
+        public Vector3Wide(Vector<float> s)
+        {
+            X = s;
+            Y = s;
+            Z = s;
+        }
+
+        /// <summary>
+        /// Performs a componentwise add between two vectors.
+        /// </summary>
+        /// <param name="a">First vector to add.</param>
+        /// <param name="b">Second vector to add.</param>
+        /// <param name="result">Sum of a and b.</param>
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
         public static void Add(in Vector3Wide a, in Vector3Wide b, out Vector3Wide result)
         {
@@ -38,7 +71,58 @@ public static void Add(in Vector3Wide v, in Vector<float> s, out Vector3Wide res
             result.Y = v.Y + s;
             result.Z = v.Z + s;
         }
+        /// <summary>
+        /// Performs a componentwise add between two vectors.
+        /// </summary>
+        /// <param name="a">First vector to add.</param>
+        /// <param name="b">Second vector to add.</param>
+        /// <returns>Sum of a and b.</returns>
+        [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;
+        }
+        /// <summary>
+        /// Finds the result of adding a scalar to every component of a vector.
+        /// </summary>
+        /// <param name="v">Vector to add to.</param>
+        /// <param name="s">Scalar to add to every component of the vector.</param>
+        /// <returns>Vector with components equal to the input vector added to the input scalar.</returns>
+        [MethodImpl(MethodImplOptions.AggressiveInlining)]
+        public static Vector3Wide operator +(Vector3Wide v, Vector<float> s)
+        {
+            Vector3Wide result;
+            result.X = v.X + s;
+            result.Y = v.Y + s;
+            result.Z = v.Z + s;
+            return result;
+        }
+        /// <summary>
+        /// Finds the result of adding a scalar to every component of a vector.
+        /// </summary>
+        /// <param name="v">Vector to add to.</param>
+        /// <param name="s">Scalar to add to every component of the vector.</param>
+        /// <returns>Vector with components equal to the input vector added to the input scalar.</returns>
+        [MethodImpl(MethodImplOptions.AggressiveInlining)]
+        public static Vector3Wide operator +(Vector<float> s, Vector3Wide v)
+        {
+            Vector3Wide result;
+            result.X = v.X + s;
+            result.Y = v.Y + s;
+            result.Z = v.Z + s;
+            return result;
+        }
 
+        /// <summary>
+        /// Subtracts one vector from another.
+        /// </summary>
+        /// <param name="a">Vector to subtract from.</param>
+        /// <param name="b">Vector to subtract from the first vector.</param>
+        /// <param name="result">Vector with components equal the input scalar subtracted from the input vector.</param>
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
         public static void Subtract(in Vector3Wide a, in Vector3Wide b, out Vector3Wide result)
         {
@@ -60,6 +144,23 @@ public static void Subtract(in Vector3Wide v, in Vector<float> s, out Vector3Wid
             result.Y = v.Y - s;
             result.Z = v.Z - s;
         }
+
+        /// <summary>
+        /// Subtracts one vector from another.
+        /// </summary>
+        /// <param name="a">Vector to subtract from.</param>
+        /// <param name="b">Vector to subtract from the first vector.</param>
+        /// <returns>Vector with components equal the input scalar subtracted from the input vector.</returns>
+        [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;
+        }
+
         /// <summary>
         /// Finds the result of subtracting the components of a vector from a scalar.
         /// </summary>
@@ -74,12 +175,46 @@ public static void Subtract(in Vector<float> s, in Vector3Wide v, out Vector3Wid
             result.Z = s - v.Z;
         }
 
+        /// <summary>
+        /// Finds the result of subtracting the components of a vector from a scalar.
+        /// </summary>
+        /// <param name="a">Vector to subtract from the scalar.</param>
+        /// <param name="b">Scalar to subtract from.</param>
+        /// <returns>Vector with components equal the input vector subtracted from the input scalar.</returns>
+        [MethodImpl(MethodImplOptions.AggressiveInlining)]
+        public static Vector3Wide operator -(Vector3Wide a, Vector<float> b)
+        {
+            Vector3Wide result;
+            result.X = a.X - b;
+            result.Y = a.Y - b;
+            result.Z = a.Z - b;
+            return result;
+        }
+
+        /// <summary>
+        /// Computes the inner product between two vectors.
+        /// </summary>
+        /// <param name="a">First vector to dot.</param>
+        /// <param name="b">Second vector to dot.</param>
+        /// <param name="result">Dot product of a and b.</param>
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
         public static void Dot(in Vector3Wide a, in Vector3Wide b, out Vector<float> result)
         {
             result = a.X * b.X + a.Y * b.Y + a.Z * b.Z;
         }
 
+        /// <summary>
+        /// Computes the inner product between two vectors.
+        /// </summary>
+        /// <param name="a">First vector to dot.</param>
+        /// <param name="b">Second vector to dot.</param>
+        /// <returns>Dot product of a and b.</returns>
+        [MethodImpl(MethodImplOptions.AggressiveInlining)]
+        public static Vector<float> Dot(Vector3Wide a, Vector3Wide b)
+        {
+            return a.X * b.X + a.Y * b.Y + a.Z * b.Z;
+        }
+
         /// <summary>
         /// Computes the per-component minimum between a scalar value and the components of a vector.
         /// </summary>
@@ -106,6 +241,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);
         }
+
+        /// <summary>
+        /// Computes the per-component minimum between a scalar value and the components of a vector.
+        /// </summary>
+        /// <param name="s">Scalar to compare to each vector component.</param>
+        /// <param name="v">Vector whose components will be compared.</param>
+        /// <returns>Vector with components matching the smaller of the scalar value and the input vector.</returns>
+        [MethodImpl(MethodImplOptions.AggressiveInlining)]
+        public static Vector3Wide Min(Vector<float> 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;
+        }
+        /// <summary>
+        /// Computes the per-component minimum of two vectors.
+        /// </summary>
+        /// <param name="a">First vector whose components will be compared.</param>
+        /// <param name="b">Second vector whose components will be compared.</param>
+        /// <returns>Vector with components matching the smaller of the two input vectors.</returns>
+        [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;
+        }
+
         /// <summary>
         /// Computes the per-component maximum between a scalar value and the components of a vector.
         /// </summary>
@@ -119,6 +286,7 @@ public static void Max(in Vector<float> s, in Vector3Wide v, out Vector3Wide res
             result.Y = Vector.Max(s, v.Y);
             result.Z = Vector.Max(s, v.Z);
         }
+
         /// <summary>
         /// Computes the per-component maximum of two vectors.
         /// </summary>
@@ -133,7 +301,44 @@ public static void Max(in Vector3Wide a, in Vector3Wide b, out Vector3Wide resul
             result.Z = Vector.Max(a.Z, b.Z);
         }
 
+        /// <summary>
+        /// Computes the per-component maximum between a scalar value and the components of a vector.
+        /// </summary>
+        /// <param name="s">Scalar to compare to each vector component.</param>
+        /// <param name="v">Vector whose components will be compared.</param>
+        /// <returns>Vector with components matching the larger of the scalar value and the input vector.</returns>
+        [MethodImpl(MethodImplOptions.AggressiveInlining)]
+        public static Vector3Wide Max(Vector<float> 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;
+        }
+        /// <summary>
+        /// Computes the per-component maximum of two vectors.
+        /// </summary>
+        /// <param name="a">First vector whose components will be compared.</param>
+        /// <param name="b">Second vector whose components will be compared.</param>
+        /// <returns>Vector with components matching the larger of the two input vectors.</returns>
+        [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;
+        }
+
 
+        /// <summary>
+        /// Scales a vector by a scalar.
+        /// </summary>
+        /// <param name="vector">Vector to scale.</param>
+        /// <param name="scalar">Scalar to apply to the vector.</param>
+        /// <param name="result">Scaled result vector.</param>
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
         public static void Scale(in Vector3Wide vector, in Vector<float> scalar, out Vector3Wide result)
         {
@@ -142,6 +347,60 @@ public static void Scale(in Vector3Wide vector, in Vector<float> scalar, out Vec
             result.Z = vector.Z * scalar;
         }
 
+        /// <summary>
+        /// Divides each component of the vector by the scalar.
+        /// </summary>
+        /// <param name="vector">Vector to divide.</param>
+        /// <param name="scalar">Scalar to divide the vector by.</param>
+        /// <returns>Value of the vector divided by the scalar.</returns>
+        [MethodImpl(MethodImplOptions.AggressiveInlining)]
+        public static Vector3Wide operator /(Vector3Wide vector, Vector<float> scalar)
+        {
+            Vector3Wide result;
+            var inverse = Vector<float>.One / scalar;
+            result.X = vector.X * inverse;
+            result.Y = vector.Y * inverse;
+            result.Z = vector.Z * inverse;
+            return result;
+        }
+
+        /// <summary>
+        /// Scales a vector by a scalar.
+        /// </summary>
+        /// <param name="vector">Vector to scale.</param>
+        /// <param name="scalar">Scalar to apply to the vector.</param>
+        /// <returns>Scaled result vector.</returns>
+        [MethodImpl(MethodImplOptions.AggressiveInlining)]
+        public static Vector3Wide operator *(Vector3Wide vector, Vector<float> scalar)
+        {
+            Vector3Wide result;
+            result.X = vector.X * scalar;
+            result.Y = vector.Y * scalar;
+            result.Z = vector.Z * scalar;
+            return result;
+        }
+
+        /// <summary>
+        /// Scales a vector by a scalar.
+        /// </summary>
+        /// <param name="vector">Vector to scale.</param>
+        /// <param name="scalar">Scalar to apply to the vector.</param>
+        /// <returns>Scaled result vector.</returns>
+        [MethodImpl(MethodImplOptions.AggressiveInlining)]
+        public static Vector3Wide operator *(Vector<float> scalar, Vector3Wide vector)
+        {
+            Vector3Wide result;
+            result.X = vector.X * scalar;
+            result.Y = vector.Y * scalar;
+            result.Z = vector.Z * scalar;
+            return result;
+        }
+
+        /// <summary>
+        /// Computes the absolute value of a vector.
+        /// </summary>
+        /// <param name="vector">Vector to take the absolute value of.</param>
+        /// <param name="result">Absolute value of the input vector.</param>
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
         public static void Abs(in Vector3Wide vector, out Vector3Wide result)
         {
@@ -150,6 +409,26 @@ public static void Abs(in Vector3Wide vector, out Vector3Wide result)
             result.Z = Vector.Abs(vector.Z);
         }
 
+        /// <summary>
+        /// Computes the absolute value of a vector.
+        /// </summary>
+        /// <param name="vector">Vector to take the absolute value of.</param>
+        /// <returns>Absolute value of the input vector.</returns>
+        [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;
+        }
+
+        /// <summary>
+        /// Negates a vector.
+        /// </summary>
+        /// <param name="v">Vector to negate.</param>
+        /// <param name="result">Negated vector.</param>
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
         public static void Negate(in Vector3Wide v, out Vector3Wide result)
         {
@@ -158,6 +437,11 @@ public static void Negate(in Vector3Wide v, out Vector3Wide result)
             result.Z = -v.Z;
         }
 
+        /// <summary>
+        /// Negates a vector in place and returns a reference to it.
+        /// </summary>
+        /// <param name="v">Vector to negate.</param>
+        /// <returns>Reference to the input parameter, mutated.</returns>
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
         public static ref Vector3Wide Negate(ref Vector3Wide v)
         {
@@ -167,6 +451,26 @@ public static ref Vector3Wide Negate(ref Vector3Wide v)
             return ref v;
         }
 
+        /// <summary>
+        /// Negates a vector.
+        /// </summary>
+        /// <param name="v">Vector to negate.</param>
+        /// <returns>Negated vector.</returns>
+        [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;
+        }
+
+        /// <summary>
+        /// Conditionally negates lanes of the vector.
+        /// </summary>
+        /// <param name="shouldNegate">Mask indicating which lanes should be negated.</param>
+        /// <param name="v">Reference to the vector to be conditionally negated.</param>
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
         public static void ConditionallyNegate(in Vector<int> shouldNegate, ref Vector3Wide v)
         {
@@ -175,6 +479,12 @@ public static void ConditionallyNegate(in Vector<int> shouldNegate, ref Vector3W
             v.Z = Vector.ConditionalSelect(shouldNegate, -v.Z, v.Z);
         }
 
+        /// <summary>
+        /// Conditionally negates lanes of the vector.
+        /// </summary>
+        /// <param name="shouldNegate">Mask indicating which lanes should be negated.</param>
+        /// <param name="v">Vector to be conditionally negated.</param>
+        /// <param name="negated">Conditionally negated result.</param>
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
         public static void ConditionallyNegate(in Vector<int> shouldNegate, in Vector3Wide v, out Vector3Wide negated)
         {
@@ -183,6 +493,28 @@ public static void ConditionallyNegate(in Vector<int> shouldNegate, in Vector3Wi
             negated.Z = Vector.ConditionalSelect(shouldNegate, -v.Z, v.Z);
         }
 
+        /// <summary>
+        /// Conditionally negates lanes of the vector.
+        /// </summary>
+        /// <param name="shouldNegate">Mask indicating which lanes should be negated.</param>
+        /// <param name="v">Vector to be conditionally negated.</param>
+        /// <returns>Conditionally negated vector.</returns>
+        [MethodImpl(MethodImplOptions.AggressiveInlining)]
+        public static Vector3Wide ConditionallyNegate(Vector<int> 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;
+        }
+
+        /// <summary>
+        /// Computes the cross product between two vectors, assuming that the vector references are not aliased.
+        /// </summary>
+        /// <param name="a">First vector to cross.</param>
+        /// <param name="b">Second vector to cross.</param>
+        /// <param name="result">Result of the cross product.</param>
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
         public static void CrossWithoutOverlap(in Vector3Wide a, in Vector3Wide b, out Vector3Wide result)
         {
@@ -191,6 +523,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;
         }
+
+        /// <summary>
+        /// Computes the cross product between two vectors.
+        /// </summary>
+        /// <param name="a">First vector to cross.</param>
+        /// <param name="b">Second vector to cross.</param>
+        /// <param name="result">Result of the cross product.</param>
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
         public static void Cross(in Vector3Wide a, in Vector3Wide b, out Vector3Wide result)
         {
@@ -198,18 +537,92 @@ public static void Cross(in Vector3Wide a, in Vector3Wide b, out Vector3Wide res
             result = temp;
         }
 
+        /// <summary>
+        /// Computes the cross product between two vectors.
+        /// </summary>
+        /// <param name="a">First vector to cross.</param>
+        /// <param name="b">Second vector to cross.</param>
+        /// <returns>Result of the cross product.</returns>
+        [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;
+        }
 
+        /// <summary>
+        /// Computes the squared length of a vector.
+        /// </summary>
+        /// <param name="v">Vector to compute the squared length of.</param>
+        /// <param name="lengthSquared">Squared length of the input vector.</param>
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
         public static void LengthSquared(in Vector3Wide v, out Vector<float> lengthSquared)
         {
             lengthSquared = v.X * v.X + v.Y * v.Y + v.Z * v.Z;
         }
+
+        /// <summary>
+        /// Computes the length of a vector.
+        /// </summary>
+        /// <param name="v">Vector to compute the length of.</param>
+        /// <param name="length">Length of the input vector.</param>
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
         public static void Length(in Vector3Wide v, out Vector<float> length)
         {
             length = Vector.SquareRoot(v.X * v.X + v.Y * v.Y + v.Z * v.Z);
         }
 
+        /// <summary>
+        /// Computes the squared length of a vector.
+        /// </summary>
+        /// <param name="v">Vector to compute the squared length of.</param>
+        /// <returns>Squared length of the input vector.</returns>
+        [MethodImpl(MethodImplOptions.AggressiveInlining)]
+        public static Vector<float> LengthSquared(Vector3Wide v)
+        {
+            return v.X * v.X + v.Y * v.Y + v.Z * v.Z;
+        }
+
+        /// <summary>
+        /// Computes the length of a vector.
+        /// </summary>
+        /// <param name="v">Vector to compute the length of.</param>
+        /// <returns>Length of the input vector.</returns>
+        [MethodImpl(MethodImplOptions.AggressiveInlining)]
+        public static Vector<float> Length(Vector3Wide v)
+        {
+            return Vector.SquareRoot(v.X * v.X + v.Y * v.Y + v.Z * v.Z);
+        }
+
+        /// <summary>
+        /// Computes the squared length of the vector.
+        /// </summary>
+        /// <returns>Squared length of this vector.</returns>
+        [MethodImpl(MethodImplOptions.AggressiveInlining)]
+        public Vector<float> LengthSquared()
+        {
+            return X * X + Y * Y + Z * Z;
+        }
+
+        /// <summary>
+        /// Computes the length of the vector.
+        /// </summary>
+        /// <returns>Length of this vector.</returns>
+        [MethodImpl(MethodImplOptions.AggressiveInlining)]
+        public Vector<float> Length()
+        {
+            return Vector.SquareRoot(X * X + Y * Y + Z * Z);
+        }
+
+        /// <summary>
+        /// Computes the distance between two vectors.
+        /// </summary>
+        /// <param name="a">First vector in the pair.</param>
+        /// <param name="b">Second vector in the pair.</param>
+        /// <param name="distance">Distance between a and b.</param>
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
         public static void Distance(in Vector3Wide a, in Vector3Wide b, out Vector<float> distance)
         {
@@ -219,6 +632,12 @@ public static void Distance(in Vector3Wide a, in Vector3Wide b, out Vector<float
             distance = Vector.SquareRoot(x * x + y * y + z * z);
         }
 
+        /// <summary>
+        /// Computes the squared distance between two vectors.
+        /// </summary>
+        /// <param name="a">First vector in the pair.</param>
+        /// <param name="b">Second vector in the pair.</param>
+        /// <param name="distanceSquared">Squared distance between a and b.</param>
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
         public static void DistanceSquared(in Vector3Wide a, in Vector3Wide b, out Vector<float> distanceSquared)
         {
@@ -228,6 +647,43 @@ public static void DistanceSquared(in Vector3Wide a, in Vector3Wide b, out Vecto
             distanceSquared = x * x + y * y + z * z;
         }
 
+        /// <summary>
+        /// Computes the distance between two vectors.
+        /// </summary>
+        /// <param name="a">First vector in the pair.</param>
+        /// <param name="b">Second vector in the pair.</param>
+        /// <returns>Distance between a and b.</returns>
+        [MethodImpl(MethodImplOptions.AggressiveInlining)]
+        public static Vector<float> 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);
+        }
+
+        /// <summary>
+        /// Computes the squared distance between two vectors.
+        /// </summary>
+        /// <param name="a">First vector in the pair.</param>
+        /// <param name="b">Second vector in the pair.</param>
+        /// <returns>Squared distance between a and b.</returns>
+        [MethodImpl(MethodImplOptions.AggressiveInlining)]
+        public static Vector<float> 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.
+
+        /// <summary>
+        /// Computes a unit length vector pointing in the same direction as the input.
+        /// </summary>
+        /// <param name="v">Vector to normalize.</param>
+        /// <param name="result">Vector pointing in the same direction as the input, but with unit length.</param>
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
         public static void Normalize(in Vector3Wide v, out Vector3Wide result)
         {
@@ -236,6 +692,26 @@ public static void Normalize(in Vector3Wide v, out Vector3Wide result)
             Scale(v, scale, out result);
         }
 
+        /// <summary>
+        /// Computes a unit length vector pointing in the same direction as the input.
+        /// </summary>
+        /// <param name="v">Vector to normalize.</param>
+        /// <returns>Vector pointing in the same direction as the input, but with unit length.</returns>
+        [MethodImpl(MethodImplOptions.AggressiveInlining)]
+        public static Vector3Wide Normalize(Vector3Wide v)
+        {
+            Length(v, out var length);
+            var scale = Vector<float>.One / length;
+            return v * scale;
+        }
+
+        /// <summary>
+        /// Selects the left or right input for each lane depending on a mask.
+        /// </summary>
+        /// <param name="condition">Mask to use to decide between the left and right value for each lane..</param>
+        /// <param name="left">Value to choose if the condition mask is set.</param>
+        /// <param name="right">Value to choose if the condition mask is unset.</param>
+        /// <param name="result">Blended result.</param>
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
         public static void ConditionalSelect(in Vector<int> condition, in Vector3Wide left, in Vector3Wide right, out Vector3Wide result)
         {
@@ -244,6 +720,23 @@ public static void ConditionalSelect(in Vector<int> condition, in Vector3Wide le
             result.Z = Vector.ConditionalSelect(condition, left.Z, right.Z);
         }
 
+        /// <summary>
+        /// Selects the left or right input for each lane depending on a mask.
+        /// </summary>
+        /// <param name="condition">Mask to use to decide between the left and right value for each lane..</param>
+        /// <param name="left">Value to choose if the condition mask is set.</param>
+        /// <param name="right">Value to choose if the condition mask is unset.</param>
+        /// <returns>Blended result.</returns>
+        [MethodImpl(MethodImplOptions.AggressiveInlining)]
+        public static Vector3Wide ConditionalSelect(Vector<int> 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;
+        }
+
         /// <summary>
         /// Multiplies the components of one vector with another.
         /// </summary>
@@ -258,6 +751,23 @@ public static void Multiply(in Vector3Wide a, in Vector3Wide b, out Vector3Wide
             result.Z = a.Z * b.Z;
         }
 
+        /// <summary>
+        /// Multiplies the components of one vector with another.
+        /// </summary>
+        /// <param name="a">First vector to multiply.</param>
+        /// <param name="b">Second vector to multiply.</param>
+        /// <returns>Result of the multiplication.</returns>
+        [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.
         /// <summary>
         /// Pulls one lane out of the wide representation.
         /// </summary>
@@ -271,7 +781,6 @@ public static void ReadSlot(ref Vector3Wide wide, int slotIndex, out Vector3 nar
             ReadFirst(offset, out narrow);
         }
 
-
         /// <summary>
         /// Pulls one lane out of the wide representation.
         /// </summary>
@@ -291,7 +800,7 @@ public static void ReadFirst(in Vector3Wide source, out Vector3 target)
         /// <param name="source">Vector to copy values from.</param>
         /// <param name="targetSlot">Wide vectorto place values into.</param>
         [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;
@@ -305,9 +814,9 @@ public static void WriteFirst(in Vector3 source, ref Vector3Wide targetSlot)
         /// <param name="slotIndex">Index of the slot to write into.</param>
         /// <param name="target">Bundle to write the value into.</param>
         [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)); 
+            WriteFirst(source, ref GatherScatter.GetOffsetInstance(ref target, slotIndex));
         }
 
         /// <summary>
@@ -316,11 +825,26 @@ public static void WriteSlot(in Vector3 source, int slotIndex, ref Vector3Wide t
         /// <param name="source">Source value to write to every bundle slot.</param>
         /// <param name="broadcasted">Bundle containing the source's components in every slot.</param>
         [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<float>(source.X);
+            broadcasted.Y = new Vector<float>(source.Y);
+            broadcasted.Z = new Vector<float>(source.Z);
+        }
+
+        /// <summary>
+        /// Expands each scalar value to every slot of the bundle.
+        /// </summary>
+        /// <param name="source">Source value to write to every bundle slot.</param>
+        /// <returns>Bundle containing the source's components in every slot.</returns>
+        [MethodImpl(MethodImplOptions.AggressiveInlining)]
+        public static Vector3Wide Broadcast(Vector3 source)
         {
+            Vector3Wide broadcasted;
             broadcasted.X = new Vector<float>(source.X);
             broadcasted.Y = new Vector<float>(source.Y);
             broadcasted.Z = new Vector<float>(source.Z);
+            return broadcasted;
         }
 
         /// <summary>
@@ -337,6 +861,23 @@ public static void Rebroadcast(in Vector3Wide source, int slotIndex, out Vector3
             broadcasted.Z = new Vector<float>(source.Z[slotIndex]);
         }
 
+
+        /// <summary>
+        /// Takes a slot from the source vector and broadcasts it into all slots of the target vector.
+        /// </summary>
+        /// <param name="source">Vector to pull values from.</param>
+        /// <param name="slotIndex">Slot in the source vectors to pull values from.</param>
+        /// <returns>Target vector to be filled with the selected data.</returns>
+        [MethodImpl(MethodImplOptions.AggressiveInlining)]
+        public static Vector3Wide Rebroadcast(Vector3Wide source, int slotIndex)
+        {
+            Vector3Wide broadcasted;
+            broadcasted.X = new Vector<float>(source.X[slotIndex]);
+            broadcasted.Y = new Vector<float>(source.Y[slotIndex]);
+            broadcasted.Z = new Vector<float>(source.Z[slotIndex]);
+            return broadcasted;
+        }
+
         /// <summary>
         /// Takes a slot from the source vector and places it into a slot of the target.
         /// </summary>
diff --git a/BepuUtilities/Vector4Wide.cs b/BepuUtilities/Vector4Wide.cs
index 0f37009bc..d6825913d 100644
--- a/BepuUtilities/Vector4Wide.cs
+++ b/BepuUtilities/Vector4Wide.cs
@@ -1,9 +1,11 @@
-using System;
-using System.Numerics;
+using System.Numerics;
 using System.Runtime.CompilerServices;
 
 namespace BepuUtilities
 {
+    /// <summary>
+    /// Four dimensional vector with <see cref="Vector{T}.Count"/> (with generic type argument of <see cref="float"/>) SIMD lanes.
+    /// </summary>
     public struct Vector4Wide
     {
         public Vector<float> X;
@@ -20,14 +22,83 @@ public static void Broadcast(in Vector4 source, out Vector4Wide broadcasted)
             broadcasted.W = new Vector<float>(source.W);
         }
 
+
+        /// <summary>
+        /// Performs a componentwise add between two vectors.
+        /// </summary>
+        /// <param name="a">First vector to add.</param>
+        /// <param name="b">Second vector to add.</param>
+        /// <param name="result">Sum of a and b.</param>
         [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;
         }
+        /// <summary>
+        /// Finds the result of adding a scalar to every component of a vector.
+        /// </summary>
+        /// <param name="v">Vector to add to.</param>
+        /// <param name="s">Scalar to add to every component of the vector.</param>
+        /// <param name="result">Vector with components equal to the input vector added to the input scalar.</param>
+        [MethodImpl(MethodImplOptions.AggressiveInlining)]
+        public static void Add(Vector4Wide v, Vector<float> s, out Vector4Wide result)
+        {
+            result.X = v.X + s;
+            result.Y = v.Y + s;
+            result.Z = v.Z + s;
+            result.W = v.W + s;
+        }
+        /// <summary>
+        /// Performs a componentwise add between two vectors.
+        /// </summary>
+        /// <param name="a">First vector to add.</param>
+        /// <param name="b">Second vector to add.</param>
+        /// <returns>Sum of a and b.</returns>
+        [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;
+        }
+        /// <summary>
+        /// Finds the result of adding a scalar to every component of a vector.
+        /// </summary>
+        /// <param name="v">Vector to add to.</param>
+        /// <param name="s">Scalar to add to every component of the vector.</param>
+        /// <returns>Vector with components equal to the input vector added to the input scalar.</returns>
+        [MethodImpl(MethodImplOptions.AggressiveInlining)]
+        public static Vector4Wide operator +(Vector4Wide v, Vector<float> 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;
+        }
+        /// <summary>
+        /// Finds the result of adding a scalar to every component of a vector.
+        /// </summary>
+        /// <param name="v">Vector to add to.</param>
+        /// <param name="s">Scalar to add to every component of the vector.</param>
+        /// <returns>Vector with components equal to the input vector added to the input scalar.</returns>
+        [MethodImpl(MethodImplOptions.AggressiveInlining)]
+        public static Vector4Wide operator +(Vector<float> 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)
@@ -90,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)]
@@ -108,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;
         }
 
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 @@
-<Project Sdk="Microsoft.NET.Sdk">
+<Project Sdk="Microsoft.NET.Sdk">
 
   <PropertyGroup>
     <OutputType>Exe</OutputType>
-    <TargetFramework>netcoreapp2.0</TargetFramework>
+    <TargetFramework>net6.0</TargetFramework>
     <AssemblyName>BepuUtilitiesTests</AssemblyName>
     <RootNamespace>BepuUtilitiesTests</RootNamespace>
+    <!--<TieredCompilation>false</TieredCompilation>-->
   </PropertyGroup>
 
   <PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Release|AnyCPU'">
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<int, float> function, int benchmarkIterations = 100000000)
+        public static void Test(string testName, Func<int, float> 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..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();
@@ -18,6 +17,8 @@ static void Main(string[] args)
             Matrix3x3Tests.Test();
             Console.WriteLine();
             Matrix4x4Tests.Test();
+            Console.WriteLine();
+            SymmetricTests.Test();
 
         }
 
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}");
-            }
-        }
-    }
-}
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);
+
+        }
+    }
+}
diff --git a/CommonSettings.props b/CommonSettings.props
new file mode 100644
index 000000000..88dfbf27f
--- /dev/null
+++ b/CommonSettings.props
@@ -0,0 +1,35 @@
+<Project>
+    <PropertyGroup>
+        <TargetFramework>net8.0</TargetFramework>
+        <Version>2.5.0-beta.24</Version>
+        <Company>Bepu Entertainment LLC</Company>
+        <Authors>Ross Nordby</Authors>
+        <Copyright>© Bepu Entertainment LLC</Copyright>
+        <PackageProjectUrl>https://github.com/bepu/bepuphysics2</PackageProjectUrl>
+        <RepositoryUrl>https://github.com/bepu/bepuphysics2</RepositoryUrl>
+        <PackageLicenseExpression>Apache-2.0</PackageLicenseExpression>
+        <LangVersion>latest</LangVersion>
+        <PackageIcon>bepuphysicslogo256.png</PackageIcon>
+        <AllowUnsafeBlocks>True</AllowUnsafeBlocks>
+        <DebugSymbols>true</DebugSymbols>
+        <!--<DebugType>full</DebugType>-->
+        <SignAssembly>true</SignAssembly>
+        <DelaySign>false</DelaySign>
+        <GenerateDocumentationFile>true</GenerateDocumentationFile>
+        <!--<TieredCompilation>false</TieredCompilation>-->
+        <IncludeSymbols>true</IncludeSymbols>
+        <SymbolPackageFormat>snupkg</SymbolPackageFormat>
+    </PropertyGroup>
+
+    <ItemGroup>
+        <None Include="..\Documentation\images\bepuphysicslogo256.png">
+            <Pack>True</Pack>
+            <PackagePath></PackagePath>
+        </None>
+    </ItemGroup>
+
+    <!-- Needed for Source Link package debugging, see https://github.com/dotnet/sourcelink -->
+    <ItemGroup>
+        <PackageReference Include="Microsoft.SourceLink.GitHub" Version="1.1.1" PrivateAssets="All" />
+    </ItemGroup>
+</Project>
\ No newline at end of file
diff --git a/DemoBenchmarks/BenchmarkHelper.cs b/DemoBenchmarks/BenchmarkHelper.cs
new file mode 100644
index 000000000..322d277c1
--- /dev/null
+++ b/DemoBenchmarks/BenchmarkHelper.cs
@@ -0,0 +1,115 @@
+using BepuPhysics;
+using BepuPhysics.Collidables;
+using BepuUtilities;
+using BepuUtilities.Memory;
+using System.Numerics;
+
+namespace DemoBenchmarks;
+
+public static class BenchmarkHelper
+{
+    public static Vector3 CreateRandomPosition(Random random, BoundingBox positionBounds)
+    {
+        var span = positionBounds.Max - positionBounds.Min;
+        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 > 1e-10f)
+            axis /= length;
+        else
+            axis = new Vector3(0, 1, 0);
+        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;
+    }
+
+    public static void CreateDeformedPlane(int width, int height, Func<int, int, Vector3> deformer, Vector3 scaling, BufferPool pool, out Mesh mesh)
+    {
+        pool.Take<Vector3>(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<Triangle>(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<Vector3>(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/CollisionBatcherTaskBenchmarks.cs b/DemoBenchmarks/CollisionBatcherTaskBenchmarks.cs
new file mode 100644
index 000000000..903a2ab32
--- /dev/null
+++ b/DemoBenchmarks/CollisionBatcherTaskBenchmarks.cs
@@ -0,0 +1,156 @@
+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;
+
+/// <summary>
+/// Evaluates performance of all collision tasks together in a collision batcher.
+/// </summary>
+public class CollisionBatcherTaskBenchmarks
+{
+    const int pairCount = 10000;
+    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<Pair> pairs;
+    CollisionTaskRegistry taskRegistry;
+    Shapes shapes;
+
+    [GlobalSetup]
+    public unsafe void Setup()
+    {
+        pool = new BufferPool();
+        pool.Take(pairCount, out pairs);
+        taskRegistry = DefaultTypes.CreateDefaultCollisionTaskRegistry();
+        shapes = new Shapes(pool, 1);
+
+        Random random = new(5);
+        CreateShapes(random, pool, shapes);
+
+        Span<float> shapeRelativeProbabilities = stackalloc float[9];
+        shapeRelativeProbabilities[0] = 1;
+        shapeRelativeProbabilities[1] = 1;
+        shapeRelativeProbabilities[2] = 1;
+        shapeRelativeProbabilities[3] = 1;
+        shapeRelativeProbabilities[4] = 0.4f;
+        shapeRelativeProbabilities[5] = 0.35f;
+
+        shapeRelativeProbabilities[6] = 0.1f;
+        shapeRelativeProbabilities[7] = 0.1f;
+        shapeRelativeProbabilities[8] = 0.1f;
+
+        var sum = 0f;
+        Span<float> 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<float> 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)
+            };
+        }
+    }
+
+    [GlobalCleanup]
+    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<TManifold>(int pairId, ref TManifold manifold) where TManifold : unmanaged, IContactManifold<TManifold>
+        {
+            var count = manifold.Count;
+            for (int i = 0; i < count; ++i)
+            {
+                ResultSum += TManifold.GetNormalReference(ref manifold, i) + TManifold.GetOffsetReference(ref manifold, i) + new Vector3(TManifold.GetDepthReference(ref manifold, i));
+            }
+        }
+    }
+
+    [Benchmark]
+    public unsafe Vector3 CollisionBatcherTasksBenchmark()
+    {
+        CollisionBatcher<Callbacks> 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/ConvexCollisionTesterBenchmarks.cs b/DemoBenchmarks/ConvexCollisionTesterBenchmarks.cs
new file mode 100644
index 000000000..d8f34defe
--- /dev/null
+++ b/DemoBenchmarks/ConvexCollisionTesterBenchmarks.cs
@@ -0,0 +1,244 @@
+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;
+
+/// <summary>
+/// Evaluates performance of all multilane convex collision testers.
+/// </summary>
+/// <remarks>
+/// Note that all of these collision testers operate across <see cref="Vector{}.Count"/> lanes simultaneously where T is of type <see cref="float"/>.
+/// <para>The number of bundles being executed does not change if <see cref="Vector{}.Count"/> changes; if larger bundles are allowed, then more lanes end up getting solved.</para>
+/// </remarks>
+public class ConvexCollisionTesterBenchmarks
+{
+    const int iterationCount = 100;
+    BufferPool pool;
+    Buffer<Vector3Wide> offsetsB;
+    Buffer<Vector<float>> speculativeMargins;
+    Buffer<QuaternionWide> orientationsA;
+    Buffer<QuaternionWide> orientationsB;
+
+    [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<float> bundleMargins = stackalloc float[Vector<float>.Count];
+        for (int i = 0; i < iterationCount; ++i)
+        {
+            Vector3Wide offsetB = default;
+            QuaternionWide orientationA = default;
+            QuaternionWide orientationB = default;
+            for (int j = 0; j < Vector<float>.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<float>(bundleMargins);
+        }
+
+        //Convex hulls are a little heavier to set up than the other shapes, so precreate one.
+        const int pointCount = 50;
+        pool.Take<Vector3>(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<ConvexHull>(Vector<float>.Count, out var hullBundle);
+        var hull = default(ConvexHullWide);
+        hull.Initialize(hullBundle.As<byte>());
+        hull.Broadcast(narrowHull);
+        Hull = hull;
+    }
+
+    [GlobalCleanup]
+    public void Cleanup()
+    {
+        //All outstanding allocations poof when the pool is cleared.
+        pool.Clear();
+    }
+
+    SphereWide Sphere => new() { Radius = new Vector<float>(1f) };
+    CapsuleWide Capsule => new() { Radius = new Vector<float>(0.5f), HalfLength = new Vector<float>(1f) };
+    BoxWide Box => new() { HalfWidth = new Vector<float>(1f), HalfHeight = new Vector<float>(1f), HalfLength = new Vector<float>(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<float>(1f), HalfLength = new Vector<float>(1f) };
+    ConvexHullWide Hull { get; set; }
+
+
+    Vector<float> TestOrientationless1Contact<TTester, TShapeA, TShapeB>(TShapeA a, TShapeB b) where TTester : unmanaged, IPairTester<TShapeA, TShapeB, Convex1ContactManifoldWide>
+    {
+        Vector<float> testSum = Vector<float>.Zero;
+        for (int i = 0; i < iterationCount; ++i)
+        {
+            TTester.Test(ref a, ref b, ref speculativeMargins[i], ref offsetsB[i], Vector<float>.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<float> TestOrientationB1Contact<TTester, TShapeA, TShapeB>(TShapeA a, TShapeB b) where TTester : unmanaged, IPairTester<TShapeA, TShapeB, Convex1ContactManifoldWide>
+    {
+        Vector<float> testSum = Vector<float>.Zero;
+        for (int i = 0; i < iterationCount; ++i)
+        {
+            TTester.Test(ref a, ref b, ref speculativeMargins[i], ref offsetsB[i], ref orientationsB[i], Vector<float>.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<float> Test2Contact<TTester, TShapeA, TShapeB>(TShapeA a, TShapeB b) where TTester : unmanaged, IPairTester<TShapeA, TShapeB, Convex2ContactManifoldWide>
+    {
+        Vector<float> testSum = Vector<float>.Zero;
+        for (int i = 0; i < iterationCount; ++i)
+        {
+            TTester.Test(ref a, ref b, ref speculativeMargins[i], ref offsetsB[i], ref orientationsA[i], ref orientationsB[i], Vector<float>.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<float> Test4Contact<TTester, TShapeA, TShapeB>(TShapeA a, TShapeB b) where TTester : unmanaged, IPairTester<TShapeA, TShapeB, Convex4ContactManifoldWide>
+    {
+        Vector<float> testSum = Vector<float>.Zero;
+        for (int i = 0; i < iterationCount; ++i)
+        {
+            TTester.Test(ref a, ref b, ref speculativeMargins[i], ref offsetsB[i], ref orientationsA[i], ref orientationsB[i], Vector<float>.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<float> SpherePairTester()
+    {
+        return TestOrientationless1Contact<SpherePairTester, SphereWide, SphereWide>(Sphere, Sphere);
+    }
+    [Benchmark]
+    public Vector<float> SphereCapsuleTester()
+    {
+        return TestOrientationB1Contact<SphereCapsuleTester, SphereWide, CapsuleWide>(Sphere, Capsule);
+    }
+    [Benchmark]
+    public Vector<float> SphereBoxTester()
+    {
+        return TestOrientationB1Contact<SphereBoxTester, SphereWide, BoxWide>(Sphere, Box);
+    }
+    [Benchmark]
+    public Vector<float> SphereTriangleTester()
+    {
+        return TestOrientationB1Contact<SphereTriangleTester, SphereWide, TriangleWide>(Sphere, Triangle);
+    }
+    [Benchmark]
+    public Vector<float> SphereCylinderTester()
+    {
+        return TestOrientationB1Contact<SphereCylinderTester, SphereWide, CylinderWide>(Sphere, Cylinder);
+    }
+    [Benchmark]
+    public Vector<float> SphereConvexHullTester()
+    {
+        return TestOrientationB1Contact<SphereConvexHullTester, SphereWide, ConvexHullWide>(Sphere, Hull);
+    }
+    [Benchmark]
+    public Vector<float> CapsulePairTester()
+    {
+        return Test2Contact<CapsulePairTester, CapsuleWide, CapsuleWide>(Capsule, Capsule);
+    }
+    [Benchmark]
+    public Vector<float> CapsuleBoxTester()
+    {
+        return Test2Contact<CapsuleBoxTester, CapsuleWide, BoxWide>(Capsule, Box);
+    }
+    [Benchmark]
+    public Vector<float> CapsuleTriangleTester()
+    {
+        return Test2Contact<CapsuleTriangleTester, CapsuleWide, TriangleWide>(Capsule, Triangle);
+    }
+    [Benchmark]
+    public Vector<float> CapsuleCylinderTester()
+    {
+        return Test2Contact<CapsuleCylinderTester, CapsuleWide, CylinderWide>(Capsule, Cylinder);
+    }
+    [Benchmark]
+    public Vector<float> CapsuleConvexHullTester()
+    {
+        return Test2Contact<CapsuleConvexHullTester, CapsuleWide, ConvexHullWide>(Capsule, Hull);
+    }
+    [Benchmark]
+    public Vector<float> BoxPairTester()
+    {
+        return Test4Contact<BoxPairTester, BoxWide, BoxWide>(Box, Box);
+    }
+    [Benchmark]
+    public Vector<float> BoxTriangleTester()
+    {
+        return Test4Contact<BoxTriangleTester, BoxWide, TriangleWide>(Box, Triangle);
+    }
+    [Benchmark]
+    public Vector<float> BoxCylinderTester()
+    {
+        return Test4Contact<BoxCylinderTester, BoxWide, CylinderWide>(Box, Cylinder);
+    }
+    [Benchmark]
+    public Vector<float> BoxConvexHullTester()
+    {
+        return Test4Contact<BoxConvexHullTester, BoxWide, ConvexHullWide>(Box, Hull);
+    }
+    [Benchmark]
+    public Vector<float> TrianglePairTester()
+    {
+        return Test4Contact<TrianglePairTester, TriangleWide, TriangleWide>(Triangle, Triangle);
+    }
+    [Benchmark]
+    public Vector<float> TriangleCylinderTester()
+    {
+        return Test4Contact<TriangleCylinderTester, TriangleWide, CylinderWide>(Triangle, Cylinder);
+    }
+    [Benchmark]
+    public Vector<float> TriangleConvexHullTester()
+    {
+        return Test4Contact<TriangleConvexHullTester, TriangleWide, ConvexHullWide>(Triangle, Hull);
+    }
+    [Benchmark]
+    public Vector<float> CylinderPairTester()
+    {
+        return Test4Contact<CylinderPairTester, CylinderWide, CylinderWide>(Cylinder, Cylinder);
+    }
+    [Benchmark]
+    public Vector<float> CylinderConvexHullTester()
+    {
+        return Test4Contact<CylinderConvexHullTester, CylinderWide, ConvexHullWide>(Cylinder, Hull);
+    }
+    [Benchmark]
+    public Vector<float> ConvexHullPairTester()
+    {
+        return Test4Contact<ConvexHullPairTester, ConvexHullWide, ConvexHullWide>(Hull, Hull);
+    }
+}
diff --git a/DemoBenchmarks/DemoBenchmarks.csproj b/DemoBenchmarks/DemoBenchmarks.csproj
new file mode 100644
index 000000000..263d7f079
--- /dev/null
+++ b/DemoBenchmarks/DemoBenchmarks.csproj
@@ -0,0 +1,21 @@
+<Project Sdk="Microsoft.NET.Sdk">
+
+  <PropertyGroup>
+    <OutputType>Exe</OutputType>
+    <TargetFramework>net8.0</TargetFramework>
+    <ImplicitUsings>enable</ImplicitUsings>
+    <AllowUnsafeBlocks>true</AllowUnsafeBlocks>
+    <StartArguments>-f *CollisionBatcherTaskBenchmarks.* *GroupedCollisionTesterBenchmarks.* *GatherScatterBenchmarks.* *OneBodyConstraintBenchmarks.* *TwoBodyConstraintBenchmarks.* *ThreeBodyConstraintBenchmarks.* *FourBodyConstraintBenchmarks.* *SweepBenchmarks.* *ShapeRayBenchmarks.* *ShapePileBenchmark.* *RagdollTubeBenchmark.* --join</StartArguments>
+    <!--<StartArguments>-f *CollisionBatcherTaskBenchmarks.* *ConvexCollisionTesterBenchmarks.* *GatherScatterBenchmarks.* *OneBodyConstraintBenchmarks.* *OneBodyConstraintBenchmarksDeep.* *TwoBodyConstraintBenchmarks.* *TwoBodyConstraintBenchmarksDeep.* *ThreeBodyConstraintBenchmarks.* *FourBodyConstraintBenchmarks.* *SweepBenchmarks.* *SweepBenchmarksDeep.* *ShapeRayBenchmarksDeep.* *ShapePileBenchmark.* *RagdollTubeBenchmark.*</StartArguments>-->
+  </PropertyGroup>
+
+  <ItemGroup>
+    <PackageReference Include="BenchmarkDotNet" Version="0.13.2" />
+  </ItemGroup>
+
+  <ItemGroup>
+    <ProjectReference Include="..\BepuPhysics\BepuPhysics.csproj" />
+    <ProjectReference Include="..\BepuUtilities\BepuUtilities.csproj" />
+  </ItemGroup>
+
+</Project>
diff --git a/DemoBenchmarks/FourBodyConstraintBenchmarks.cs b/DemoBenchmarks/FourBodyConstraintBenchmarks.cs
new file mode 100644
index 000000000..1973edea0
--- /dev/null
+++ b/DemoBenchmarks/FourBodyConstraintBenchmarks.cs
@@ -0,0 +1,63 @@
+using BenchmarkDotNet.Attributes;
+using BepuPhysics;
+using BepuPhysics.Constraints;
+using BepuPhysics.Constraints.Contact;
+using BepuUtilities;
+using System.Numerics;
+
+namespace DemoBenchmarks;
+
+/// <summary>
+/// Evaluates performance of all four body constraints.
+/// </summary>
+/// <remarks>
+/// Note that all constraints operate across <see cref="Vector{}.Count"/> lanes simultaneously where T is of type <see cref="float"/>.
+/// <para>The number of bundles being executed does not change if <see cref="Vector{}.Count"/> changes; if larger bundles are allowed, then more lanes end up getting solved.</para>
+/// </remarks>
+public class FourBodyConstraintBenchmarks
+{
+    static (BodyVelocityWide, BodyVelocityWide, BodyVelocityWide, BodyVelocityWide) BenchmarkFourBodyConstraint<TConstraintFunctions, TPrestep, TAccumulatedImpulse>(
+        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<TPrestep, TAccumulatedImpulse> where TPrestep : unmanaged where TAccumulatedImpulse : unmanaged
+    {
+        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 = 1000;
+        const float inverseDt = 60f;
+        const float dt = 1f / inverseDt;
+        for (int i = 0; i < iterations; ++i)
+        {
+            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);
+            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);
+    }
+
+    //Not a lot of these yet!
+    [Benchmark]
+    public (BodyVelocityWide, BodyVelocityWide, BodyVelocityWide, BodyVelocityWide) VolumeConstraint()
+    {
+        var prestep = new VolumeConstraintPrestepData
+        {
+            TargetScaledVolume = Vector<float>.One,
+            SpringSettings = new() { TwiceDampingRatio = new Vector<float>(2), AngularFrequency = new Vector<float>(20 * MathF.PI) }
+        };
+
+        QuaternionWide.Broadcast(Quaternion.Identity, out var orientation);
+        var inertia = new BodyInertiaWide { InverseInertiaTensor = new Symmetric3x3Wide { XX = Vector<float>.One, YY = Vector<float>.One, ZZ = Vector<float>.One }, InverseMass = Vector<float>.One };
+        return BenchmarkFourBodyConstraint<VolumeConstraintFunctions, VolumeConstraintPrestepData, Vector<float>>(
+            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/GatherScatterBenchmarks.cs b/DemoBenchmarks/GatherScatterBenchmarks.cs
new file mode 100644
index 000000000..8647eca8a
--- /dev/null
+++ b/DemoBenchmarks/GatherScatterBenchmarks.cs
@@ -0,0 +1,162 @@
+using BenchmarkDotNet.Attributes;
+using BepuPhysics;
+using BepuPhysics.Collidables;
+using BepuPhysics.CollisionDetection;
+using BepuPhysics.Constraints;
+using BepuUtilities;
+using BepuUtilities.Memory;
+using System.Numerics;
+using static DemoBenchmarks.BenchmarkHelper;
+
+namespace DemoBenchmarks;
+
+/// <summary>
+/// Evaluates performance of scatter/gather operations used by constraints to pull body data.
+/// </summary>
+public class GatherScatterBenchmarks
+{
+    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<TManifold>(int workerIndex, CollidablePair pair, ref TManifold manifold, out PairMaterialProperties pairMaterial) where TManifold : unmanaged, IContactManifold<TManifold>
+        {
+            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<int> bodyIndices, Vector3Wide position, QuaternionWide orientation, BodyInertiaWide localInertia, Vector<int> integrationMask, int workerIndex, Vector<float> dt, ref BodyVelocityWide velocity) { }
+
+    }
+
+
+    const int iterationCount = 1000;
+    const int bodyCount = 1000;
+    BufferPool pool;
+
+
+    Buffer<Vector<int>> bodyIndices;
+    Buffer<RigidPoseWide> poses;
+    Buffer<BodyVelocityWide> velocities;
+    Buffer<BodyInertiaWide> 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<int> bodyIndicesBundle = stackalloc int[Vector<int>.Count];
+        for (int i = 0; i < iterationCount; ++i)
+        {
+            for (int j = 0; j < Vector<int>.Count; ++j)
+            {
+                bodyIndicesBundle[j] = random.Next(0, bodyCount);
+            }
+            bodyIndices[i] = new Vector<int>(bodyIndicesBundle);
+
+            simulation.Bodies.GatherState<AccessAll>(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<float> GatherState()
+    {
+        Vector<float> sum = default;
+        for (int i = 0; i < iterationCount; ++i)
+        {
+            simulation.Bodies.GatherState<AccessAll>(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;
+    }
+
+
+    //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 ScatterState()
+    {
+        var mask = new Vector<int>(-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);
+            simulation.Bodies.ScatterInertia(ref inertias[i], bodyIndices[i], mask);
+            simulation.Bodies.ScatterVelocities<AccessAll>(ref velocities[i], ref bodyIndices[i]);
+        }
+    }
+
+    //[Benchmark]
+    //public unsafe void ScatterPose()
+    //{
+    //    var mask = new Vector<int>(-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<int>(-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<AccessAll>(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;
+
+/// <summary>
+/// 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 <see cref="ConvexCollisionTesterBenchmarks"/>.
+/// </summary>
+/// <remarks>
+/// Note that all of these collision testers operate across <see cref="Vector{}.Count"/> lanes simultaneously where T is of type <see cref="float"/>.
+/// <para>The number of bundles being executed does not change if <see cref="Vector{}.Count"/> changes; if larger bundles are allowed, then more lanes end up getting solved.</para>
+/// </remarks>
+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<float> CheapCollisionBenchmarks()
+    {
+        return
+            benchmarks.SpherePairTester() +
+            benchmarks.SphereCapsuleTester() +
+            benchmarks.SphereBoxTester() +
+            benchmarks.SphereTriangleTester() +
+            benchmarks.SphereCylinderTester() +
+            benchmarks.CapsulePairTester();
+    }
+
+    [Benchmark]
+    public Vector<float> ModerateCostCollisionBenchmarks()
+    {
+        return
+            benchmarks.CapsuleBoxTester() +
+            benchmarks.CapsuleTriangleTester() +
+            benchmarks.CapsuleCylinderTester() +
+            benchmarks.BoxPairTester() +
+            benchmarks.BoxTriangleTester() +
+            benchmarks.TrianglePairTester();
+    }
+
+    [Benchmark]
+    public Vector<float> ExpensiveCollisionBenchmarks()
+    {
+        return
+            benchmarks.SphereConvexHullTester() +
+            benchmarks.CapsuleConvexHullTester() +
+            benchmarks.BoxConvexHullTester() +
+            benchmarks.TriangleConvexHullTester() +
+            benchmarks.CylinderConvexHullTester() +
+            benchmarks.ConvexHullPairTester() +
+            benchmarks.BoxCylinderTester() +
+            benchmarks.TriangleCylinderTester() +
+            benchmarks.CylinderPairTester();
+    }
+}
diff --git a/DemoBenchmarks/OneBodyConstraintBenchmarks.cs b/DemoBenchmarks/OneBodyConstraintBenchmarks.cs
new file mode 100644
index 000000000..440d028e9
--- /dev/null
+++ b/DemoBenchmarks/OneBodyConstraintBenchmarks.cs
@@ -0,0 +1,113 @@
+using BenchmarkDotNet.Attributes;
+using BepuPhysics;
+using BepuPhysics.Constraints;
+using BepuPhysics.Constraints.Contact;
+using BepuUtilities;
+using System.Numerics;
+
+namespace DemoBenchmarks;
+
+/// <summary>
+/// Evaluates performance of a representative subset of one body constraints. Excluded types are benchmarked in <see cref="OneBodyConstraintBenchmarksDeep"/>.
+/// </summary>
+/// <remarks>
+/// Note that all constraints operate across <see cref="Vector{}.Count"/> lanes simultaneously where T is of type <see cref="float"/>.
+/// <para>The number of bundles being executed does not change if <see cref="Vector{}.Count"/> changes; if larger bundles are allowed, then more lanes end up getting solved.</para>
+/// </remarks>
+public class OneBodyConstraintBenchmarks
+{
+    public static BodyVelocityWide BenchmarkOneBodyConstraint<TConstraintFunctions, TPrestep, TAccumulatedImpulse>(Vector3Wide positionA, QuaternionWide orientationA, BodyInertiaWide inertiaA, TPrestep prestep)
+        where TConstraintFunctions : unmanaged, IOneBodyConstraintFunctions<TPrestep, TAccumulatedImpulse> where TPrestep : unmanaged where TAccumulatedImpulse : unmanaged
+    {
+        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)
+        {
+            TConstraintFunctions.WarmStart(positionA, orientationA, inertiaA, ref prestep, ref accumulatedImpulse, ref velocityA);
+            TConstraintFunctions.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<float>.Zero, OffsetA = Vector3Wide.Broadcast(new Vector3(1, 0, 0)) },
+            Contact1 = new() { Depth = Vector<float>.Zero, OffsetA = Vector3Wide.Broadcast(new Vector3(1, 0, 0)) },
+            Contact2 = new() { Depth = Vector<float>.Zero, OffsetA = Vector3Wide.Broadcast(new Vector3(1, 0, 0)) },
+            Contact3 = new() { Depth = Vector<float>.Zero, OffsetA = Vector3Wide.Broadcast(new Vector3(1, 0, 0)) },
+            MaterialProperties = new MaterialPropertiesWide
+            {
+                FrictionCoefficient = new Vector<float>(1f),
+                MaximumRecoveryVelocity = new Vector<float>(2f),
+                SpringSettings = new() { TwiceDampingRatio = new Vector<float>(2), AngularFrequency = new Vector<float>(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<float>.One, YY = Vector<float>.One, ZZ = Vector<float>.One }, InverseMass = Vector<float>.One };
+        return BenchmarkOneBodyConstraint<Contact4OneBodyFunctions, Contact4OneBodyPrestepData, Contact4AccumulatedImpulses>(new Vector3Wide(), orientationA, inertia, prestep);
+    }
+
+    [Benchmark]
+    public BodyVelocityWide Contact4NonconvexOneBody()
+    {
+        var prestep = new Contact4NonconvexOneBodyPrestepData
+        {
+            Contact0 = new() { Depth = Vector<float>.Zero, Offset = Vector3Wide.Broadcast(new Vector3(1, 0, 0)), Normal = Vector3Wide.Broadcast(new Vector3(0, 1, 0)) },
+            Contact1 = new() { Depth = Vector<float>.Zero, Offset = Vector3Wide.Broadcast(new Vector3(1, 0, 0)), Normal = Vector3Wide.Broadcast(new Vector3(0, 1, 0)) },
+            Contact2 = new() { Depth = Vector<float>.Zero, Offset = Vector3Wide.Broadcast(new Vector3(1, 0, 0)), Normal = Vector3Wide.Broadcast(new Vector3(0, 1, 0)) },
+            Contact3 = new() { Depth = Vector<float>.Zero, Offset = Vector3Wide.Broadcast(new Vector3(1, 0, 0)), Normal = Vector3Wide.Broadcast(new Vector3(0, 1, 0)) },
+            MaterialProperties = new MaterialPropertiesWide
+            {
+                FrictionCoefficient = new Vector<float>(1f),
+                MaximumRecoveryVelocity = new Vector<float>(2f),
+                SpringSettings = new() { TwiceDampingRatio = new Vector<float>(2), AngularFrequency = new Vector<float>(20 * MathF.PI) }
+            },
+        };
+
+        QuaternionWide.Broadcast(Quaternion.Identity, out var orientation);
+        var inertia = new BodyInertiaWide { InverseInertiaTensor = new Symmetric3x3Wide { XX = Vector<float>.One, YY = Vector<float>.One, ZZ = Vector<float>.One }, InverseMass = Vector<float>.One };
+        return BenchmarkOneBodyConstraint<ContactNonconvexOneBodyFunctions<Contact4NonconvexOneBodyPrestepData, Contact4NonconvexAccumulatedImpulses>,
+            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<float>.Zero, MaximumForce = new Vector<float>(float.MaxValue), MaximumSpeed = new Vector<float>(float.MaxValue) },
+            SpringSettings = new() { TwiceDampingRatio = new Vector<float>(2), AngularFrequency = new Vector<float>(20 * MathF.PI) },
+            TargetOrientation = orientation
+        };
+
+        var inertia = new BodyInertiaWide { InverseInertiaTensor = new Symmetric3x3Wide { XX = Vector<float>.One, YY = Vector<float>.One, ZZ = Vector<float>.One }, InverseMass = Vector<float>.One };
+        return BenchmarkOneBodyConstraint<OneBodyAngularServoFunctions, OneBodyAngularServoPrestepData, Vector3Wide>(new Vector3Wide(), orientation, inertia, prestep);
+    }
+
+    [Benchmark]
+    public BodyVelocityWide OneBodyLinearMotor()
+    {
+        QuaternionWide.Broadcast(Quaternion.Identity, out var orientation);
+        var prestep = new OneBodyLinearMotorPrestepData
+        {
+            Settings = new() { Damping = Vector<float>.One, MaximumForce = Vector<float>.One },
+        };
+
+        var inertia = new BodyInertiaWide { InverseInertiaTensor = new Symmetric3x3Wide { XX = Vector<float>.One, YY = Vector<float>.One, ZZ = Vector<float>.One }, InverseMass = Vector<float>.One };
+        return BenchmarkOneBodyConstraint<OneBodyLinearMotorFunctions, OneBodyLinearMotorPrestepData, Vector3Wide>(new Vector3Wide(), orientation, inertia, prestep);
+    }
+}
diff --git a/DemoBenchmarks/OneBodyConstraintBenchmarksDeep.cs b/DemoBenchmarks/OneBodyConstraintBenchmarksDeep.cs
new file mode 100644
index 000000000..2f1b99ead
--- /dev/null
+++ b/DemoBenchmarks/OneBodyConstraintBenchmarksDeep.cs
@@ -0,0 +1,153 @@
+using BenchmarkDotNet.Attributes;
+using BepuPhysics;
+using BepuPhysics.Constraints;
+using BepuPhysics.Constraints.Contact;
+using BepuUtilities;
+using System.Numerics;
+using static DemoBenchmarks.OneBodyConstraintBenchmarks;
+
+namespace DemoBenchmarks;
+
+/// <summary>
+/// Evaluates performance of all one body constraints excluded from <see cref="OneBodyConstraintBenchmarks"/>
+/// </summary>
+/// <remarks>
+/// Note that all constraints operate across <see cref="Vector{}.Count"/> lanes simultaneously where T is of type <see cref="float"/>.
+/// <para>The number of bundles being executed does not change if <see cref="Vector{}.Count"/> changes; if larger bundles are allowed, then more lanes end up getting solved.</para>
+/// </remarks>
+public class OneBodyConstraintBenchmarksDeep
+{
+    [Benchmark]
+    public BodyVelocityWide Contact1OneBody()
+    {
+        var prestep = new Contact1OneBodyPrestepData
+        {
+            Contact0 = new() { Depth = Vector<float>.Zero, OffsetA = Vector3Wide.Broadcast(new Vector3(1, 0, 0)) },
+            MaterialProperties = new MaterialPropertiesWide
+            {
+                FrictionCoefficient = new Vector<float>(1f),
+                MaximumRecoveryVelocity = new Vector<float>(2f),
+                SpringSettings = new() { TwiceDampingRatio = new Vector<float>(2), AngularFrequency = new Vector<float>(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<float>.One, YY = Vector<float>.One, ZZ = Vector<float>.One }, InverseMass = Vector<float>.One };
+        return BenchmarkOneBodyConstraint<Contact1OneBodyFunctions, Contact1OneBodyPrestepData, Contact1AccumulatedImpulses>(new Vector3Wide(), orientation, inertia, prestep);
+    }
+
+    [Benchmark]
+    public BodyVelocityWide Contact2OneBody()
+    {
+        var prestep = new Contact2OneBodyPrestepData
+        {
+            Contact0 = new() { Depth = Vector<float>.Zero, OffsetA = Vector3Wide.Broadcast(new Vector3(1, 0, 0)) },
+            Contact1 = new() { Depth = Vector<float>.Zero, OffsetA = Vector3Wide.Broadcast(new Vector3(1, 0, 0)) },
+            MaterialProperties = new MaterialPropertiesWide
+            {
+                FrictionCoefficient = new Vector<float>(1f),
+                MaximumRecoveryVelocity = new Vector<float>(2f),
+                SpringSettings = new() { TwiceDampingRatio = new Vector<float>(2), AngularFrequency = new Vector<float>(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<float>.One, YY = Vector<float>.One, ZZ = Vector<float>.One }, InverseMass = Vector<float>.One };
+        return BenchmarkOneBodyConstraint<Contact2OneBodyFunctions, Contact2OneBodyPrestepData, Contact2AccumulatedImpulses>(new Vector3Wide(), orientation, inertia, prestep);
+    }
+
+    [Benchmark]
+    public BodyVelocityWide Contact3OneBody()
+    {
+        var prestep = new Contact3OneBodyPrestepData
+        {
+            Contact0 = new() { Depth = Vector<float>.Zero, OffsetA = Vector3Wide.Broadcast(new Vector3(1, 0, 0)) },
+            Contact1 = new() { Depth = Vector<float>.Zero, OffsetA = Vector3Wide.Broadcast(new Vector3(1, 0, 0)) },
+            Contact2 = new() { Depth = Vector<float>.Zero, OffsetA = Vector3Wide.Broadcast(new Vector3(1, 0, 0)) },
+            MaterialProperties = new MaterialPropertiesWide
+            {
+                FrictionCoefficient = new Vector<float>(1f),
+                MaximumRecoveryVelocity = new Vector<float>(2f),
+                SpringSettings = new() { TwiceDampingRatio = new Vector<float>(2), AngularFrequency = new Vector<float>(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<float>.One, YY = Vector<float>.One, ZZ = Vector<float>.One }, InverseMass = Vector<float>.One };
+        return BenchmarkOneBodyConstraint<Contact3OneBodyFunctions, Contact3OneBodyPrestepData, Contact3AccumulatedImpulses>(new Vector3Wide(), orientationA, inertia, prestep);
+    }
+
+    [Benchmark]
+    public BodyVelocityWide Contact2NonconvexOneBody()
+    {
+        var prestep = new Contact2NonconvexOneBodyPrestepData
+        {
+            Contact0 = new() { Depth = Vector<float>.Zero, Offset = Vector3Wide.Broadcast(new Vector3(1, 0, 0)), Normal = Vector3Wide.Broadcast(new Vector3(0, 1, 0)) },
+            Contact1 = new() { Depth = Vector<float>.Zero, Offset = Vector3Wide.Broadcast(new Vector3(1, 0, 0)), Normal = Vector3Wide.Broadcast(new Vector3(0, 1, 0)) },
+            MaterialProperties = new MaterialPropertiesWide
+            {
+                FrictionCoefficient = new Vector<float>(1f),
+                MaximumRecoveryVelocity = new Vector<float>(2f),
+                SpringSettings = new() { TwiceDampingRatio = new Vector<float>(2), AngularFrequency = new Vector<float>(20 * MathF.PI) }
+            },
+        };
+
+        QuaternionWide.Broadcast(Quaternion.Identity, out var orientation);
+        var inertia = new BodyInertiaWide { InverseInertiaTensor = new Symmetric3x3Wide { XX = Vector<float>.One, YY = Vector<float>.One, ZZ = Vector<float>.One }, InverseMass = Vector<float>.One };
+        return BenchmarkOneBodyConstraint<ContactNonconvexOneBodyFunctions<Contact2NonconvexOneBodyPrestepData, Contact2NonconvexAccumulatedImpulses>,
+            Contact2NonconvexOneBodyPrestepData, Contact2NonconvexAccumulatedImpulses>(new Vector3Wide(), orientation, inertia, prestep);
+    }
+
+    [Benchmark]
+    public BodyVelocityWide Contact3NonconvexOneBody()
+    {
+        var prestep = new Contact3NonconvexOneBodyPrestepData
+        {
+            Contact0 = new() { Depth = Vector<float>.Zero, Offset = Vector3Wide.Broadcast(new Vector3(1, 0, 0)), Normal = Vector3Wide.Broadcast(new Vector3(0, 1, 0)) },
+            Contact1 = new() { Depth = Vector<float>.Zero, Offset = Vector3Wide.Broadcast(new Vector3(1, 0, 0)), Normal = Vector3Wide.Broadcast(new Vector3(0, 1, 0)) },
+            Contact2 = new() { Depth = Vector<float>.Zero, Offset = Vector3Wide.Broadcast(new Vector3(1, 0, 0)), Normal = Vector3Wide.Broadcast(new Vector3(0, 1, 0)) },
+            MaterialProperties = new MaterialPropertiesWide
+            {
+                FrictionCoefficient = new Vector<float>(1f),
+                MaximumRecoveryVelocity = new Vector<float>(2f),
+                SpringSettings = new() { TwiceDampingRatio = new Vector<float>(2), AngularFrequency = new Vector<float>(20 * MathF.PI) }
+            },
+        };
+
+        QuaternionWide.Broadcast(Quaternion.Identity, out var orientation);
+        var inertia = new BodyInertiaWide { InverseInertiaTensor = new Symmetric3x3Wide { XX = Vector<float>.One, YY = Vector<float>.One, ZZ = Vector<float>.One }, InverseMass = Vector<float>.One };
+        return BenchmarkOneBodyConstraint<ContactNonconvexOneBodyFunctions<Contact3NonconvexOneBodyPrestepData, Contact3NonconvexAccumulatedImpulses>,
+            Contact3NonconvexOneBodyPrestepData, Contact3NonconvexAccumulatedImpulses>(new Vector3Wide(), orientation, inertia, prestep);
+    }
+
+    [Benchmark]
+    public BodyVelocityWide OneBodyAngularMotor()
+    {
+        QuaternionWide.Broadcast(Quaternion.Identity, out var orientation);
+        var prestep = new OneBodyAngularMotorPrestepData
+        {
+            Settings = new() { Damping = Vector<float>.One, MaximumForce = Vector<float>.One },
+            TargetVelocity = default
+        };
+
+        var inertia = new BodyInertiaWide { InverseInertiaTensor = new Symmetric3x3Wide { XX = Vector<float>.One, YY = Vector<float>.One, ZZ = Vector<float>.One }, InverseMass = Vector<float>.One };
+        return BenchmarkOneBodyConstraint<OneBodyAngularMotorFunctions, OneBodyAngularMotorPrestepData, Vector3Wide>(new Vector3Wide(), orientation, inertia, prestep);
+    }
+
+    [Benchmark]
+    public BodyVelocityWide OneBodyLinearServo()
+    {
+        QuaternionWide.Broadcast(Quaternion.Identity, out var orientation);
+        var prestep = new OneBodyLinearServoPrestepData
+        {
+            ServoSettings = new() { BaseSpeed = Vector<float>.Zero, MaximumForce = new Vector<float>(float.MaxValue), MaximumSpeed = new Vector<float>(float.MaxValue) },
+            SpringSettings = new() { TwiceDampingRatio = new Vector<float>(2), AngularFrequency = new Vector<float>(20 * MathF.PI) }
+        };
+
+        var inertia = new BodyInertiaWide { InverseInertiaTensor = new Symmetric3x3Wide { XX = Vector<float>.One, YY = Vector<float>.One, ZZ = Vector<float>.One }, InverseMass = Vector<float>.One };
+        return BenchmarkOneBodyConstraint<OneBodyLinearServoFunctions, OneBodyLinearServoPrestepData, Vector3Wide>(new Vector3Wide(), orientation, inertia, prestep);
+    }
+}
diff --git a/DemoBenchmarks/Program.cs b/DemoBenchmarks/Program.cs
new file mode 100644
index 000000000..59e457681
--- /dev/null
+++ b/DemoBenchmarks/Program.cs
@@ -0,0 +1,10 @@
+using BenchmarkDotNet.Running;
+using DemoBenchmarks;
+
+public class BepuPhysics2Benchmarks
+{
+    public static void Main(string[] args)
+    {
+        BenchmarkSwitcher.FromAssembly(typeof(BepuPhysics2Benchmarks).Assembly).Run(args);
+    }
+}
diff --git a/DemoBenchmarks/README.md b/DemoBenchmarks/README.md
new file mode 100644
index 000000000..0a3a4834a
--- /dev/null
+++ b/DemoBenchmarks/README.md
@@ -0,0 +1,53 @@
+# Benchmarks
+
+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 benchmarks, 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:
+
+[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.
+
+[FourBodyConstraintBenchmarks](FourBodyConstraintBenchmarks.cs): Four body constraints, one benchmark per type.
+
+[ShapeRayBenchmarksDeep](ShapeRayBenchmarksDeep.cs): Shape-ray tests, one benchmark per type.
+
+[SweepBenchmarks](SweepBenchmarks.cs): Linear + angular sweep tests for shape pairs, one benchmark per type pair.
+
+[GatherScatterBenchmarks](GatherScatterBenchmarks.cs): Tests SIMD body data gathering/scattering for constraint solving. One benchmark for gather and one for scatter.
+
+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`:
+
+[OneBodyConstraintBenchmarksDeep](OneBodyConstraintBenchmarksDeep.cs): One body constraints excluded from `OneBodyConstraintBenchmarks`.
+
+[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:
+
+[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.
+
+[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.
diff --git a/DemoBenchmarks/RagdollTubeBenchmark.cs b/DemoBenchmarks/RagdollTubeBenchmark.cs
new file mode 100644
index 000000000..303444f56
--- /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;
+
+/// <summary>
+/// Evaluates performance of a simulation similar to the first timesteps of the RagdollTubeDemo.
+/// </summary>
+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<SubgroupCollisionFilter> CollisionFilters;
+        public PairMaterialProperties Material;
+        public SubgroupFilteredCallbacks(CollidableProperty<SubgroupCollisionFilter> filters)
+        {
+            CollisionFilters = filters;
+            Material = new PairMaterialProperties(1, 2, new SpringSettings(30, 1));
+        }
+        public SubgroupFilteredCallbacks(CollidableProperty<SubgroupCollisionFilter> 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<TManifold>(int workerIndex, CollidablePair pair, ref TManifold manifold, out PairMaterialProperties pairMaterial) where TManifold : unmanaged, IContactManifold<TManifold>
+        {
+            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<float> linearDampingDt;
+        Vector<float> angularDampingDt;
+
+        public void PrepareForIntegration(float dt)
+        {
+            linearDampingDt = new Vector<float>(MathF.Pow(MathHelper.Clamp(1 - LinearDamping, 0, 1), dt));
+            angularDampingDt = new Vector<float>(MathF.Pow(MathHelper.Clamp(1 - AngularDamping, 0, 1), dt));
+            gravityWideDt = Vector3Wide.Broadcast(Gravity * dt);
+        }
+        public void IntegrateVelocity(Vector<int> bodyIndices, Vector3Wide position, QuaternionWide orientation, BodyInertiaWide localInertia, Vector<int> integrationMask, int workerIndex, Vector<float> dt, ref BodyVelocityWide velocity)
+        {
+            velocity.Linear = (velocity.Linear + gravityWideDt) * linearDampingDt;
+            velocity.Angular = velocity.Angular * angularDampingDt;
+        }
+    }
+    static BodyHandle AddBody<TShape>(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<SubgroupCollisionFilter> 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<SubgroupCollisionFilter> 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<SubgroupCollisionFilter> 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<SubgroupCollisionFilter>();
+        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 RagdollTubeBenchmarks()
+    {
+        for (int i = 0; i < timestepCount; ++i)
+        {
+            Simulation.Timestep(1 / 60f);
+        }
+    }
+}
diff --git a/DemoBenchmarks/ShapePileBenchmark.cs b/DemoBenchmarks/ShapePileBenchmark.cs
new file mode 100644
index 000000000..fcd4745c7
--- /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;
+
+/// <summary>
+/// Evaluates performance of a simulation similar to the first timesteps of the ShapePileTestDemo.
+/// </summary>
+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<float> linearDampingDt;
+        Vector<float> angularDampingDt;
+        public void PrepareForIntegration(float dt)
+        {
+            linearDampingDt = new Vector<float>(MathF.Pow(MathHelper.Clamp(1 - LinearDamping, 0, 1), dt));
+            angularDampingDt = new Vector<float>(MathF.Pow(MathHelper.Clamp(1 - AngularDamping, 0, 1), dt));
+            gravityWideDt = Vector3Wide.Broadcast(Gravity * dt);
+        }
+        public void IntegrateVelocity(Vector<int> bodyIndices, Vector3Wide position, QuaternionWide orientation, BodyInertiaWide localInertia, Vector<int> integrationMask, int workerIndex, Vector<float> 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<TManifold>(int workerIndex, CollidablePair pair, ref TManifold manifold, out PairMaterialProperties pairMaterial) where TManifold : unmanaged, IContactManifold<TManifold>
+        {
+            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<Vector3>(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 = 4;
+        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 ShapePileBenchmarks()
+    {
+        for (int i = 0; i < timestepCount; ++i)
+        {
+            Simulation.Timestep(1 / 60f);
+        }
+    }
+}
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;
+
+/// <summary>
+/// Evaluates performance of shape ray tests. Performs groups of types in single benchmarks: all convexes in one, compounds in the other.
+/// </summary>
+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/ShapeRayBenchmarksDeep.cs b/DemoBenchmarks/ShapeRayBenchmarksDeep.cs
new file mode 100644
index 000000000..63c4c9e57
--- /dev/null
+++ b/DemoBenchmarks/ShapeRayBenchmarksDeep.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;
+
+/// <summary>
+/// Evaluates performance of shape ray tests. Each benchmark covers a different shape type.
+/// </summary>
+public class ShapeRayBenchmarksDeep
+{
+    const int iterationCount = 100;
+    BufferPool pool;
+
+    struct Iteration
+    {
+        public RigidPose Pose;
+        public RayData Ray;
+    }
+
+    Buffer<Iteration> 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<TShape>() 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[TShape.TypeId].RayTest(0, iteration.Pose, iteration.Ray, ref maximumT, pool, ref hitHandler);
+        }
+        return hitHandler.ResultSum;
+    }
+
+    [Benchmark]
+    public unsafe Vector3 RaySphere() => Test<Sphere>();
+    [Benchmark]
+    public unsafe Vector3 RayCapsule() => Test<Capsule>();
+    [Benchmark]
+    public unsafe Vector3 RayBox() => Test<Box>();
+    [Benchmark]
+    public unsafe Vector3 RayTriangle() => Test<Triangle>();
+    [Benchmark]
+    public unsafe Vector3 RayCylinder() => Test<Cylinder>();
+    [Benchmark]
+    public unsafe Vector3 RayConvexHull() => Test<ConvexHull>();
+    [Benchmark]
+    public unsafe Vector3 RayCompound() => Test<Compound>();
+    [Benchmark]
+    public unsafe Vector3 RayBigCompound() => Test<BigCompound>();
+    [Benchmark]
+    public unsafe Vector3 RayMesh() => Test<Mesh>();
+}
diff --git a/DemoBenchmarks/SweepBenchmarks.cs b/DemoBenchmarks/SweepBenchmarks.cs
new file mode 100644
index 000000000..69155c412
--- /dev/null
+++ b/DemoBenchmarks/SweepBenchmarks.cs
@@ -0,0 +1,258 @@
+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;
+
+public class Sweeper
+{
+    const int iterationCount = 10;
+    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<Pair> pairs;
+    SweepTaskRegistry taskRegistry;
+    Shapes shapes;
+
+    public Sweeper()
+    {
+        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<float> 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<float> 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<float> 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 = 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 = 0.1f * (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 Filter : ISweepFilter
+    {
+        [MethodImpl(MethodImplOptions.AggressiveInlining)]
+        public bool AllowTest(int childA, int childB)
+        {
+            return true;
+        }
+    }
+
+    public unsafe Vector3 Test<TA, TB>() where TA : unmanaged, IShape where TB : unmanaged, IShape
+    {
+        var task = taskRegistry.GetTask<TA, TB>();
+        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);
+        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;
+    }
+}
+
+/// <summary>
+/// Evaluates performance of a representative subset of sweep tests.
+/// </summary>
+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<Sphere, Sphere>();
+    [Benchmark]
+    public void SphereCapsule() => sweeper.Test<Sphere, Capsule>();
+    //[Benchmark]
+    //public void SphereBox() => sweeper.Test<Sphere, Box>();
+    [Benchmark]
+    public void SphereTriangle() => sweeper.Test<Sphere, Triangle>();
+    //[Benchmark]
+    //public void SphereCylinder() => sweeper.Test<Sphere, Cylinder>();
+    //[Benchmark]
+    //public void SphereConvexHull() => Test<Sphere, ConvexHull>(); //GJK
+    //[Benchmark]
+    //public void SphereCompound() => Test<Sphere, Compound>();
+    //[Benchmark]
+    //public void SphereBigCompound() => Test<Sphere, BigCompound>();
+    //[Benchmark]
+    //public void SphereMesh() => Test<Sphere, Mesh>();
+    [Benchmark]
+    public void CapsuleCapsule() => sweeper.Test<Capsule, Capsule>();
+    [Benchmark]
+    public void CapsuleBox() => sweeper.Test<Capsule, Box>();
+    //[Benchmark]
+    //public void CapsuleTriangle() => Test<Capsule, Triangle>(); //GJK
+    [Benchmark]
+    public void CapsuleCylinder() => sweeper.Test<Capsule, Cylinder>(); //GJK
+    //[Benchmark]
+    //public void CapsuleConvexHull() => Test<Capsule, ConvexHull>(); //GJK
+    //[Benchmark]
+    //public void CapsuleCompound() => Test<Capsule, Compound>();
+    //[Benchmark]
+    //public void CapsuleBigCompound() => Test<Capsule, BigCompound>();
+    //[Benchmark]
+    //public void CapsuleMesh() => Test<Capsule, Mesh>();
+    //[Benchmark]
+    //public void BoxBox() => Test<Box, Box>(); //GJK
+    //[Benchmark]
+    //public void BoxTriangle() => Test<Box, Triangle>(); //GJK
+    //[Benchmark]
+    //public void BoxCylinder() => Test<Box, Cylinder>(); //GJK
+    //[Benchmark]
+    //public void BoxConvexHull() => Test<Box, ConvexHull>(); //GJK
+    [Benchmark]
+    public void BoxCompound() => sweeper.Test<Box, Compound>();
+    //[Benchmark]
+    //public void BoxBigCompound() => Test<Box, BigCompound>(); //Well covered by bigcompound-bigcompound.
+    [Benchmark]
+    public void BoxMesh() => sweeper.Test<Box, Mesh>();
+    //[Benchmark]
+    //public void TriangleTriangle() => Test<Triangle, Triangle>(); //GJK
+    //[Benchmark]
+    //public void TriangleCylinder() => Test<Triangle, Cylinder>(); //GJK
+    //[Benchmark]
+    //public void TriangleConvexHull() => Test<Triangle, ConvexHull>(); //GJK
+    //[Benchmark]
+    //public void TriangleCompound() => Test<Triangle, Compound>();
+    //[Benchmark]
+    //public void TriangleBigCompound() => Test<Triangle, BigCompound>();
+    //[Benchmark]
+    //public void TriangleMesh() => Test<Triangle, Mesh>();
+    //[Benchmark]
+    //public void CylinderCylinder() => Test<Cylinder, Cylinder>(); //GJK
+    //[Benchmark]
+    //public void CylinderConvexHull() => Test<Cylinder, ConvexHull>(); //GJK
+    //[Benchmark]
+    //public void CylinderCompound() => Test<Cylinder, Compound>();
+    //[Benchmark]
+    //public void CylinderBigCompound() => Test<Cylinder, BigCompound>();
+    //[Benchmark]
+    //public void CylinderMesh() => Test<Cylinder, Mesh>();
+    //[Benchmark]
+    //public void ConvexHullConvexHull() => Test<ConvexHull, ConvexHull>(); //GJK
+    //[Benchmark]
+    //public void ConvexHullCompound() => Test<ConvexHull, Compound>();
+    //[Benchmark]
+    //public void ConvexHullBigCompound() => Test<ConvexHull, BigCompound>();
+    //[Benchmark]
+    //public void ConvexHullMesh() => Test<ConvexHull, Mesh>();
+    //[Benchmark]
+    //public void CompoundCompound() => Test<Compound, Compound>();
+    //[Benchmark]
+    //public void CompoundBigCompound() => Test<Compound, BigCompound>();
+    //[Benchmark]
+    //public void CompoundMesh() => Test<Compound, Mesh>();
+    [Benchmark]
+    public void BigCompoundBigCompound() => sweeper.Test<BigCompound, BigCompound>();
+    //[Benchmark]
+    //public void BigCompoundMesh() => Test<BigCompound, Mesh>();
+    //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;
+
+/// <summary>
+/// Evaluates performance of all sweep tests excluded from <see cref="SweepBenchmarks"/>.
+/// </summary>
+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<Sphere, Sphere>();
+    [Benchmark]
+    public void SphereBox() => sweeper.Test<Sphere, Box>();
+    [Benchmark]
+    public void SphereCylinder() => sweeper.Test<Sphere, Cylinder>();
+    [Benchmark]
+    public void SphereConvexHull() => sweeper.Test<Sphere, ConvexHull>(); //GJK
+    [Benchmark]
+    public void SphereCompound() => sweeper.Test<Sphere, Compound>();
+    [Benchmark]
+    public void SphereBigCompound() => sweeper.Test<Sphere, BigCompound>();
+    [Benchmark]
+    public void SphereMesh() => sweeper.Test<Sphere, Mesh>();
+    [Benchmark]
+    public void CapsuleTriangle() => sweeper.Test<Capsule, Triangle>(); //GJK
+    [Benchmark]
+    public void CapsuleConvexHull() => sweeper.Test<Capsule, ConvexHull>(); //GJK
+    [Benchmark]
+    public void CapsuleCompound() => sweeper.Test<Capsule, Compound>();
+    [Benchmark]
+    public void CapsuleBigCompound() => sweeper.Test<Capsule, BigCompound>();
+    [Benchmark]
+    public void CapsuleMesh() => sweeper.Test<Capsule, Mesh>();
+    [Benchmark]
+    public void BoxBox() => sweeper.Test<Box, Box>(); //GJK
+    [Benchmark]
+    public void BoxTriangle() => sweeper.Test<Box, Triangle>(); //GJK
+    [Benchmark]
+    public void BoxCylinder() => sweeper.Test<Box, Cylinder>(); //GJK
+    [Benchmark]
+    public void BoxConvexHull() => sweeper.Test<Box, ConvexHull>(); //GJK
+    [Benchmark]
+    public void BoxBigCompound() => sweeper.Test<Box, BigCompound>(); //Well covered by bigcompound-bigcompound.
+    [Benchmark]
+    public void TriangleTriangle() => sweeper.Test<Triangle, Triangle>(); //GJK
+    [Benchmark]
+    public void TriangleCylinder() => sweeper.Test<Triangle, Cylinder>(); //GJK
+    [Benchmark]
+    public void TriangleConvexHull() => sweeper.Test<Triangle, ConvexHull>(); //GJK
+    [Benchmark]
+    public void TriangleCompound() => sweeper.Test<Triangle, Compound>();
+    [Benchmark]
+    public void TriangleBigCompound() => sweeper.Test<Triangle, BigCompound>();
+    [Benchmark]
+    public void TriangleMesh() => sweeper.Test<Triangle, Mesh>();
+    [Benchmark]
+    public void CylinderCylinder() => sweeper.Test<Cylinder, Cylinder>(); //GJK
+    [Benchmark]
+    public void CylinderConvexHull() => sweeper.Test<Cylinder, ConvexHull>(); //GJK
+    [Benchmark]
+    public void CylinderCompound() => sweeper.Test<Cylinder, Compound>();
+    [Benchmark]
+    public void CylinderBigCompound() => sweeper.Test<Cylinder, BigCompound>();
+    [Benchmark]
+    public void CylinderMesh() => sweeper.Test<Cylinder, Mesh>();
+    [Benchmark]
+    public void ConvexHullConvexHull() => sweeper.Test<ConvexHull, ConvexHull>(); //GJK
+    [Benchmark]
+    public void ConvexHullCompound() => sweeper.Test<ConvexHull, Compound>();
+    [Benchmark]
+    public void ConvexHullBigCompound() => sweeper.Test<ConvexHull, BigCompound>();
+    [Benchmark]
+    public void ConvexHullMesh() => sweeper.Test<ConvexHull, Mesh>();
+    [Benchmark]
+    public void CompoundCompound() => sweeper.Test<Compound, Compound>();
+    [Benchmark]
+    public void CompoundBigCompound() => sweeper.Test<Compound, BigCompound>();
+    [Benchmark]
+    public void CompoundMesh() => sweeper.Test<Compound, Mesh>();
+    [Benchmark]
+    public void BigCompoundMesh() => sweeper.Test<BigCompound, Mesh>();
+    //No mesh-mesh!
+}
diff --git a/DemoBenchmarks/ThreeBodyConstraintBenchmarks.cs b/DemoBenchmarks/ThreeBodyConstraintBenchmarks.cs
new file mode 100644
index 000000000..2b19ec0b4
--- /dev/null
+++ b/DemoBenchmarks/ThreeBodyConstraintBenchmarks.cs
@@ -0,0 +1,58 @@
+using BenchmarkDotNet.Attributes;
+using BepuPhysics;
+using BepuPhysics.Constraints;
+using BepuPhysics.Constraints.Contact;
+using BepuUtilities;
+using System.Numerics;
+
+namespace DemoBenchmarks;
+
+/// <summary>
+/// Evaluates performance of all three body constraints.
+/// </summary>
+/// <remarks>
+/// Note that all constraints operate across <see cref="Vector{}.Count"/> lanes simultaneously where T is of type <see cref="float"/>.
+/// <para>The number of bundles being executed does not change if <see cref="Vector{}.Count"/> changes; if larger bundles are allowed, then more lanes end up getting solved.</para>
+/// </remarks>
+public class ThreeBodyConstraintBenchmarks
+{
+    static (BodyVelocityWide, BodyVelocityWide, BodyVelocityWide) BenchmarkThreeBodyConstraint<TConstraintFunctions, TPrestep, TAccumulatedImpulse>(
+        Vector3Wide positionA, QuaternionWide orientationA, BodyInertiaWide inertiaA,
+        Vector3Wide positionB, QuaternionWide orientationB, BodyInertiaWide inertiaB,
+        Vector3Wide positionC, QuaternionWide orientationC, BodyInertiaWide inertiaC, TPrestep prestep)
+        where TConstraintFunctions : unmanaged, IThreeBodyConstraintFunctions<TPrestep, TAccumulatedImpulse> where TPrestep : unmanaged where TAccumulatedImpulse : unmanaged
+    {
+        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 = 1000;
+        const float inverseDt = 60f;
+        const float dt = 1f / inverseDt;
+        for (int i = 0; i < iterations; ++i)
+        {
+            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);
+    }
+
+    //Not a lot of these yet!
+    [Benchmark]
+    public (BodyVelocityWide, BodyVelocityWide, BodyVelocityWide) AreaConstraint()
+    {
+        var prestep = new AreaConstraintPrestepData
+        {
+            TargetScaledArea = Vector<float>.One,
+            SpringSettings = new() { TwiceDampingRatio = new Vector<float>(2), AngularFrequency = new Vector<float>(20 * MathF.PI) }
+        };
+
+        QuaternionWide.Broadcast(Quaternion.Identity, out var orientation);
+        var inertia = new BodyInertiaWide { InverseInertiaTensor = new Symmetric3x3Wide { XX = Vector<float>.One, YY = Vector<float>.One, ZZ = Vector<float>.One }, InverseMass = Vector<float>.One };
+        return BenchmarkThreeBodyConstraint<AreaConstraintFunctions, AreaConstraintPrestepData, Vector<float>>(
+            new Vector3Wide(), orientation, inertia,
+            Vector3Wide.Broadcast(new Vector3(2, 0, 0)), orientation, inertia,
+            Vector3Wide.Broadcast(new Vector3(0, 2, 0)), orientation, inertia, prestep);
+    }
+}
diff --git a/DemoBenchmarks/TwoBodyConstraintBenchmarks.cs b/DemoBenchmarks/TwoBodyConstraintBenchmarks.cs
new file mode 100644
index 000000000..9658d81e4
--- /dev/null
+++ b/DemoBenchmarks/TwoBodyConstraintBenchmarks.cs
@@ -0,0 +1,300 @@
+using BenchmarkDotNet.Attributes;
+using BepuPhysics;
+using BepuPhysics.Constraints;
+using BepuPhysics.Constraints.Contact;
+using BepuUtilities;
+using System.Numerics;
+
+namespace DemoBenchmarks;
+
+/// <summary>
+/// Evaluates performance of a representative subset of two body constraints. Excluded types are benchmarked in <see cref="TwoBodyConstraintBenchmarksDeep"/>.
+/// </summary>
+/// <remarks>
+/// Note that all constraints operate across <see cref="Vector{}.Count"/> lanes simultaneously where T is of type <see cref="float"/>.
+/// <para>The number of bundles being executed does not change if <see cref="Vector{}.Count"/> changes; if larger bundles are allowed, then more lanes end up getting solved.</para>
+/// </remarks>
+public class TwoBodyConstraintBenchmarks
+{
+    public static (BodyVelocityWide, BodyVelocityWide) BenchmarkTwoBodyConstraint<TConstraintFunctions, TPrestep, TAccumulatedImpulse>(
+        Vector3Wide positionA, QuaternionWide orientationA, BodyInertiaWide inertiaA,
+        Vector3Wide positionB, QuaternionWide orientationB, BodyInertiaWide inertiaB, TPrestep prestep)
+        where TConstraintFunctions : unmanaged, ITwoBodyConstraintFunctions<TPrestep, TAccumulatedImpulse> where TPrestep : unmanaged where TAccumulatedImpulse : unmanaged
+    {
+        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 = 1000;
+        const float inverseDt = 60f;
+        const float dt = 1f / inverseDt;
+        for (int i = 0; i < iterations; ++i)
+        {
+            TConstraintFunctions.WarmStart(positionA, orientationA, inertiaA, positionB, orientationB, inertiaB, ref prestep, ref accumulatedImpulse, ref velocityA, ref velocityB);
+            TConstraintFunctions.Solve(positionA, orientationA, inertiaA, positionB, orientationB, inertiaB, dt, inverseDt, ref prestep, ref accumulatedImpulse, ref velocityA, ref velocityB);
+        }
+        return (velocityA, velocityB);
+    }
+
+    //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()
+    {
+        var prestep = new Contact4PrestepData
+        {
+            Contact0 = new ConvexContactWide { Depth = Vector<float>.Zero, OffsetA = Vector3Wide.Broadcast(new Vector3(1, 0, 0)) },
+            Contact1 = new ConvexContactWide { Depth = Vector<float>.Zero, OffsetA = Vector3Wide.Broadcast(new Vector3(1, 0, 0)) },
+            Contact2 = new ConvexContactWide { Depth = Vector<float>.Zero, OffsetA = Vector3Wide.Broadcast(new Vector3(1, 0, 0)) },
+            Contact3 = new ConvexContactWide { Depth = Vector<float>.Zero, OffsetA = Vector3Wide.Broadcast(new Vector3(1, 0, 0)) },
+            MaterialProperties = new MaterialPropertiesWide
+            {
+                FrictionCoefficient = new Vector<float>(1f),
+                MaximumRecoveryVelocity = new Vector<float>(2f),
+                SpringSettings = new() { TwiceDampingRatio = new Vector<float>(2), AngularFrequency = new Vector<float>(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<float>.One, YY = Vector<float>.One, ZZ = Vector<float>.One }, InverseMass = Vector<float>.One };
+        return BenchmarkTwoBodyConstraint<Contact4Functions, Contact4PrestepData, Contact4AccumulatedImpulses>(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<float>.Zero, Offset = Vector3Wide.Broadcast(new Vector3(1, 0, 0)), Normal = Vector3Wide.Broadcast(new Vector3(0, 1, 0)) },
+            Contact1 = new NonconvexContactPrestepData { Depth = Vector<float>.Zero, Offset = Vector3Wide.Broadcast(new Vector3(1, 0, 1)), Normal = Vector3Wide.Broadcast(new Vector3(0, 1, 0)) },
+            Contact2 = new NonconvexContactPrestepData { Depth = Vector<float>.Zero, Offset = Vector3Wide.Broadcast(new Vector3(1, 1, 0)), Normal = Vector3Wide.Broadcast(new Vector3(0, 1, 0)) },
+            Contact3 = new NonconvexContactPrestepData { Depth = Vector<float>.Zero, Offset = Vector3Wide.Broadcast(new Vector3(1, 1, 1)), Normal = Vector3Wide.Broadcast(new Vector3(0, 1, 0)) },
+            MaterialProperties = new MaterialPropertiesWide
+            {
+                FrictionCoefficient = new Vector<float>(1f),
+                MaximumRecoveryVelocity = new Vector<float>(2f),
+                SpringSettings = new() { TwiceDampingRatio = new Vector<float>(2), AngularFrequency = new Vector<float>(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<float>.One, YY = Vector<float>.One, ZZ = Vector<float>.One }, InverseMass = Vector<float>.One };
+        return BenchmarkTwoBodyConstraint<ContactNonconvexTwoBodyFunctions<Contact4NonconvexPrestepData, Contact4NonconvexAccumulatedImpulses>,
+            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<float>(2), AngularFrequency = new Vector<float>(20 * MathF.PI) }
+        };
+
+        QuaternionWide.Broadcast(Quaternion.Identity, out var orientation);
+        var inertia = new BodyInertiaWide { InverseInertiaTensor = new Symmetric3x3Wide { XX = Vector<float>.One, YY = Vector<float>.One, ZZ = Vector<float>.One }, InverseMass = Vector<float>.One };
+        return BenchmarkTwoBodyConstraint<BallSocketFunctions, BallSocketPrestepData, Vector3Wide>(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<float>(2), AngularFrequency = new Vector<float>(20 * MathF.PI) }
+        };
+
+        QuaternionWide.Broadcast(Quaternion.Identity, out var orientation);
+        var inertia = new BodyInertiaWide { InverseInertiaTensor = new Symmetric3x3Wide { XX = Vector<float>.One, YY = Vector<float>.One, ZZ = Vector<float>.One }, InverseMass = Vector<float>.One };
+        return BenchmarkTwoBodyConstraint<AngularHingeFunctions, AngularHingePrestepData, Vector2Wide>(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<float>(2), AngularFrequency = new Vector<float>(20 * MathF.PI) }
+        };
+
+        QuaternionWide.Broadcast(Quaternion.Identity, out var orientation);
+        var inertia = new BodyInertiaWide { InverseInertiaTensor = new Symmetric3x3Wide { XX = Vector<float>.One, YY = Vector<float>.One, ZZ = Vector<float>.One }, InverseMass = Vector<float>.One };
+        return BenchmarkTwoBodyConstraint<AngularSwivelHingeFunctions, AngularSwivelHingePrestepData, Vector<float>>(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<float>(0.9f),
+            SpringSettings = new() { TwiceDampingRatio = new Vector<float>(2), AngularFrequency = new Vector<float>(20 * MathF.PI) }
+        };
+
+        QuaternionWide.Broadcast(Quaternion.Identity, out var orientation);
+        var inertia = new BodyInertiaWide { InverseInertiaTensor = new Symmetric3x3Wide { XX = Vector<float>.One, YY = Vector<float>.One, ZZ = Vector<float>.One }, InverseMass = Vector<float>.One };
+        return BenchmarkTwoBodyConstraint<SwingLimitFunctions, SwingLimitPrestepData, Vector<float>>(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>(float.MaxValue), MaximumSpeed = new Vector<float>(float.MaxValue) },
+            SpringSettings = new() { TwiceDampingRatio = new Vector<float>(2), AngularFrequency = new Vector<float>(20 * MathF.PI) }
+        };
+
+        var inertia = new BodyInertiaWide { InverseInertiaTensor = new Symmetric3x3Wide { XX = Vector<float>.One, YY = Vector<float>.One, ZZ = Vector<float>.One }, InverseMass = Vector<float>.One };
+        return BenchmarkTwoBodyConstraint<TwistServoFunctions, TwistServoPrestepData, Vector<float>>(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()
+    {
+        QuaternionWide.Broadcast(Quaternion.Identity, out var orientation);
+        var prestep = new AngularServoPrestepData
+        {
+            TargetRelativeRotationLocalA = orientation,
+            ServoSettings = new ServoSettingsWide { MaximumForce = new Vector<float>(float.MaxValue), MaximumSpeed = new Vector<float>(float.MaxValue) },
+            SpringSettings = new() { TwiceDampingRatio = new Vector<float>(2), AngularFrequency = new Vector<float>(20 * MathF.PI) }
+        };
+
+        var inertia = new BodyInertiaWide { InverseInertiaTensor = new Symmetric3x3Wide { XX = Vector<float>.One, YY = Vector<float>.One, ZZ = Vector<float>.One }, InverseMass = Vector<float>.One };
+        return BenchmarkTwoBodyConstraint<AngularServoFunctions, AngularServoPrestepData, Vector3Wide>(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()
+    {
+        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<float>(2), AngularFrequency = new Vector<float>(20 * MathF.PI) }
+        };
+
+        var inertia = new BodyInertiaWide { InverseInertiaTensor = new Symmetric3x3Wide { XX = Vector<float>.One, YY = Vector<float>.One, ZZ = Vector<float>.One }, InverseMass = Vector<float>.One };
+        return BenchmarkTwoBodyConstraint<WeldFunctions, WeldPrestepData, WeldAccumulatedImpulses>(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<float>.One,
+            ServoSettings = new ServoSettingsWide { MaximumForce = new Vector<float>(float.MaxValue), MaximumSpeed = new Vector<float>(float.MaxValue) },
+            SpringSettings = new() { TwiceDampingRatio = new Vector<float>(2), AngularFrequency = new Vector<float>(20 * MathF.PI) }
+        };
+
+        var inertia = new BodyInertiaWide { InverseInertiaTensor = new Symmetric3x3Wide { XX = Vector<float>.One, YY = Vector<float>.One, ZZ = Vector<float>.One }, InverseMass = Vector<float>.One };
+        return BenchmarkTwoBodyConstraint<DistanceServoFunctions, DistanceServoPrestepData, Vector<float>>(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()
+    {
+        QuaternionWide.Broadcast(Quaternion.Identity, out var orientation);
+        var prestep = new CenterDistancePrestepData
+        {
+            TargetDistance = new Vector<float>(0.5f),
+            SpringSettings = new() { TwiceDampingRatio = new Vector<float>(2), AngularFrequency = new Vector<float>(20 * MathF.PI) }
+        };
+
+        var inertia = new BodyInertiaWide { InverseInertiaTensor = new Symmetric3x3Wide { XX = Vector<float>.One, YY = Vector<float>.One, ZZ = Vector<float>.One }, InverseMass = Vector<float>.One };
+        return BenchmarkTwoBodyConstraint<CenterDistanceConstraintFunctions, CenterDistancePrestepData, Vector<float>>(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>(float.MaxValue), MaximumSpeed = new Vector<float>(float.MaxValue) },
+            SpringSettings = new() { TwiceDampingRatio = new Vector<float>(2), AngularFrequency = new Vector<float>(20 * MathF.PI) }
+        };
+
+        var inertia = new BodyInertiaWide { InverseInertiaTensor = new Symmetric3x3Wide { XX = Vector<float>.One, YY = Vector<float>.One, ZZ = Vector<float>.One }, InverseMass = Vector<float>.One };
+        return BenchmarkTwoBodyConstraint<PointOnLineServoFunctions, PointOnLineServoPrestepData, Vector2Wide>(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>(float.MaxValue), MaximumSpeed = new Vector<float>(float.MaxValue) },
+            SpringSettings = new() { TwiceDampingRatio = new Vector<float>(2), AngularFrequency = new Vector<float>(20 * MathF.PI) }
+        };
+
+        var inertia = new BodyInertiaWide { InverseInertiaTensor = new Symmetric3x3Wide { XX = Vector<float>.One, YY = Vector<float>.One, ZZ = Vector<float>.One }, InverseMass = Vector<float>.One };
+        return BenchmarkTwoBodyConstraint<LinearAxisServoFunctions, LinearAxisServoPrestepData, Vector<float>>(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()
+    {
+        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<float>(2), AngularFrequency = new Vector<float>(20 * MathF.PI) }
+        };
+
+        QuaternionWide.Broadcast(Quaternion.Identity, out var orientation);
+        var inertia = new BodyInertiaWide { InverseInertiaTensor = new Symmetric3x3Wide { XX = Vector<float>.One, YY = Vector<float>.One, ZZ = Vector<float>.One }, InverseMass = Vector<float>.One };
+        return BenchmarkTwoBodyConstraint<SwivelHingeFunctions, SwivelHingePrestepData, Vector4Wide>(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<float>(2), AngularFrequency = new Vector<float>(20 * MathF.PI) }
+        };
+
+        QuaternionWide.Broadcast(Quaternion.Identity, out var orientation);
+        var inertia = new BodyInertiaWide { InverseInertiaTensor = new Symmetric3x3Wide { XX = Vector<float>.One, YY = Vector<float>.One, ZZ = Vector<float>.One }, InverseMass = Vector<float>.One };
+        return BenchmarkTwoBodyConstraint<HingeFunctions, HingePrestepData, HingeAccumulatedImpulses>(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/TwoBodyConstraintBenchmarksDeep.cs b/DemoBenchmarks/TwoBodyConstraintBenchmarksDeep.cs
new file mode 100644
index 000000000..ce8966699
--- /dev/null
+++ b/DemoBenchmarks/TwoBodyConstraintBenchmarksDeep.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;
+
+/// <summary>
+/// Evaluates performance of all two body constraints excluded from <see cref="TwoBodyConstraintBenchmarks"/>.
+/// </summary>
+/// <remarks>
+/// Note that all constraints operate across <see cref="Vector{}.Count"/> lanes simultaneously where T is of type <see cref="float"/>.
+/// <para>The number of bundles being executed does not change if <see cref="Vector{}.Count"/> changes; if larger bundles are allowed, then more lanes end up getting solved.</para>
+/// </remarks>
+public class TwoBodyConstraintBenchmarksDeep
+{
+    [Benchmark]
+    public (BodyVelocityWide, BodyVelocityWide) Contact1()
+    {
+        var prestep = new Contact1PrestepData
+        {
+            Contact0 = new ConvexContactWide { Depth = Vector<float>.Zero, OffsetA = Vector3Wide.Broadcast(new Vector3(1, 0, 0)) },
+            MaterialProperties = new MaterialPropertiesWide
+            {
+                FrictionCoefficient = new Vector<float>(1f),
+                MaximumRecoveryVelocity = new Vector<float>(2f),
+                SpringSettings = new() { TwiceDampingRatio = new Vector<float>(2), AngularFrequency = new Vector<float>(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<float>.One, YY = Vector<float>.One, ZZ = Vector<float>.One }, InverseMass = Vector<float>.One };
+        return BenchmarkTwoBodyConstraint<Contact1Functions, Contact1PrestepData, Contact1AccumulatedImpulses>(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<float>.Zero, OffsetA = Vector3Wide.Broadcast(new Vector3(1, 0, 0)) },
+            Contact1 = new ConvexContactWide { Depth = Vector<float>.Zero, OffsetA = Vector3Wide.Broadcast(new Vector3(1, 0, 0)) },
+            MaterialProperties = new MaterialPropertiesWide
+            {
+                FrictionCoefficient = new Vector<float>(1f),
+                MaximumRecoveryVelocity = new Vector<float>(2f),
+                SpringSettings = new() { TwiceDampingRatio = new Vector<float>(2), AngularFrequency = new Vector<float>(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<float>.One, YY = Vector<float>.One, ZZ = Vector<float>.One }, InverseMass = Vector<float>.One };
+        return BenchmarkTwoBodyConstraint<Contact2Functions, Contact2PrestepData, Contact2AccumulatedImpulses>(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<float>.Zero, OffsetA = Vector3Wide.Broadcast(new Vector3(1, 0, 0)) },
+            Contact1 = new ConvexContactWide { Depth = Vector<float>.Zero, OffsetA = Vector3Wide.Broadcast(new Vector3(1, 0, 0)) },
+            Contact2 = new ConvexContactWide { Depth = Vector<float>.Zero, OffsetA = Vector3Wide.Broadcast(new Vector3(1, 0, 0)) },
+            MaterialProperties = new MaterialPropertiesWide
+            {
+                FrictionCoefficient = new Vector<float>(1f),
+                MaximumRecoveryVelocity = new Vector<float>(2f),
+                SpringSettings = new() { TwiceDampingRatio = new Vector<float>(2), AngularFrequency = new Vector<float>(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<float>.One, YY = Vector<float>.One, ZZ = Vector<float>.One }, InverseMass = Vector<float>.One };
+        return BenchmarkTwoBodyConstraint<Contact3Functions, Contact3PrestepData, Contact3AccumulatedImpulses>(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<float>.Zero, Offset = Vector3Wide.Broadcast(new Vector3(1, 0, 0)), Normal = Vector3Wide.Broadcast(new Vector3(0, 1, 0)) },
+            Contact1 = new NonconvexContactPrestepData { Depth = Vector<float>.Zero, Offset = Vector3Wide.Broadcast(new Vector3(1, 0, 1)), Normal = Vector3Wide.Broadcast(new Vector3(0, 1, 0)) },
+            MaterialProperties = new MaterialPropertiesWide
+            {
+                FrictionCoefficient = new Vector<float>(1f),
+                MaximumRecoveryVelocity = new Vector<float>(2f),
+                SpringSettings = new() { TwiceDampingRatio = new Vector<float>(2), AngularFrequency = new Vector<float>(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<float>.One, YY = Vector<float>.One, ZZ = Vector<float>.One }, InverseMass = Vector<float>.One };
+        return BenchmarkTwoBodyConstraint<ContactNonconvexTwoBodyFunctions<Contact2NonconvexPrestepData, Contact2NonconvexAccumulatedImpulses>,
+            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<float>.Zero, Offset = Vector3Wide.Broadcast(new Vector3(1, 0, 0)), Normal = Vector3Wide.Broadcast(new Vector3(0, 1, 0)) },
+            Contact1 = new NonconvexContactPrestepData { Depth = Vector<float>.Zero, Offset = Vector3Wide.Broadcast(new Vector3(1, 0, 1)), Normal = Vector3Wide.Broadcast(new Vector3(0, 1, 0)) },
+            Contact2 = new NonconvexContactPrestepData { Depth = Vector<float>.Zero, Offset = Vector3Wide.Broadcast(new Vector3(1, 1, 0)), Normal = Vector3Wide.Broadcast(new Vector3(0, 1, 0)) },
+            MaterialProperties = new MaterialPropertiesWide
+            {
+                FrictionCoefficient = new Vector<float>(1f),
+                MaximumRecoveryVelocity = new Vector<float>(2f),
+                SpringSettings = new() { TwiceDampingRatio = new Vector<float>(2), AngularFrequency = new Vector<float>(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<float>.One, YY = Vector<float>.One, ZZ = Vector<float>.One }, InverseMass = Vector<float>.One };
+        return BenchmarkTwoBodyConstraint<ContactNonconvexTwoBodyFunctions<Contact3NonconvexPrestepData, Contact3NonconvexAccumulatedImpulses>,
+            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<float>.Zero,
+            MaximumAngle = Vector<float>.One,
+            SpringSettings = new() { TwiceDampingRatio = new Vector<float>(2), AngularFrequency = new Vector<float>(20 * MathF.PI) }
+        };
+
+        var inertia = new BodyInertiaWide { InverseInertiaTensor = new Symmetric3x3Wide { XX = Vector<float>.One, YY = Vector<float>.One, ZZ = Vector<float>.One }, InverseMass = Vector<float>.One };
+        return BenchmarkTwoBodyConstraint<TwistLimitFunctions, TwistLimitPrestepData, Vector<float>>(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<float>.One, MaximumForce = new Vector<float>(float.MaxValue) }
+        };
+
+        var inertia = new BodyInertiaWide { InverseInertiaTensor = new Symmetric3x3Wide { XX = Vector<float>.One, YY = Vector<float>.One, ZZ = Vector<float>.One }, InverseMass = Vector<float>.One };
+        return BenchmarkTwoBodyConstraint<TwistMotorFunctions, TwistMotorPrestepData, Vector<float>>(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<float>.One, MaximumForce = new Vector<float>(float.MaxValue) }
+        };
+
+        var inertia = new BodyInertiaWide { InverseInertiaTensor = new Symmetric3x3Wide { XX = Vector<float>.One, YY = Vector<float>.One, ZZ = Vector<float>.One }, InverseMass = Vector<float>.One };
+        return BenchmarkTwoBodyConstraint<AngularMotorFunctions, AngularMotorPrestepData, Vector3Wide>(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<float>(0.5f),
+            MaximumDistance = new Vector<float>(1.5f),
+            SpringSettings = new() { TwiceDampingRatio = new Vector<float>(2), AngularFrequency = new Vector<float>(20 * MathF.PI) }
+        };
+
+        var inertia = new BodyInertiaWide { InverseInertiaTensor = new Symmetric3x3Wide { XX = Vector<float>.One, YY = Vector<float>.One, ZZ = Vector<float>.One }, InverseMass = Vector<float>.One };
+        return BenchmarkTwoBodyConstraint<DistanceLimitFunctions, DistanceLimitPrestepData, Vector<float>>(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<float>.One, MaximumForce = new Vector<float>(float.MaxValue) }
+        };
+
+        var inertia = new BodyInertiaWide { InverseInertiaTensor = new Symmetric3x3Wide { XX = Vector<float>.One, YY = Vector<float>.One, ZZ = Vector<float>.One }, InverseMass = Vector<float>.One };
+        return BenchmarkTwoBodyConstraint<LinearAxisMotorFunctions, LinearAxisMotorPrestepData, Vector<float>>(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<float>(3),
+            MinimumOffset = new Vector<float>(-3),
+            SpringSettings = new() { TwiceDampingRatio = new Vector<float>(2), AngularFrequency = new Vector<float>(20 * MathF.PI) }
+        };
+
+        var inertia = new BodyInertiaWide { InverseInertiaTensor = new Symmetric3x3Wide { XX = Vector<float>.One, YY = Vector<float>.One, ZZ = Vector<float>.One }, InverseMass = Vector<float>.One };
+        return BenchmarkTwoBodyConstraint<LinearAxisLimitFunctions, LinearAxisLimitPrestepData, Vector<float>>(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<float>.One, MaximumForce = new Vector<float>(float.MaxValue) }
+        };
+
+        var inertia = new BodyInertiaWide { InverseInertiaTensor = new Symmetric3x3Wide { XX = Vector<float>.One, YY = Vector<float>.One, ZZ = Vector<float>.One }, InverseMass = Vector<float>.One };
+        return BenchmarkTwoBodyConstraint<AngularAxisMotorFunctions, AngularAxisMotorPrestepData, Vector<float>>(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>(float.MaxValue), MaximumSpeed = new Vector<float>(float.MaxValue) },
+            SpringSettings = new() { TwiceDampingRatio = new Vector<float>(2), AngularFrequency = new Vector<float>(20 * MathF.PI) }
+        };
+
+        QuaternionWide.Broadcast(Quaternion.Identity, out var orientation);
+        var inertia = new BodyInertiaWide { InverseInertiaTensor = new Symmetric3x3Wide { XX = Vector<float>.One, YY = Vector<float>.One, ZZ = Vector<float>.One }, InverseMass = Vector<float>.One };
+        return BenchmarkTwoBodyConstraint<BallSocketServoFunctions, BallSocketServoPrestepData, Vector3Wide>(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<float>.One, MaximumForce = new Vector<float>(float.MaxValue) }
+        };
+
+        QuaternionWide.Broadcast(Quaternion.Identity, out var orientation);
+        var inertia = new BodyInertiaWide { InverseInertiaTensor = new Symmetric3x3Wide { XX = Vector<float>.One, YY = Vector<float>.One, ZZ = Vector<float>.One }, InverseMass = Vector<float>.One };
+        return BenchmarkTwoBodyConstraint<BallSocketMotorFunctions, BallSocketMotorPrestepData, Vector3Wide>(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<float>.One,
+            Settings = new() { Damping = Vector<float>.One, MaximumForce = new Vector<float>(float.MaxValue) }
+        };
+
+        QuaternionWide.Broadcast(Quaternion.Identity, out var orientation);
+        var inertia = new BodyInertiaWide { InverseInertiaTensor = new Symmetric3x3Wide { XX = Vector<float>.One, YY = Vector<float>.One, ZZ = Vector<float>.One }, InverseMass = Vector<float>.One };
+        return BenchmarkTwoBodyConstraint<AngularAxisGearMotorFunctions, AngularAxisGearMotorPrestepData, Vector<float>>(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<float>(0.2f),
+            MaximumDistance = new Vector<float>(3),
+            SpringSettings = new() { TwiceDampingRatio = new Vector<float>(2), AngularFrequency = new Vector<float>(20 * MathF.PI) }
+        };
+
+        QuaternionWide.Broadcast(Quaternion.Identity, out var orientation);
+        var inertia = new BodyInertiaWide { InverseInertiaTensor = new Symmetric3x3Wide { XX = Vector<float>.One, YY = Vector<float>.One, ZZ = Vector<float>.One }, InverseMass = Vector<float>.One };
+        return BenchmarkTwoBodyConstraint<CenterDistanceLimitFunctions, CenterDistanceLimitPrestepData, Vector<float>>(new Vector3Wide(), orientation, inertia, Vector3Wide.Broadcast(new Vector3(2, 0, 0)), orientation, inertia, prestep);
+    }
+
+}
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/DemoContentBuilder.csproj b/DemoContentBuilder/DemoContentBuilder.csproj
index bc3518ae2..bda6630ec 100644
--- a/DemoContentBuilder/DemoContentBuilder.csproj
+++ b/DemoContentBuilder/DemoContentBuilder.csproj
@@ -2,7 +2,7 @@
 
   <PropertyGroup>
     <OutputType>Exe</OutputType>
-    <TargetFramework>net5.0</TargetFramework>
+    <TargetFramework>net9.0</TargetFramework>
     <Platforms>x64</Platforms>
     <LangVersion>latest</LangVersion>
     <CopyLocalLockFileAssemblies>true</CopyLocalLockFileAssemblies>
@@ -14,7 +14,7 @@
     <PackageReference Include="SharpDX" Version="4.0.1" />
     <PackageReference Include="SharpDX.D3DCompiler" Version="4.0.1" />
     <PackageReference Include="SharpFontStandard" Version="4.0.1" />
-    <PackageReference Include="SixLabors.ImageSharp" Version="1.0.0-beta0007" />
+    <PackageReference Include="SixLabors.ImageSharp" Version="3.1.7" />
   </ItemGroup>
 
   <ItemGroup>
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 © <year> The FreeType
+    Project (www.freetype.org).  All rights reserved.
+   """
+
+  Please replace <year> 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/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/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<TriangleContent>();
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/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<string> shaderPaths,
             out List<ContentBuildInput> contentToBuild)
         {
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 dfa72ca2f..4b200bdd8 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
@@ -15,31 +9,25 @@ public static class Texture2DBuilder
     {
         public unsafe static Texture2DContent Build(Stream dataStream)
         {
-            using (var rawImage = SixLabors.ImageSharp.Image.Load(dataStream))
-            using (var image = rawImage.CloneAs<Rgba32>())
-            {
-                var pixels = image.GetPixelSpan();
-                var imageData = new int[pixels.Length];
-                fixed (int* imageDataPointer = imageData)
-                {
-                    var casted = new Span<Rgba32>(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<Rgba32, byte>(ref sourceRow[0]), (uint)(sizeof(Rgba32) * image.Width));
-                }
-                content.Unpin();
-                return content;
-            }
+            using var rawImage = SixLabors.ImageSharp.Image.Load(dataStream);
+            using var image = rawImage.CloneAs<Rgba32>();
+            //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.
+            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<Rgba32, byte>(ref sourceRow[0]), (uint)(sizeof(Rgba32) * image.Width));
+                }
+            });
+            content.Unpin();
+            return content;
         }
     }
 }
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<string, IContent> pathsToContent = new Dictionary<string, IContent>();
-
-        public T Load<T>(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.");
-        }
-
-        /// <summary>
-        /// Loads a content archive, previously saved using ContentArchive.Save, from a stream.
-        /// </summary>
-        /// <param name="stream">Stream to load from.</param>
-        /// <returns>Archive of loaded content.</returns>
-        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;
-        }
-
-        /// <summary>
-        /// Saves out a set of path-content pairs in a format loadable as a ContentArchive.
-        /// </summary>
-        /// <param name="pathsToContent">Path-content pairs to save.</param>
-        /// <param name="stream">Output stream to save to.</param>
-        public static void Save(Dictionary<string, IContent> 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<string, IContent> pathsToContent = new Dictionary<string, IContent>();
+
+        public T Load<T>(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.");
+        }
+
+        /// <summary>
+        /// Loads a content archive, previously saved using ContentArchive.Save, from a stream.
+        /// </summary>
+        /// <param name="stream">Stream to load from.</param>
+        /// <returns>Archive of loaded content.</returns>
+        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;
+        }
+
+        /// <summary>
+        /// Saves out a set of path-content pairs in a format loadable as a ContentArchive.
+        /// </summary>
+        /// <param name="pathsToContent">Path-content pairs to save.</param>
+        /// <param name="stream">Output stream to save to.</param>
+        public static void Save(Dictionary<string, IContent> 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);
+                }
+            }
+        }
+    }
+}
diff --git a/DemoContentLoader/DemoContentLoader.csproj b/DemoContentLoader/DemoContentLoader.csproj
index f544a3068..901c488a0 100644
--- a/DemoContentLoader/DemoContentLoader.csproj
+++ b/DemoContentLoader/DemoContentLoader.csproj
@@ -1,7 +1,7 @@
 <Project Sdk="Microsoft.NET.Sdk">
 
   <PropertyGroup>
-    <TargetFramework>net5.0</TargetFramework>
+    <TargetFramework>net9.0</TargetFramework>
     <LangVersion>latest</LangVersion>
     <AllowUnsafeBlocks>True</AllowUnsafeBlocks>
   </PropertyGroup>
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 09ce6289d..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
 {
@@ -30,7 +27,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/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.
         /// </summary>
         /// <param name="bufferData">Data to load into the buffer.</param>
-        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/DemoRenderer.csproj b/DemoRenderer.GL/DemoRenderer.csproj
old mode 100644
new mode 100755
index ecf92ad0a..2f25a39f9
--- a/DemoRenderer.GL/DemoRenderer.csproj
+++ b/DemoRenderer.GL/DemoRenderer.csproj
@@ -1,13 +1,13 @@
 <Project Sdk="Microsoft.NET.Sdk">
   <PropertyGroup>
-    <TargetFramework>net5.0</TargetFramework>
+    <TargetFramework>net9.0</TargetFramework>
     <LangVersion>latest</LangVersion>
     <AllowUnsafeBlocks>True</AllowUnsafeBlocks>
   </PropertyGroup>
 
   <Target Name="PreBuild" BeforeTargets="PreBuildEvent">
-    <Exec Condition="'$(Configuration)'=='Debug'" Command="dotnet run -p &quot;$(ProjectDir)../DemoContentBuilder&quot; -- &quot;$(ProjectDir)$(ProjectName).content&quot; -debug" />
-    <Exec Condition="'$(Configuration)'!='Debug'" Command="dotnet run -p &quot;$(ProjectDir)../DemoContentBuilder&quot; -- &quot;$(ProjectDir)$(ProjectName).content&quot;" />
+    <Exec Condition="'$(Configuration)'=='Debug'" Command="dotnet run --project &quot;$(ProjectDir)../DemoContentBuilder&quot; -- &quot;$(ProjectDir)$(ProjectName).content&quot; -debug" />
+    <Exec Condition="'$(Configuration)'!='Debug'" Command="dotnet run --project &quot;$(ProjectDir)../DemoContentBuilder&quot; -- &quot;$(ProjectDir)$(ProjectName).content&quot;" />
   </Target>
   
   <ItemGroup>
diff --git a/DemoRenderer.GL/Helpers.cs b/DemoRenderer.GL/Helpers.cs
index 4ead92849..a9edeef41 100644
--- a/DemoRenderer.GL/Helpers.cs
+++ b/DemoRenderer.GL/Helpers.cs
@@ -8,6 +8,7 @@ namespace DemoRenderer
 {
     public static class Helpers
     {
+
         /// <summary>
         /// Creates an index buffer of the specified size for screenspace quads.
         /// </summary>
@@ -41,7 +42,7 @@ public static uint[] GetQuadIndices(int quadCount)
         /// <summary>
         /// Creates an index buffer of the specified size for boxes.
         /// </summary>
-        /// <param name="boxCount">Number of boxes to create indices for.</param>
+        /// <param name="quadCount">Number of boxes to create indices for.</param>
         /// <returns>Index buffer for boxes.</returns> 
         /// <remarks>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.</remarks>
         public static uint[] GetBoxIndices(int boxCount)
@@ -106,7 +107,7 @@ public static uint[] GetBoxIndices(int boxCount)
         /// <param name="color">RGB color to pack.</param>
         /// <returns>Color packed into 32 bits.</returns>
         [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)
         /// <param name="color">RGBA color to pack.</param>
         /// <returns>Color packed into 32 bits.</returns>
         [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)
         /// <param name="source">Orientation to pack.</param>
         /// <param name="packed">W-less packed orientation, with remaining components negated to guarantee that the reconstructed positive W component is valid.</param>
         [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)
         /// <param name="source">Orientation to pack.</param>
         /// <param name="packed">Packed orientation.</param>
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public unsafe static ulong PackOrientationU64(ref 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<float, Vector4>(ref source.X);
             var clamped = Vector4.Max(new Vector4(-1), Vector4.Min(new Vector4(1), vectorSource));
-            ulong packed;
-            ref var packedShorts = ref Unsafe.As<ulong, ushort>(ref *&packed);
+            Unsafe.SkipInit(out ulong packed);
+            ref var packedShorts = ref Unsafe.As<ulong, ushort>(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));
@@ -213,10 +214,10 @@ public unsafe static ulong PackOrientationU64(ref 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<float, uint>(ref *&unpacked);
+            ref var reinterpreted = ref Unsafe.As<float, uint>(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<T>(ref T disposable) where T : IDisposable
+        {
+            if (disposable != null)
+                disposable.Dispose();
+            disposable = default(T);
+        }
     }
 }
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)
             {
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/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()
         {
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<Vector4> vert
 
         public bool Allocate(ulong id, int vertexCount, out int start, out Buffer<Vector4> 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<Vector
             }
             //Didn't fit. We need to resize.
             var copyCount = TriangleBuffer.Capacity + vertexCount;
-            var newSize = 1 << SpanHelper.GetContainingPowerOf2(copyCount);
-            Pool.ResizeToAtLeast(ref this.vertices, newSize, copyCount);
+            var newSize = (int)BitOperations.RoundUpToPowerOf2((uint)copyCount);
+            Pool.ResizeToAtLeast(ref this.vertices, newSize, 0);
             allocator.Capacity = newSize;
             allocator.Allocate(id, vertexCount, out longStart);
             start = (int)longStart;
diff --git a/DemoRenderer.GL/ShapeDrawing/ShapesExtractor.cs b/DemoRenderer.GL/ShapeDrawing/ShapesExtractor.cs
index 704a9f924..963648d51 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<SphereInstance> Spheres;
+        internal QuickList<CapsuleInstance> Capsules;
+        internal QuickList<CylinderInstance> Cylinders;
+        internal QuickList<BoxInstance> Boxes;
+        internal QuickList<TriangleInstance> Triangles;
+        internal QuickList<MeshInstance> Meshes;
+
+        public ShapeCache(int initialCapacityPerShapeType, BufferPool pool)
+        {
+            Spheres = new QuickList<SphereInstance>(initialCapacityPerShapeType, pool);
+            Capsules = new QuickList<CapsuleInstance>(initialCapacityPerShapeType, pool);
+            Cylinders = new QuickList<CylinderInstance>(initialCapacityPerShapeType, pool);
+            Boxes = new QuickList<BoxInstance>(initialCapacityPerShapeType, pool);
+            Triangles = new QuickList<TriangleInstance>(initialCapacityPerShapeType, pool);
+            Meshes = new QuickList<MeshInstance>(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<SphereInstance> spheres;
-        internal QuickList<CapsuleInstance> capsules;
-        internal QuickList<CylinderInstance> cylinders;
-        internal QuickList<BoxInstance> boxes;
-        internal QuickList<TriangleInstance> triangles;
-        internal QuickList<MeshInstance> 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<SphereInstance>(initialCapacityPerShapeType, pool);
-            capsules = new QuickList<CapsuleInstance>(initialCapacityPerShapeType, pool);
-            cylinders = new QuickList<CylinderInstance>(initialCapacityPerShapeType, pool);
-            boxes = new QuickList<BoxInstance>(initialCapacityPerShapeType, pool);
-            triangles = new QuickList<TriangleInstance>(initialCapacityPerShapeType, pool);
-            meshes = new QuickList<MeshInstance>(initialCapacityPerShapeType, pool);
+            ShapeCache = new ShapeCache(initialCapacityPerShapeType, pool);
             this.MeshCache = new MeshCache(pool);
             this.pool = pool;
             this.looper = looper;
@@ -38,26 +65,23 @@ 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<CompoundChild> children, Shapes shapes, in RigidPose pose, in Vector3 color)
+        private void AddCompoundChildren(ref Buffer<CompoundChild> 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);
+                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);
             }
         }
 
         [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 +94,7 @@ public unsafe void AddShape(void* shapeData, int shapeType, Shapes shapes, ref R
                         instance.Radius = Unsafe.AsRef<Sphere>(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 +104,9 @@ public unsafe void AddShape(void* shapeData, int shapeType, Shapes shapes, ref R
                         ref var capsule = ref Unsafe.AsRef<Capsule>(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 +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:
@@ -106,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:
@@ -120,9 +144,9 @@ public unsafe void AddShape(void* shapeData, int shapeType, Shapes shapes, ref R
                         ref var cylinder = ref Unsafe.AsRef<Cylinder>(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 +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;
-                        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<Vector4> 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 +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)
@@ -165,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<Compound>(shapeData).Children, shapes, pose, color);
+                        AddCompoundChildren(ref Unsafe.AsRef<Compound>(shapeData).Children, shapes, pose, color, ref shapeCache, pool);
                     }
                     break;
                 case BigCompound.Id:
                     {
-                        AddCompoundChildren(ref Unsafe.AsRef<BigCompound>(shapeData).Children, shapes, pose, color);
+                        AddCompoundChildren(ref Unsafe.AsRef<BigCompound>(shapeData).Children, shapes, pose, color, ref shapeCache, pool);
                     }
                     break;
                 case Mesh.Id:
@@ -184,11 +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;
-                        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<Vector4> 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 +242,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, 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>(TShape shape, Shapes shapes, ref RigidPose pose, in Vector3 color) where TShape : IShape
+        public unsafe void AddShape<TShape>(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), TShape.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,10 +290,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];
             Vector3 color;
             Helpers.UnpackColor((uint)HashHelper.Rehash(handle.Value), out Vector3 colorVariation);
-            if (Bodies.IsKinematic(inertia))
+            ref var state = ref set.DynamicsState[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);
@@ -265,11 +320,11 @@ 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, 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.
@@ -277,10 +332,142 @@ 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, 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<Job> jobs;
+        //The extractor can operate over one or multiple simulations. We cache them locally for threads to access.
+        Simulation[] simulations;
+        Simulation simulation;
+        Buffer<ShapeCache> workerCaches;
+
+        void PrepareForMultithreadedExecution(IThreadDispatcher threadDispatcher)
+        {
+            jobs = new QuickList<Job>(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.WorkerPools[i]);
+            }
+        }
+
+        void EndMultithreadedExecution()
+        {
+            jobs.Dispose(pool);
+            for (int i = 0; i < workerCaches.Length; ++i)
+            {
+                workerCaches[i].Dispose(looper.Dispatcher.WorkerPools[i]);
+            }
+            looper.Dispatcher = null;
+            pool.Return(ref workerCaches);
+        }
+
+        static void CreateJobs(Simulation simulation, int simulationIndex, ref QuickList<Job> 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.WorkerPools[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<TShape>(ref QuickList<TShape> workerCache, ref QuickList<TShape> 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)
             {
@@ -289,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/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/Constraints/AngularSwivelHingeLineExtractor.cs b/DemoRenderer/Constraints/AngularSwivelHingeLineExtractor.cs
index a8f5e1340..2ae16c0e4 100644
--- a/DemoRenderer/Constraints/AngularSwivelHingeLineExtractor.cs
+++ b/DemoRenderer/Constraints/AngularSwivelHingeLineExtractor.cs
@@ -8,14 +8,14 @@ namespace DemoRenderer.Constraints
 {
     struct AngularSwivelHingeLineExtractor : IConstraintLineExtractor<AngularSwivelHingePrestepData>
     {
-        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<LineInstance> 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].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 4f5ab7fdb..13d9781de 100644
--- a/DemoRenderer/Constraints/BallSocketLineExtractor.cs
+++ b/DemoRenderer/Constraints/BallSocketLineExtractor.cs
@@ -8,14 +8,14 @@ namespace DemoRenderer.Constraints
 {
     struct BallSocketLineExtractor : IConstraintLineExtractor<BallSocketPrestepData>
     {
-        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<LineInstance> lines)
         {
             //Could do bundles of constraints at a time, but eh.
-            var poseA = bodies.Sets[setIndex].Poses[bodyIndices[0]];
-            var poseB = bodies.Sets[setIndex].Poses[bodyIndices[1]];
+            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 b283de84c..e6a79c23c 100644
--- a/DemoRenderer/Constraints/BallSocketMotorLineExtractor.cs
+++ b/DemoRenderer/Constraints/BallSocketMotorLineExtractor.cs
@@ -8,14 +8,14 @@ namespace DemoRenderer.Constraints
 {
     struct BallSocketMotorLineExtractor : IConstraintLineExtractor<BallSocketMotorPrestepData>
     {
-        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<LineInstance> lines)
         {
             //Could do bundles of constraints at a time, but eh.
-            var poseA = bodies.Sets[setIndex].Poses[bodyIndices[0]];
-            var poseB = bodies.Sets[setIndex].Poses[bodyIndices[1]];
+            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 276c06158..9033faf74 100644
--- a/DemoRenderer/Constraints/BallSocketServoLineExtractor.cs
+++ b/DemoRenderer/Constraints/BallSocketServoLineExtractor.cs
@@ -8,14 +8,14 @@ namespace DemoRenderer.Constraints
 {
     struct BallSocketServoLineExtractor : IConstraintLineExtractor<BallSocketServoPrestepData>
     {
-        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<LineInstance> lines)
         {
             //Could do bundles of constraints at a time, but eh.
-            var poseA = bodies.Sets[setIndex].Poses[bodyIndices[0]];
-            var poseB = bodies.Sets[setIndex].Poses[bodyIndices[1]];
+            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/BoundingBoxLineExtractor.cs b/DemoRenderer/Constraints/BoundingBoxLineExtractor.cs
index 3991825dc..f3f96bf4e 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;
 
@@ -14,29 +10,17 @@ namespace DemoRenderer.Constraints
 {
     public class BoundingBoxLineExtractor
     {
-        const int jobsPerThread = 4;
-        QuickList<ThreadJob> jobs;
-        BroadPhase broadPhase;
-        int masterLinesCount;
-        Buffer<LineInstance> masterLinesSpan;
-
-        struct ThreadJob
+        internal struct ThreadJob
         {
+            public int SimulationIndex;
             public int LeafStart;
             public int LeafCount;
+            public int TargetLineStart;
             public bool CoversActiveCollidables;
         }
 
-        BufferPool pool;
-        Action<int> workDelegate;
-        public BoundingBoxLineExtractor(BufferPool pool)
-        {
-            this.pool = pool;
-            jobs = new QuickList<ThreadJob>(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)
+        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);
@@ -58,16 +42,14 @@ 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);
         }
-        private unsafe void Work(int jobIndex)
+        internal unsafe void ExecuteJob(Buffer<LineInstance> 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 +66,17 @@ private unsafe void Work(int jobIndex)
 
                 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<ThreadJob> jobs)
+        void CreateJobsForTree(in Tree tree, bool active, int simulationIndex, ref QuickList<ThreadJob> 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 +87,30 @@ void CreateJobsForTree(in Tree tree, bool active, ref QuickList<ThreadJob> 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<LineInstance> lines, ParallelLooper looper, BufferPool pool)
+        internal void CreateJobs(Simulation simulation, int simulationIndex, ref QuickList<LineInstance> lines, ref QuickList<ThreadJob> 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/CenterDistanceLimitLineExtractor.cs b/DemoRenderer/Constraints/CenterDistanceLimitLineExtractor.cs
new file mode 100644
index 000000000..8946c2ee1
--- /dev/null
+++ b/DemoRenderer/Constraints/CenterDistanceLimitLineExtractor.cs
@@ -0,0 +1,60 @@
+using BepuUtilities.Collections;
+using BepuPhysics;
+using BepuPhysics.Constraints;
+using System.Numerics;
+using BepuUtilities;
+
+namespace DemoRenderer.Constraints
+{
+    struct CenterDistanceLimitLineExtractor : IConstraintLineExtractor<CenterDistanceLimitPrestepData>
+    {
+        public static int LinesPerConstraint => 5;
+
+        public static unsafe void ExtractLines(ref CenterDistanceLimitPrestepData prestepBundle, int setIndex, int* bodyIndices,
+            Bodies bodies, ref Vector3 tint, ref QuickList<LineInstance> lines)
+        {
+            //Could do bundles of constraints at a time, but eh.
+            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;
+            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/CenterDistanceLineExtractor.cs b/DemoRenderer/Constraints/CenterDistanceLineExtractor.cs
index 2ee8d7662..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;
@@ -9,14 +8,14 @@ namespace DemoRenderer.Constraints
 {
     struct CenterDistanceLineExtractor : IConstraintLineExtractor<CenterDistancePrestepData>
     {
-        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<LineInstance> lines)
         {
             //Could do bundles of constraints at a time, but eh.
-            var poseA = bodies.Sets[setIndex].Poses[bodyIndices[0]];
-            var poseB = bodies.Sets[setIndex].Poses[bodyIndices[1]];
+            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/ConstraintLineExtractor.cs b/DemoRenderer/Constraints/ConstraintLineExtractor.cs
index af8b8af62..6e5c94c4d 100644
--- a/DemoRenderer/Constraints/ConstraintLineExtractor.cs
+++ b/DemoRenderer/Constraints/ConstraintLineExtractor.cs
@@ -4,7 +4,6 @@
 using BepuPhysics;
 using BepuPhysics.Constraints;
 using System.Runtime.CompilerServices;
-using System.Threading.Tasks;
 using System.Diagnostics;
 using BepuUtilities;
 using System.Numerics;
@@ -15,9 +14,9 @@ namespace DemoRenderer.Constraints
 {
     unsafe interface IConstraintLineExtractor<TPrestep>
     {
-        int LinesPerConstraint { get; }
+        static abstract int LinesPerConstraint { get; }
 
-        void ExtractLines(ref TPrestep prestepBundle, int setIndex, int* bodyLocations, Bodies bodies, ref Vector3 tint, ref QuickList<LineInstance> lines);
+        static abstract void ExtractLines(ref TPrestep prestepBundle, int setIndex, int* bodyLocations, Bodies bodies, ref Vector3 tint, ref QuickList<LineInstance> lines);
     }
     abstract class TypeLineExtractor
     {
@@ -25,12 +24,12 @@ abstract class TypeLineExtractor
         public abstract void ExtractLines(Bodies bodies, int setIndex, ref TypeBatch typeBatch, int constraintStart, int constraintCount, ref QuickList<LineInstance> lines);
     }
 
-    class TypeLineExtractor<T, TBodyReferences, TPrestep, TProjection, TAccumulatedImpulses> : TypeLineExtractor
+    class TypeLineExtractor<T, TBodyReferences, TPrestep, TAccumulatedImpulses> : TypeLineExtractor
         where TBodyReferences : unmanaged
         where TPrestep : unmanaged
         where T : struct, IConstraintLineExtractor<TPrestep>
     {
-        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<LineInstance> lines)
         {
@@ -41,7 +40,6 @@ public unsafe override void ExtractLines(Bodies bodies, int setIndex, ref TypeBa
             var bodyCount = Unsafe.SizeOf<TBodyReferences>() / Unsafe.SizeOf<Vector<int>>();
             Debug.Assert(bodyCount * Unsafe.SizeOf<Vector<int>>() == Unsafe.SizeOf<TBodyReferences>());
             var bodyIndices = stackalloc int[bodyCount];
-            var extractor = default(T);
 
             var constraintEnd = constraintStart + constraintCount;
             if (setIndex == 0)
@@ -49,16 +47,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<TBodyReferences, Vector<int>>(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<TBodyReferences, Vector<int>>(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) & Bodies.BodyReferenceMask;
+                        }
+                        T.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,45 +67,46 @@ 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<TBodyReferences, Vector<int>>(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<TBodyReferences, Vector<int>>(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) & Bodies.BodyReferenceMask;
+                            Debug.Assert(bodies.HandleToLocation[bodyHandle].SetIndex == setIndex);
+                            bodyIndices[j] = bodies.HandleToLocation[bodyHandle].Index & Bodies.BodyReferenceMask;
+                        }
+                        T.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);
                 }
             }
         }
     }
 
-    internal class ConstraintLineExtractor : IDisposable
+    internal class ConstraintLineExtractor
     {
         TypeLineExtractor[] lineExtractors;
-        const int jobsPerThread = 4;
-        QuickList<ThreadJob> 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<LineInstance> jobLines;
         }
 
         public bool Enabled { get; set; } = true;
 
-        Action<int> executeJobDelegate;
 
         ref TypeLineExtractor AllocateSlot(int typeId)
         {
@@ -112,70 +114,65 @@ 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<BallSocketLineExtractor, TwoBodyReferences, BallSocketPrestepData, BallSocketProjection, Vector3Wide>();
-            AllocateSlot(BallSocketServoTypeProcessor.BatchTypeId) = new TypeLineExtractor<BallSocketServoLineExtractor, TwoBodyReferences, BallSocketServoPrestepData, BallSocketServoProjection, Vector3Wide>();
-            AllocateSlot(BallSocketMotorTypeProcessor.BatchTypeId) = new TypeLineExtractor<BallSocketMotorLineExtractor, TwoBodyReferences, BallSocketMotorPrestepData, BallSocketMotorProjection, Vector3Wide>();
-            AllocateSlot(WeldTypeProcessor.BatchTypeId) = new TypeLineExtractor<WeldLineExtractor, TwoBodyReferences, WeldPrestepData, WeldProjection, WeldAccumulatedImpulses>();
-            AllocateSlot(DistanceServoTypeProcessor.BatchTypeId) = new TypeLineExtractor<DistanceServoLineExtractor, TwoBodyReferences, DistanceServoPrestepData, DistanceServoProjection, Vector<float>>();
-            AllocateSlot(DistanceLimitTypeProcessor.BatchTypeId) = new TypeLineExtractor<DistanceLimitLineExtractor, TwoBodyReferences, DistanceLimitPrestepData, DistanceLimitProjection, Vector<float>>();
-            AllocateSlot(CenterDistanceTypeProcessor.BatchTypeId) = new TypeLineExtractor<CenterDistanceLineExtractor, TwoBodyReferences, CenterDistancePrestepData, CenterDistanceProjection, Vector<float>>();
-            AllocateSlot(PointOnLineServoTypeProcessor.BatchTypeId) = new TypeLineExtractor<PointOnLineLineExtractor, TwoBodyReferences, PointOnLineServoPrestepData, PointOnLineServoProjection, Vector2Wide>();
-            AllocateSlot(LinearAxisServoTypeProcessor.BatchTypeId) = new TypeLineExtractor<LinearAxisServoLineExtractor, TwoBodyReferences, LinearAxisServoPrestepData, LinearAxisServoProjection, Vector<float>>();
-            AllocateSlot(AngularSwivelHingeTypeProcessor.BatchTypeId) = new TypeLineExtractor<AngularSwivelHingeLineExtractor, TwoBodyReferences, AngularSwivelHingePrestepData, AngularSwivelHingeProjection, Vector<float>>();
-            AllocateSlot(SwivelHingeTypeProcessor.BatchTypeId) = new TypeLineExtractor<SwivelHingeLineExtractor, TwoBodyReferences, SwivelHingePrestepData, SwivelHingeProjection, Vector4Wide>();
-            AllocateSlot(HingeTypeProcessor.BatchTypeId) = new TypeLineExtractor<HingeLineExtractor, TwoBodyReferences, HingePrestepData, HingeProjection, HingeAccumulatedImpulses>();
-            AllocateSlot(OneBodyLinearServoTypeProcessor.BatchTypeId) = new TypeLineExtractor<OneBodyLinearServoLineExtractor, Vector<int>, OneBodyLinearServoPrestepData, OneBodyLinearServoProjection, Vector<float>>();
-
-            AllocateSlot(Contact1OneBodyTypeProcessor.BatchTypeId) = new TypeLineExtractor<Contact1OneBodyLineExtractor, Vector<int>, Contact1OneBodyPrestepData, Contact1OneBodyProjection, Contact1AccumulatedImpulses>();
-            AllocateSlot(Contact2OneBodyTypeProcessor.BatchTypeId) = new TypeLineExtractor<Contact2OneBodyLineExtractor, Vector<int>, Contact2OneBodyPrestepData, Contact2OneBodyProjection, Contact2AccumulatedImpulses>();
-            AllocateSlot(Contact3OneBodyTypeProcessor.BatchTypeId) = new TypeLineExtractor<Contact3OneBodyLineExtractor, Vector<int>, Contact3OneBodyPrestepData, Contact3OneBodyProjection, Contact3AccumulatedImpulses>();
-            AllocateSlot(Contact4OneBodyTypeProcessor.BatchTypeId) = new TypeLineExtractor<Contact4OneBodyLineExtractor, Vector<int>, Contact4OneBodyPrestepData, Contact4OneBodyProjection, Contact4AccumulatedImpulses>();
-
-            AllocateSlot(Contact1TypeProcessor.BatchTypeId) = new TypeLineExtractor<Contact1LineExtractor, TwoBodyReferences, Contact1PrestepData, Contact1Projection, Contact1AccumulatedImpulses>();
-            AllocateSlot(Contact2TypeProcessor.BatchTypeId) = new TypeLineExtractor<Contact2LineExtractor, TwoBodyReferences, Contact2PrestepData, Contact2Projection, Contact2AccumulatedImpulses>();
-            AllocateSlot(Contact3TypeProcessor.BatchTypeId) = new TypeLineExtractor<Contact3LineExtractor, TwoBodyReferences, Contact3PrestepData, Contact3Projection, Contact3AccumulatedImpulses>();
-            AllocateSlot(Contact4TypeProcessor.BatchTypeId) = new TypeLineExtractor<Contact4LineExtractor, TwoBodyReferences, Contact4PrestepData, Contact4Projection, Contact4AccumulatedImpulses>();
-
-            AllocateSlot(Contact2NonconvexOneBodyTypeProcessor.BatchTypeId) = new TypeLineExtractor<Contact2NonconvexOneBodyLineExtractor, Vector<int>, Contact2NonconvexOneBodyPrestepData, Contact2NonconvexOneBodyProjection, Contact2NonconvexAccumulatedImpulses>();
-            AllocateSlot(Contact3NonconvexOneBodyTypeProcessor.BatchTypeId) = new TypeLineExtractor<Contact3NonconvexOneBodyLineExtractor, Vector<int>, Contact3NonconvexOneBodyPrestepData, Contact3NonconvexOneBodyProjection, Contact3NonconvexAccumulatedImpulses>();
-            AllocateSlot(Contact4NonconvexOneBodyTypeProcessor.BatchTypeId) = new TypeLineExtractor<Contact4NonconvexOneBodyLineExtractor, Vector<int>, Contact4NonconvexOneBodyPrestepData, Contact4NonconvexOneBodyProjection, Contact4NonconvexAccumulatedImpulses>();
-            //AllocateSlot(Contact5NonconvexOneBodyTypeProcessor.BatchTypeId) = new TypeLineExtractor<Contact5NonconvexOneBodyLineExtractor, Vector<int>, Contact5NonconvexOneBodyPrestepData, Contact5NonconvexOneBodyProjection, Contact5NonconvexAccumulatedImpulses>();
-            //AllocateSlot(Contact6NonconvexOneBodyTypeProcessor.BatchTypeId) = new TypeLineExtractor<Contact6NonconvexOneBodyLineExtractor, Vector<int>, Contact6NonconvexOneBodyPrestepData, Contact6NonconvexOneBodyProjection, Contact6NonconvexAccumulatedImpulses>();
-            //AllocateSlot(Contact7NonconvexOneBodyTypeProcessor.BatchTypeId) = new TypeLineExtractor<Contact7NonconvexOneBodyLineExtractor, Vector<int>, Contact7NonconvexOneBodyPrestepData, Contact7NonconvexOneBodyProjection, Contact7NonconvexAccumulatedImpulses>();
-            //AllocateSlot(Contact8NonconvexOneBodyTypeProcessor.BatchTypeId) = new TypeLineExtractor<Contact8NonconvexOneBodyLineExtractor, Vector<int>, Contact8NonconvexOneBodyPrestepData, Contact8NonconvexOneBodyProjection, Contact8NonconvexAccumulatedImpulses>();
-
-            AllocateSlot(Contact2NonconvexTypeProcessor.BatchTypeId) = new TypeLineExtractor<Contact2NonconvexLineExtractor, TwoBodyReferences, Contact2NonconvexPrestepData, Contact2NonconvexProjection, Contact2NonconvexAccumulatedImpulses>();
-            AllocateSlot(Contact3NonconvexTypeProcessor.BatchTypeId) = new TypeLineExtractor<Contact3NonconvexLineExtractor, TwoBodyReferences, Contact3NonconvexPrestepData, Contact3NonconvexProjection, Contact3NonconvexAccumulatedImpulses>();
-            AllocateSlot(Contact4NonconvexTypeProcessor.BatchTypeId) = new TypeLineExtractor<Contact4NonconvexLineExtractor, TwoBodyReferences, Contact4NonconvexPrestepData, Contact4NonconvexProjection, Contact4NonconvexAccumulatedImpulses>();
-            //AllocateSlot(Contact5NonconvexTypeProcessor.BatchTypeId) = new TypeLineExtractor<Contact5NonconvexLineExtractor, TwoBodyReferences, Contact5NonconvexPrestepData, Contact5NonconvexProjection, Contact5NonconvexAccumulatedImpulses>();
-            //AllocateSlot(Contact6NonconvexTypeProcessor.BatchTypeId) = new TypeLineExtractor<Contact6NonconvexLineExtractor, TwoBodyReferences, Contact6NonconvexPrestepData, Contact6NonconvexProjection, Contact6NonconvexAccumulatedImpulses>();
-            //AllocateSlot(Contact7NonconvexTypeProcessor.BatchTypeId) = new TypeLineExtractor<Contact7NonconvexLineExtractor, TwoBodyReferences, Contact7NonconvexPrestepData, Contact7NonconvexProjection, Contact7NonconvexAccumulatedImpulses>();
-            //AllocateSlot(Contact8NonconvexTypeProcessor.BatchTypeId) = new TypeLineExtractor<Contact8NonconvexLineExtractor, TwoBodyReferences, Contact8NonconvexPrestepData, Contact8NonconvexProjection, Contact8NonconvexAccumulatedImpulses>();
-
-            jobs = new QuickList<ThreadJob>(Environment.ProcessorCount * (jobsPerThread + 1), pool);
-
-            executeJobDelegate = ExecuteJob;
+            AllocateSlot(BallSocketTypeProcessor.BatchTypeId) = new TypeLineExtractor<BallSocketLineExtractor, TwoBodyReferences, BallSocketPrestepData, Vector3Wide>();
+            AllocateSlot(BallSocketServoTypeProcessor.BatchTypeId) = new TypeLineExtractor<BallSocketServoLineExtractor, TwoBodyReferences, BallSocketServoPrestepData, Vector3Wide>();
+            AllocateSlot(BallSocketMotorTypeProcessor.BatchTypeId) = new TypeLineExtractor<BallSocketMotorLineExtractor, TwoBodyReferences, BallSocketMotorPrestepData, Vector3Wide>();
+            AllocateSlot(WeldTypeProcessor.BatchTypeId) = new TypeLineExtractor<WeldLineExtractor, TwoBodyReferences, WeldPrestepData, WeldAccumulatedImpulses>();
+            AllocateSlot(DistanceServoTypeProcessor.BatchTypeId) = new TypeLineExtractor<DistanceServoLineExtractor, TwoBodyReferences, DistanceServoPrestepData, Vector<float>>();
+            AllocateSlot(DistanceLimitTypeProcessor.BatchTypeId) = new TypeLineExtractor<DistanceLimitLineExtractor, TwoBodyReferences, DistanceLimitPrestepData, Vector<float>>();
+            AllocateSlot(CenterDistanceTypeProcessor.BatchTypeId) = new TypeLineExtractor<CenterDistanceLineExtractor, TwoBodyReferences, CenterDistancePrestepData, Vector<float>>();
+            AllocateSlot(CenterDistanceLimitTypeProcessor.BatchTypeId) = new TypeLineExtractor<CenterDistanceLimitLineExtractor, TwoBodyReferences, CenterDistanceLimitPrestepData, Vector<float>>();
+            AllocateSlot(PointOnLineServoTypeProcessor.BatchTypeId) = new TypeLineExtractor<PointOnLineLineExtractor, TwoBodyReferences, PointOnLineServoPrestepData, Vector2Wide>();
+            AllocateSlot(LinearAxisServoTypeProcessor.BatchTypeId) = new TypeLineExtractor<LinearAxisServoLineExtractor, TwoBodyReferences, LinearAxisServoPrestepData, Vector<float>>();
+            AllocateSlot(AngularSwivelHingeTypeProcessor.BatchTypeId) = new TypeLineExtractor<AngularSwivelHingeLineExtractor, TwoBodyReferences, AngularSwivelHingePrestepData, Vector<float>>();
+            AllocateSlot(SwivelHingeTypeProcessor.BatchTypeId) = new TypeLineExtractor<SwivelHingeLineExtractor, TwoBodyReferences, SwivelHingePrestepData, Vector4Wide>();
+            AllocateSlot(HingeTypeProcessor.BatchTypeId) = new TypeLineExtractor<HingeLineExtractor, TwoBodyReferences, HingePrestepData, HingeAccumulatedImpulses>();
+            AllocateSlot(OneBodyLinearServoTypeProcessor.BatchTypeId) = new TypeLineExtractor<OneBodyLinearServoLineExtractor, Vector<int>, OneBodyLinearServoPrestepData, Vector<float>>();
+
+            AllocateSlot(Contact1OneBodyTypeProcessor.BatchTypeId) = new TypeLineExtractor<Contact1OneBodyLineExtractor, Vector<int>, Contact1OneBodyPrestepData, Contact1AccumulatedImpulses>();
+            AllocateSlot(Contact2OneBodyTypeProcessor.BatchTypeId) = new TypeLineExtractor<Contact2OneBodyLineExtractor, Vector<int>, Contact2OneBodyPrestepData, Contact2AccumulatedImpulses>();
+            AllocateSlot(Contact3OneBodyTypeProcessor.BatchTypeId) = new TypeLineExtractor<Contact3OneBodyLineExtractor, Vector<int>, Contact3OneBodyPrestepData, Contact3AccumulatedImpulses>();
+            AllocateSlot(Contact4OneBodyTypeProcessor.BatchTypeId) = new TypeLineExtractor<Contact4OneBodyLineExtractor, Vector<int>, Contact4OneBodyPrestepData, Contact4AccumulatedImpulses>();
+
+            AllocateSlot(Contact1TypeProcessor.BatchTypeId) = new TypeLineExtractor<Contact1LineExtractor, TwoBodyReferences, Contact1PrestepData, Contact1AccumulatedImpulses>();
+            AllocateSlot(Contact2TypeProcessor.BatchTypeId) = new TypeLineExtractor<Contact2LineExtractor, TwoBodyReferences, Contact2PrestepData, Contact2AccumulatedImpulses>();
+            AllocateSlot(Contact3TypeProcessor.BatchTypeId) = new TypeLineExtractor<Contact3LineExtractor, TwoBodyReferences, Contact3PrestepData, Contact3AccumulatedImpulses>();
+            AllocateSlot(Contact4TypeProcessor.BatchTypeId) = new TypeLineExtractor<Contact4LineExtractor, TwoBodyReferences, Contact4PrestepData, Contact4AccumulatedImpulses>();
+
+            AllocateSlot(Contact2NonconvexOneBodyTypeProcessor.BatchTypeId) = new TypeLineExtractor<Contact2NonconvexOneBodyLineExtractor, Vector<int>, Contact2NonconvexOneBodyPrestepData, Contact2NonconvexAccumulatedImpulses>();
+            AllocateSlot(Contact3NonconvexOneBodyTypeProcessor.BatchTypeId) = new TypeLineExtractor<Contact3NonconvexOneBodyLineExtractor, Vector<int>, Contact3NonconvexOneBodyPrestepData, Contact3NonconvexAccumulatedImpulses>();
+            AllocateSlot(Contact4NonconvexOneBodyTypeProcessor.BatchTypeId) = new TypeLineExtractor<Contact4NonconvexOneBodyLineExtractor, Vector<int>, Contact4NonconvexOneBodyPrestepData, Contact4NonconvexAccumulatedImpulses>();
+            //AllocateSlot(Contact5NonconvexOneBodyTypeProcessor.BatchTypeId) = new TypeLineExtractor<Contact5NonconvexOneBodyLineExtractor, Vector<int>, Contact5NonconvexOneBodyPrestepData, Contact5NonconvexAccumulatedImpulses>();
+            //AllocateSlot(Contact6NonconvexOneBodyTypeProcessor.BatchTypeId) = new TypeLineExtractor<Contact6NonconvexOneBodyLineExtractor, Vector<int>, Contact6NonconvexOneBodyPrestepData, Contact6NonconvexAccumulatedImpulses>();
+            //AllocateSlot(Contact7NonconvexOneBodyTypeProcessor.BatchTypeId) = new TypeLineExtractor<Contact7NonconvexOneBodyLineExtractor, Vector<int>, Contact7NonconvexOneBodyPrestepData, Contact7NonconvexAccumulatedImpulses>();
+            //AllocateSlot(Contact8NonconvexOneBodyTypeProcessor.BatchTypeId) = new TypeLineExtractor<Contact8NonconvexOneBodyLineExtractor, Vector<int>, Contact8NonconvexOneBodyPrestepData, Contact8NonconvexAccumulatedImpulses>();
+
+            AllocateSlot(Contact2NonconvexTypeProcessor.BatchTypeId) = new TypeLineExtractor<Contact2NonconvexLineExtractor, TwoBodyReferences, Contact2NonconvexPrestepData, Contact2NonconvexAccumulatedImpulses>();
+            AllocateSlot(Contact3NonconvexTypeProcessor.BatchTypeId) = new TypeLineExtractor<Contact3NonconvexLineExtractor, TwoBodyReferences, Contact3NonconvexPrestepData, Contact3NonconvexAccumulatedImpulses>();
+            AllocateSlot(Contact4NonconvexTypeProcessor.BatchTypeId) = new TypeLineExtractor<Contact4NonconvexLineExtractor, TwoBodyReferences, Contact4NonconvexPrestepData, Contact4NonconvexAccumulatedImpulses>();
+            //AllocateSlot(Contact5NonconvexTypeProcessor.BatchTypeId) = new TypeLineExtractor<Contact5NonconvexLineExtractor, TwoBodyReferences, Contact5NonconvexPrestepData, Contact5NonconvexAccumulatedImpulses>();
+            //AllocateSlot(Contact6NonconvexTypeProcessor.BatchTypeId) = new TypeLineExtractor<Contact6NonconvexLineExtractor, TwoBodyReferences, Contact6NonconvexPrestepData, Contact6NonconvexAccumulatedImpulses>();
+            //AllocateSlot(Contact7NonconvexTypeProcessor.BatchTypeId) = new TypeLineExtractor<Contact7NonconvexLineExtractor, TwoBodyReferences, Contact7NonconvexPrestepData, Contact7NonconvexAccumulatedImpulses>();
+            //AllocateSlot(Contact8NonconvexTypeProcessor.BatchTypeId) = new TypeLineExtractor<Contact8NonconvexLineExtractor, TwoBodyReferences, Contact8NonconvexPrestepData, Contact8NonconvexAccumulatedImpulses>();
         }
 
-        Bodies bodies;
-        Solver solver;
-        private void ExecuteJob(int jobIndex)
+        internal void ExecuteJob(Buffer<LineInstance> 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);
+            var targetLines = new QuickList<LineInstance>(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<LineInstance> lines, ParallelLooper looper)
+        internal void CreateJobs(Simulation simulation, int simulationIndex, bool showConstraints, bool showContacts, ref QuickList<LineInstance> lines, ref QuickList<ThreadJob> 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];
@@ -188,76 +185,36 @@ 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);
-                                neededLineCapacity += extractor.LinesPerConstraint * typeBatch.ConstraintCount;
+                                var linesForTypeBatch = extractor.LinesPerConstraint * constraintCount;
+                                neededLineCapacity += linesForTypeBatch;
+                                newLineCount += linesForTypeBatch;
                             }
                         }
                     }
                 }
             }
-            var maximumJobSize = 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);
-            lines.Count = neededLineCapacity; //Line additions will be performed on suballocated lists. This count will be used by the renderer when reading line data.
-            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<LineInstance>(lines.Span);
-                //By setting the count, we work around the fact that Array<T> doesn't support slicing.
-                jobs[i].jobLines.Count = jobs[i].LineStart;
-            }
-            this.bodies = bodies;
-            this.solver = solver;
-            looper.For(0, jobs.Count, executeJobDelegate);
-            this.bodies = null;
-            this.solver = solver;
+            if (newLineCount > 0)
+                lines.Allocate(newLineCount, pool);
         }
 
-        public void Dispose()
-        {
-            jobs.Dispose(pool);
-        }
     }
 }
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<float> depthWide,
-            in Vector3 tint, ref QuickList<LineInstance> lines)
+            Vector3 tint, ref QuickList<LineInstance> lines)
         {
             Vector3Wide.ReadFirst(offsetAWide, out var offsetA);
             Vector3Wide.ReadFirst(normalWide, out var normal);
diff --git a/DemoRenderer/Constraints/ContactLineExtractors.cs b/DemoRenderer/Constraints/ContactLineExtractors.cs
index 0b614bd34..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;
 
@@ -9,35 +6,35 @@ namespace BepuPhysics.Constraints.Contact
 {  
     struct Contact1OneBodyLineExtractor : IConstraintLineExtractor<Contact1OneBodyPrestepData>
     {
-        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<LineInstance> lines)
         {
-            var poseA = bodies.Sets[setIndex].Poses[bodyIndices[0]];
+            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);
         }
     }
     struct Contact2OneBodyLineExtractor : IConstraintLineExtractor<Contact2OneBodyPrestepData>
     {
-        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<LineInstance> lines)
         {
-            var poseA = bodies.Sets[setIndex].Poses[bodyIndices[0]];
+            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);
         }
     }
     struct Contact3OneBodyLineExtractor : IConstraintLineExtractor<Contact3OneBodyPrestepData>
     {
-        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<LineInstance> lines)
         {
-            var poseA = bodies.Sets[setIndex].Poses[bodyIndices[0]];
+            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);
@@ -45,12 +42,12 @@ public unsafe void ExtractLines(ref Contact3OneBodyPrestepData prestepBundle, in
     }
     struct Contact4OneBodyLineExtractor : IConstraintLineExtractor<Contact4OneBodyPrestepData>
     {
-        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<LineInstance> lines)
         {
-            var poseA = bodies.Sets[setIndex].Poses[bodyIndices[0]];
+            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);
@@ -59,35 +56,35 @@ public unsafe void ExtractLines(ref Contact4OneBodyPrestepData prestepBundle, in
     }
     struct Contact1LineExtractor : IConstraintLineExtractor<Contact1PrestepData>
     {
-        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<LineInstance> lines)
         {
-            var poseA = bodies.Sets[setIndex].Poses[bodyIndices[0]];
+            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);
         }
     }
     struct Contact2LineExtractor : IConstraintLineExtractor<Contact2PrestepData>
     {
-        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<LineInstance> lines)
         {
-            var poseA = bodies.Sets[setIndex].Poses[bodyIndices[0]];
+            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);
         }
     }
     struct Contact3LineExtractor : IConstraintLineExtractor<Contact3PrestepData>
     {
-        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<LineInstance> lines)
         {
-            var poseA = bodies.Sets[setIndex].Poses[bodyIndices[0]];
+            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);
@@ -95,12 +92,12 @@ public unsafe void ExtractLines(ref Contact3PrestepData prestepBundle, int setIn
     }
     struct Contact4LineExtractor : IConstraintLineExtractor<Contact4PrestepData>
     {
-        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<LineInstance> lines)
         {
-            var poseA = bodies.Sets[setIndex].Poses[bodyIndices[0]];
+            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);
@@ -109,24 +106,24 @@ public unsafe void ExtractLines(ref Contact4PrestepData prestepBundle, int setIn
     }
     struct Contact2NonconvexOneBodyLineExtractor : IConstraintLineExtractor<Contact2NonconvexOneBodyPrestepData>
     {
-        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<LineInstance> lines)
         {
-            var poseA = bodies.Sets[setIndex].Poses[bodyIndices[0]];
+            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);
         }
     }
     struct Contact3NonconvexOneBodyLineExtractor : IConstraintLineExtractor<Contact3NonconvexOneBodyPrestepData>
     {
-        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<LineInstance> lines)
         {
-            var poseA = bodies.Sets[setIndex].Poses[bodyIndices[0]];
+            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);
@@ -134,12 +131,12 @@ public unsafe void ExtractLines(ref Contact3NonconvexOneBodyPrestepData prestepB
     }
     struct Contact4NonconvexOneBodyLineExtractor : IConstraintLineExtractor<Contact4NonconvexOneBodyPrestepData>
     {
-        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<LineInstance> lines)
         {
-            var poseA = bodies.Sets[setIndex].Poses[bodyIndices[0]];
+            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);
@@ -148,24 +145,24 @@ public unsafe void ExtractLines(ref Contact4NonconvexOneBodyPrestepData prestepB
     }
     struct Contact2NonconvexLineExtractor : IConstraintLineExtractor<Contact2NonconvexPrestepData>
     {
-        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<LineInstance> lines)
         {
-            var poseA = bodies.Sets[setIndex].Poses[bodyIndices[0]];
+            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);
         }
     }
     struct Contact3NonconvexLineExtractor : IConstraintLineExtractor<Contact3NonconvexPrestepData>
     {
-        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<LineInstance> lines)
         {
-            var poseA = bodies.Sets[setIndex].Poses[bodyIndices[0]];
+            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);
@@ -173,12 +170,12 @@ public unsafe void ExtractLines(ref Contact3NonconvexPrestepData prestepBundle,
     }
     struct Contact4NonconvexLineExtractor : IConstraintLineExtractor<Contact4NonconvexPrestepData>
     {
-        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<LineInstance> lines)
         {
-            var poseA = bodies.Sets[setIndex].Poses[bodyIndices[0]];
+            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/ContactLineExtractors.tt b/DemoRenderer/Constraints/ContactLineExtractors.tt
index 1bfaa38ab..c8b15dac1 100644
--- a/DemoRenderer/Constraints/ContactLineExtractors.tt
+++ b/DemoRenderer/Constraints/ContactLineExtractors.tt
@@ -26,12 +26,12 @@ for (int convexity = 0; convexity <= 1; ++convexity)
 #>
     struct Contact<#=contactCount#><#=convexitySuffix#><#=bodySuffix#>LineExtractor : IConstraintLineExtractor<Contact<#=contactCount#><#=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<LineInstance> lines)
         {
-            var poseA = bodies.Sets[setIndex].Poses[bodyIndices[0]];
+            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 deb985b28..22041caca 100644
--- a/DemoRenderer/Constraints/DistanceLimitLineExtractor.cs
+++ b/DemoRenderer/Constraints/DistanceLimitLineExtractor.cs
@@ -8,14 +8,14 @@ namespace DemoRenderer.Constraints
 {
     struct DistanceLimitLineExtractor : IConstraintLineExtractor<DistanceLimitPrestepData>
     {
-        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<LineInstance> lines)
         {
             //Could do bundles of constraints at a time, but eh.
-            var poseA = bodies.Sets[setIndex].Poses[bodyIndices[0]];
-            var poseB = bodies.Sets[setIndex].Poses[bodyIndices[1]];
+            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 612d97d74..fa6e0585c 100644
--- a/DemoRenderer/Constraints/DistanceServoLineExtractor.cs
+++ b/DemoRenderer/Constraints/DistanceServoLineExtractor.cs
@@ -8,14 +8,14 @@ namespace DemoRenderer.Constraints
 {
     struct DistanceServoLineExtractor : IConstraintLineExtractor<DistanceServoPrestepData>
     {
-        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<LineInstance> lines)
         {
             //Could do bundles of constraints at a time, but eh.
-            var poseA = bodies.Sets[setIndex].Poses[bodyIndices[0]];
-            var poseB = bodies.Sets[setIndex].Poses[bodyIndices[1]];
+            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 a0aaf920e..a7e29b360 100644
--- a/DemoRenderer/Constraints/HingeLineExtractor.cs
+++ b/DemoRenderer/Constraints/HingeLineExtractor.cs
@@ -8,14 +8,14 @@ namespace DemoRenderer.Constraints
 {
     struct HingeLineExtractor : IConstraintLineExtractor<HingePrestepData>
     {
-        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<LineInstance> 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].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/LineExtractor.cs b/DemoRenderer/Constraints/LineExtractor.cs
index 20ee3cd57..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
@@ -15,22 +14,111 @@ public class LineExtractor : IDisposable
 
         BufferPool pool;
         ParallelLooper looper;
+        LooperAction executeJobAction;
+
+        public bool ShowConstraints = true;
+        public bool ShowContacts;
+        public bool ShowBoundingBoxes;
+
         public LineExtractor(BufferPool pool, ParallelLooper looper, int initialLineCapacity = 8192)
         {
             lines = new QuickList<LineInstance>(initialLineCapacity, pool);
-            constraints = new ConstraintLineExtractor(pool);
-            boundingBoxes = new BoundingBoxLineExtractor(pool);
+            constraints = new ConstraintLineExtractor();
+            boundingBoxes = new BoundingBoxLineExtractor();
             this.pool = pool;
             this.looper = looper;
+            executeJobAction = ExecuteJob;
+
+        }
+
+        Simulation[] simulations;
+        Simulation simulation;
+        QuickList<ConstraintLineExtractor.ThreadJob> constraintJobs;
+        QuickList<BoundingBoxLineExtractor.ThreadJob> 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(Bodies bodies, Solver solver, BroadPhase broadPhase, bool showConstraints = true, bool showContacts = false, bool showBoundingBoxes = false, IThreadDispatcher threadDispatcher = null)
+        public void Extract(Simulation[] simulations, IThreadDispatcher threadDispatcher = null)
         {
+            if (ShowConstraints || ShowContacts)
+            {
+                constraintJobs = new QuickList<ConstraintLineExtractor.ThreadJob>(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<BoundingBoxLineExtractor.ThreadJob>(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, executeJobAction);
+            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<ConstraintLineExtractor.ThreadJob>(128, pool);
+                constraints.CreateJobs(simulation, 0, ShowConstraints, ShowContacts, ref lines, ref constraintJobs, pool);
+            }
+            if (ShowBoundingBoxes)
+            {
+                boundingBoxJobs = new QuickList<BoundingBoxLineExtractor.ThreadJob>(128, pool);
+                boundingBoxes.CreateJobs(simulation, 0, ref lines, ref boundingBoxJobs, pool);
+            }
+            looper.Dispatcher = threadDispatcher;
+            looper.For(0, constraintJobs.Count + boundingBoxJobs.Count, executeJobAction);
+            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 +139,6 @@ public void ClearInstances()
         public void Dispose()
         {
             lines.Dispose(pool);
-            constraints.Dispose();
-            boundingBoxes.Dispose();
         }
     }
 }
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/Constraints/LinearAxisServoLineExtractor.cs b/DemoRenderer/Constraints/LinearAxisServoLineExtractor.cs
index 41e0c98ea..65f39304b 100644
--- a/DemoRenderer/Constraints/LinearAxisServoLineExtractor.cs
+++ b/DemoRenderer/Constraints/LinearAxisServoLineExtractor.cs
@@ -8,14 +8,14 @@ namespace DemoRenderer.Constraints
 {
     struct LinearAxisServoLineExtractor : IConstraintLineExtractor<LinearAxisServoPrestepData>
     {
-        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<LineInstance> lines)
         {
             //Could do bundles of constraints at a time, but eh.
-            var poseA = bodies.Sets[setIndex].Poses[bodyIndices[0]];
-            var poseB = bodies.Sets[setIndex].Poses[bodyIndices[1]];
+            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 52a5cdbb9..1b1224ddb 100644
--- a/DemoRenderer/Constraints/OneBodyLinearServoLineExtractor.cs
+++ b/DemoRenderer/Constraints/OneBodyLinearServoLineExtractor.cs
@@ -8,13 +8,13 @@ namespace DemoRenderer.Constraints
 {
     struct OneBodyLinearServoLineExtractor : IConstraintLineExtractor<OneBodyLinearServoPrestepData>
     {
-        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<LineInstance> 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].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 67c41272f..ba42d0d0e 100644
--- a/DemoRenderer/Constraints/PointOnLineLineExtractor.cs
+++ b/DemoRenderer/Constraints/PointOnLineLineExtractor.cs
@@ -8,14 +8,14 @@ namespace DemoRenderer.Constraints
 {
     struct PointOnLineLineExtractor : IConstraintLineExtractor<PointOnLineServoPrestepData>
     {
-        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<LineInstance> lines)
         {
             //Could do bundles of constraints at a time, but eh.
-            var poseA = bodies.Sets[setIndex].Poses[bodyIndices[0]];
-            var poseB = bodies.Sets[setIndex].Poses[bodyIndices[1]];
+            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 3c6412c09..24755ac82 100644
--- a/DemoRenderer/Constraints/SwivelHingeLineExtractor.cs
+++ b/DemoRenderer/Constraints/SwivelHingeLineExtractor.cs
@@ -8,14 +8,14 @@ namespace DemoRenderer.Constraints
 {
     struct SwivelHingeLineExtractor : IConstraintLineExtractor<SwivelHingePrestepData>
     {
-        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<LineInstance> 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].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 fec7ab02b..4a5fda742 100644
--- a/DemoRenderer/Constraints/WeldLineExtractor.cs
+++ b/DemoRenderer/Constraints/WeldLineExtractor.cs
@@ -8,14 +8,14 @@ namespace DemoRenderer.Constraints
 {
     struct WeldLineExtractor : IConstraintLineExtractor<WeldPrestepData>
     {
-        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<LineInstance> lines)
         {
             //Could do bundles of constraints at a time, but eh.
-            var poseA = bodies.Sets[setIndex].Poses[bodyIndices[0]];
-            var poseB = bodies.Sets[setIndex].Poses[bodyIndices[1]];
+            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/DemoRenderer.csproj b/DemoRenderer/DemoRenderer.csproj
index dbe8bd341..8fcbcf60b 100644
--- a/DemoRenderer/DemoRenderer.csproj
+++ b/DemoRenderer/DemoRenderer.csproj
@@ -1,13 +1,13 @@
 <Project Sdk="Microsoft.NET.Sdk">
   <PropertyGroup>
-    <TargetFramework>net5.0</TargetFramework>
+    <TargetFramework>net9.0</TargetFramework>
     <LangVersion>latest</LangVersion>
     <AllowUnsafeBlocks>True</AllowUnsafeBlocks>
   </PropertyGroup>
 
   <Target Name="PreBuild" BeforeTargets="PreBuildEvent">
-    <Exec Condition="'$(Configuration)'=='Debug'" Command="dotnet &quot;$(ProjectDir)..\DemoContentBuilder\bin\x64\$(Configuration)\net5.0\DemoContentBuilder.dll&quot; &quot;$(ProjectDir)$(ProjectName).content&quot; -debug" />
-    <Exec Condition="'$(Configuration)'!='Debug'" Command="dotnet &quot;$(ProjectDir)..\DemoContentBuilder\bin\x64\Release\net5.0\DemoContentBuilder.dll&quot; &quot;$(ProjectDir)$(ProjectName).content&quot;" />
+    <Exec Condition="'$(Configuration)'=='Debug'" Command="dotnet &quot;$(ProjectDir)..\DemoContentBuilder\bin\x64\$(Configuration)\net9.0\DemoContentBuilder.dll&quot; &quot;$(ProjectDir)$(ProjectName).content&quot; -debug" />
+    <Exec Condition="'$(Configuration)'!='Debug'" Command="dotnet &quot;$(ProjectDir)..\DemoContentBuilder\bin\x64\Release\net9.0\DemoContentBuilder.dll&quot; &quot;$(ProjectDir)$(ProjectName).content&quot;" />
   </Target>
   
   <ItemGroup>
diff --git a/DemoRenderer/Helpers.cs b/DemoRenderer/Helpers.cs
index a3ba21cc8..251092d2f 100644
--- a/DemoRenderer/Helpers.cs
+++ b/DemoRenderer/Helpers.cs
@@ -146,7 +146,7 @@ public static uint[] GetBoxIndices(int boxCount)
         /// <param name="color">RGB color to pack.</param>
         /// <returns>Color packed into 32 bits.</returns>
         [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)
         /// <param name="color">RGBA color to pack.</param>
         /// <returns>Color packed into 32 bits.</returns>
         [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)
         /// <param name="source">Orientation to pack.</param>
         /// <param name="packed">W-less packed orientation, with remaining components negated to guarantee that the reconstructed positive W component is valid.</param>
         [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)
         /// <param name="source">Orientation to pack.</param>
         /// <param name="packed">Packed orientation.</param>
         [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<float, Vector4>(ref source.X);
@@ -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)
         /// <param name="linear">Linear input to apply the curve to.</param>
         /// <param name="srgb">Transformed value.</param>
         [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/ParallelLooper.cs b/DemoRenderer/ParallelLooper.cs
index 43fd80ce8..9c04104cc 100644
--- a/DemoRenderer/ParallelLooper.cs
+++ b/DemoRenderer/ParallelLooper.cs
@@ -1,24 +1,39 @@
 using BepuUtilities;
 using System;
-using System.Collections.Generic;
-using System.Text;
 using System.Threading;
 
 namespace DemoRenderer
 {
     /// <summary>
-    /// 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.
+    /// Function called by the <see cref="ParallelLooper"/> for each job.
+    /// </summary>
+    /// <param name="jobIndex">Index of the job to execute.</param>
+    /// <param name="workerIndex">Index of the worker executing the job.</param>
+    public delegate void LooperAction(int jobIndex, int workerIndex);
+
+    /// <summary>
+    /// Function called by the <see cref="ParallelLooper"/> when a worker determines that there is no more work available.
+    /// </summary>
+    /// <param name="workerIndex">Index of the worker that finished all available work.</param>
+    public delegate void LooperWorkerDone(int workerIndex);
+
+    /// <summary>
+    /// Simple multithreaded for loop provider built on an <see cref="IThreadDispatcher"/>. Performs an atomic operation for every object in the loop, so pre-chunking the works into jobs is important.
     /// </summary>
     /// <remarks>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.</remarks>
-    public class ParallelLooper
+    public unsafe class ParallelLooper
     {
-        Action<int> workerDelegate;
+        Action<int> dispatcherWorker;
+
+        /// <summary>
+        /// Gets or sets the dispatcher used by the looper.
+        /// </summary>
         public IThreadDispatcher Dispatcher { get; set; }
-        
+
         public ParallelLooper()
         {
-            workerDelegate = Worker;
+            dispatcherWorker = Worker;
         }
 
         void Worker(int workerIndex)
@@ -28,29 +43,42 @@ 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<int> work;
+        LooperAction iteration;
+        LooperWorkerDone workerDone;
 
-        public void For(int start, int exclusiveEnd, Action<int> work)
+        /// <summary>
+        /// Executes an action for each index in the given range.
+        /// </summary>
+        /// <param name="start">Inclusive start index of the execution range.</param>
+        /// <param name="exclusiveEnd">Exclusive end index of the execution range.</param>
+        /// <param name="workAction">Delegate to invoke for each index.</param>
+        /// <param name="workerDone">Delegate to invoke after all workers are done.</param>
+        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, exclusiveEnd - start);
+                this.iteration = null;
+                this.workerDone = workerDone;
             }
         }
     }
diff --git a/DemoRenderer/Renderer.cs b/DemoRenderer/Renderer.cs
index 2b7220447..7657bf34e 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.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, 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.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/MeshCache.cs b/DemoRenderer/ShapeDrawing/MeshCache.cs
index 7edc1cf83..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<Vector
 
         public unsafe bool Allocate(ulong id, int vertexCount, out int start, out Buffer<Vector3> vertices)
         {
+            requestedIds.Add(id, Pool);
             if (TryGetExistingMesh(id, out start, out vertices))
             {
                 return false;
@@ -69,8 +70,8 @@ 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);
-            Pool.ResizeToAtLeast(ref this.vertices, newSize, copyCount);
+            var newSize = (int)BitOperations.RoundUpToPowerOf2((uint)copyCount);
+            Pool.ResizeToAtLeast(ref this.vertices, newSize, 0);
             allocator.Capacity = newSize;
             allocator.Allocate(id, vertexCount, out longStart);
             start = (int)longStart;
diff --git a/DemoRenderer/ShapeDrawing/ShapesExtractor.cs b/DemoRenderer/ShapeDrawing/ShapesExtractor.cs
index 04bbfb316..911a1c1ea 100644
--- a/DemoRenderer/ShapeDrawing/ShapesExtractor.cs
+++ b/DemoRenderer/ShapeDrawing/ShapesExtractor.cs
@@ -7,58 +7,85 @@
 using System.Runtime.CompilerServices;
 using SharpDX.Direct3D11;
 using System;
+using System.Diagnostics;
 
 namespace DemoRenderer.ShapeDrawing
 {
+    public struct ShapeCache
+    {
+        internal QuickList<SphereInstance> Spheres;
+        internal QuickList<CapsuleInstance> Capsules;
+        internal QuickList<CylinderInstance> Cylinders;
+        internal QuickList<BoxInstance> Boxes;
+        internal QuickList<TriangleInstance> Triangles;
+        internal QuickList<MeshInstance> Meshes;
+
+        public ShapeCache(int initialCapacityPerShapeType, IUnmanagedMemoryPool pool)
+        {
+            Spheres = new QuickList<SphereInstance>(initialCapacityPerShapeType, pool);
+            Capsules = new QuickList<CapsuleInstance>(initialCapacityPerShapeType, pool);
+            Cylinders = new QuickList<CylinderInstance>(initialCapacityPerShapeType, pool);
+            Boxes = new QuickList<BoxInstance>(initialCapacityPerShapeType, pool);
+            Triangles = new QuickList<TriangleInstance>(initialCapacityPerShapeType, pool);
+            Meshes = new QuickList<MeshInstance>(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(IUnmanagedMemoryPool 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<SphereInstance> spheres;
-        internal QuickList<CapsuleInstance> capsules;
-        internal QuickList<CylinderInstance> cylinders;
-        internal QuickList<BoxInstance> boxes;
-        internal QuickList<TriangleInstance> triangles;
-        internal QuickList<MeshInstance> meshes;
 
         BufferPool pool;
+        public ShapeCache ShapeCache;
         public MeshCache MeshCache;
 
         ParallelLooper looper;
+        LooperAction addShapesForJobAction;
+        LooperWorkerDone workerDoneAction;
         public ShapesExtractor(Device device, ParallelLooper looper, BufferPool pool, int initialCapacityPerShapeType = 1024)
         {
-            spheres = new QuickList<SphereInstance>(initialCapacityPerShapeType, pool);
-            capsules = new QuickList<CapsuleInstance>(initialCapacityPerShapeType, pool);
-            cylinders = new QuickList<CylinderInstance>(initialCapacityPerShapeType, pool);
-            boxes = new QuickList<BoxInstance>(initialCapacityPerShapeType, pool);
-            triangles = new QuickList<TriangleInstance>(initialCapacityPerShapeType, pool);
-            meshes = new QuickList<MeshInstance>(initialCapacityPerShapeType, pool);
+            ShapeCache = new ShapeCache(initialCapacityPerShapeType, pool);
             this.MeshCache = new MeshCache(device, pool);
             this.pool = pool;
             this.looper = looper;
+            addShapesForJobAction = AddShapesForJob;
+            workerDoneAction = WorkerDone;
         }
 
         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<CompoundChild> children, Shapes shapes, in RigidPose pose, in Vector3 color)
+        private unsafe void AddCompoundChildren(ref Buffer<CompoundChild> children, Shapes shapes, RigidPose pose, Vector3 color, ref ShapeCache shapeCache, IUnmanagedMemoryPool 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);
+                Compound.GetWorldPose(child.AsPose(), pose, out var childPose);
+                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, 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.
@@ -71,7 +98,7 @@ public unsafe void AddShape(void* shapeData, int shapeType, Shapes shapes, ref R
                         instance.Radius = Unsafe.AsRef<Sphere>(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 +108,9 @@ public unsafe void AddShape(void* shapeData, int shapeType, Shapes shapes, ref R
                         ref var capsule = ref Unsafe.AsRef<Capsule>(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 +123,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 +134,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 +148,9 @@ public unsafe void AddShape(void* shapeData, int shapeType, Shapes shapes, ref R
                         ref var cylinder = ref Unsafe.AsRef<Cylinder>(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,10 +159,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<Vector3> 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)
@@ -144,7 +178,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)
@@ -166,17 +203,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<Compound>(shapeData).Children, shapes, pose, color);
+                        AddCompoundChildren(ref Unsafe.AsRef<Compound>(shapeData).Children, shapes, pose, color, ref shapeCache, pool);
                     }
                     break;
                 case BigCompound.Id:
                     {
-                        AddCompoundChildren(ref Unsafe.AsRef<BigCompound>(shapeData).Children, shapes, pose, color);
+                        AddCompoundChildren(ref Unsafe.AsRef<BigCompound>(shapeData).Children, shapes, pose, color, ref shapeCache, pool);
                     }
                     break;
                 case Mesh.Id:
@@ -185,11 +222,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<Vector3> 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)
@@ -202,31 +246,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, IUnmanagedMemoryPool 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>(TShape shape, Shapes shapes, ref RigidPose pose, in Vector3 color) where TShape : IShape
+        public unsafe void AddShape<TShape>(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), TShape.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, IUnmanagedMemoryPool pool)
         {
             ref var set = ref bodies.Sets[setIndex];
             var handle = set.IndexToHandle[indexInSet];
@@ -236,10 +294,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];
             Vector3 color;
             Helpers.UnpackColor((uint)HashHelper.Rehash(handle.Value), out Vector3 colorVariation);
-            if (Bodies.IsKinematic(inertia))
+            ref var state = ref set.DynamicsState[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,11 +324,11 @@ 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, 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, IUnmanagedMemoryPool pool)
         {
             var handle = statics.IndexToHandle[index];
             //Statics don't have any activity states. Just some simple variation on a central static color.
@@ -278,10 +336,142 @@ 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, 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<Job> jobs;
+        //The extractor can operate over one or multiple simulations. We cache them locally for threads to access.
+        Simulation[] simulations;
+        Simulation simulation;
+        Buffer<ShapeCache> workerCaches;
+
+        void PrepareForMultithreadedExecution(IThreadDispatcher threadDispatcher)
+        {
+            jobs = new QuickList<Job>(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.WorkerPools[i]);
+            }
+        }
+
+        void EndMultithreadedExecution()
+        {
+            jobs.Dispose(pool);
+            for (int i = 0; i < workerCaches.Length; ++i)
+            {
+                workerCaches[i].Dispose(looper.Dispatcher.WorkerPools[i]);
+            }
+            looper.Dispatcher = null;
+            pool.Return(ref workerCaches);
+        }
+
+        static void CreateJobs(Simulation simulation, int simulationIndex, ref QuickList<Job> 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.WorkerPools[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<TShape>(ref QuickList<TShape> workerCache, ref QuickList<TShape> 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 +480,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, addShapesForJobAction, workerDoneAction);
+                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, addShapesForJobAction, workerDoneAction);
+                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/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/DemoRenderer/UI/RenderableImage.cs b/DemoRenderer/UI/RenderableImage.cs
index fe8dae805..fc0d9fb5f 100644
--- a/DemoRenderer/UI/RenderableImage.cs
+++ b/DemoRenderer/UI/RenderableImage.cs
@@ -10,7 +10,7 @@
 namespace DemoRenderer.UI
 {
     /// <summary>
-    /// 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.
     /// </summary>
     public class RenderableImage : IDisposable
     {
@@ -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);
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/ConstraintDescriptionMappingTests.cs b/DemoTests/ConstraintDescriptionMappingTests.cs
index ad86fef63..9a29241ef 100644
--- a/DemoTests/ConstraintDescriptionMappingTests.cs
+++ b/DemoTests/ConstraintDescriptionMappingTests.cs
@@ -25,13 +25,13 @@ static void FillWithRandomBytes<T>(ref T item, Random random) where T : struct
         }
         static void Test<T>(BufferPool pool, Random random, int constraintTypeBodyCount) where T : unmanaged, IConstraintDescription<T>
         {
-            var simulation = Simulation.Create(pool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(), new PositionFirstTimestepper());
+            var simulation = Simulation.Create(pool, new DemoNarrowPhaseCallbacks(new SpringSettings(30, 1)), new DemoPoseIntegratorCallbacks(), new SolveDescription(8, 1));
 
 
             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);
             }
 
@@ -57,7 +57,7 @@ static void Test<T>(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)
@@ -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<Contact1OneBody>(pool, random, 1);
             Test<Contact2OneBody>(pool, random, 1);
@@ -145,6 +145,8 @@ public static void TestMappings()
             Test<OneBodyAngularMotor>(pool, random, 1);
             Test<OneBodyLinearServo>(pool, random, 1);
             Test<OneBodyLinearMotor>(pool, random, 1);
+            Test<AngularAxisGearMotor>(pool, random, 2);
+            Test<CenterDistanceLimit>(pool, random, 2);
 
             pool.Clear();
         }
diff --git a/DemoTests/DemoTests.csproj b/DemoTests/DemoTests.csproj
index 72cf83db0..05d93a15f 100644
--- a/DemoTests/DemoTests.csproj
+++ b/DemoTests/DemoTests.csproj
@@ -1,10 +1,11 @@
 <Project Sdk="Microsoft.NET.Sdk">
 
   <PropertyGroup>
-    <TargetFramework>net5.0</TargetFramework>
+    <TargetFramework>net9.0</TargetFramework>
     <AllowUnsafeBlocks>true</AllowUnsafeBlocks>
     <IsPackable>false</IsPackable>
     <LangVersion>latest</LangVersion>
+    <Platforms>AnyCPU;x86;x64</Platforms>
   </PropertyGroup>
 
   <ItemGroup>
diff --git a/DemoTests/InertiaTensorTests.cs b/DemoTests/InertiaTensorTests.cs
index a95617041..b953d23f9 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;
@@ -23,7 +24,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 +44,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 +64,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 +83,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 +102,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 +135,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)
         {
@@ -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<TInertiaTester>(ref TInertiaTester tester) where TInertiaTester : IInertiaTester
         {
             tester.ComputeBounds(out var min, out var max);
@@ -183,13 +221,7 @@ static void CheckInertia<TInertiaTester>(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,105 @@ static void CheckInertia<TInertiaTester>(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, Vector3 normal, int childIndex)
+            {
+                ++Counter;
             }
         }
 
-        static bool ValuesAreSimilar(float a, float b)
+
+        private 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);
+            if (random.NextDouble() < 0.5)
+            {
+                //Bit hacky. Technically two codepaths, want to cover both.
+                Span<float> 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;
+            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, 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!)
+                        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]
@@ -228,22 +339,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 +362,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 +375,7 @@ public static void Test()
                 var pointSet = new QuickList<Vector3>(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)
                 {
@@ -277,6 +388,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/DemoTests/PairDeterminismTests.cs b/DemoTests/PairDeterminismTests.cs
index 137d240fb..35aa5272a 100644
--- a/DemoTests/PairDeterminismTests.cs
+++ b/DemoTests/PairDeterminismTests.cs
@@ -67,15 +67,23 @@ public unsafe void OnPairCompleted<TManifold>(int pairId, ref TManifold manifold
         }
 
         static void ComputeCollisions(CollisionTaskRegistry registry, Shapes shapes, BufferPool pool,
-            ref Manifolds manifolds, CollidableDescription a, CollidableDescription b, ref Buffer<RigidPose> posesA, ref Buffer<RigidPose> posesB, Buffer<int> remapIndices, int pairCount)
+            ref Manifolds manifolds, CollidableDescription a, CollidableDescription b, ref Buffer<RigidPose> posesA, ref Buffer<RigidPose> posesB, Buffer<int> remapIndices, int pairCount, Random random)
         {
-            var batcher = new CollisionBatcher<BatcherCallbacks>(pool, shapes, registry, 1 / 60f, new BatcherCallbacks { Pool = pool, Manifolds = manifolds });
+            const float dt = Demo.TimestepDuration;
+            var callbacks = new BatcherCallbacks { Pool = pool, Manifolds = manifolds };
+            var batcher = new CollisionBatcher<BatcherCallbacks>(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.SpeculativeMargin, b.SpeculativeMargin), 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();
+                    batcher = new CollisionBatcher<BatcherCallbacks>(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]}");
@@ -224,39 +232,40 @@ public static void Test()
             var points = new QuickList<Vector3>(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);
 
             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;
+            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);
-            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);
-            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 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 = 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;
@@ -315,6 +324,7 @@ public static void Test()
 
             TestPair(meshCollidable, meshCollidable, bounds, pairCount, poseIterations, remapIterations, registry, pool, shapes, randomSeed);
 
+            pool.Clear();
         }
     }
 }
diff --git a/DemoTests/TestUtilities.cs b/DemoTests/TestUtilities.cs
index 542fb6c66..b2785e625 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);
             }
@@ -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;
 
@@ -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.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);
                         var velocityHash = ComputeHash(ref velocity.Linear, 211) + ComputeHash(ref velocity.Angular, 397);
                         hash += set.IndexToHandle[bodyIndex].Value * (poseHash + velocityHash);
diff --git a/DemoUtilities/DemoUtilities.csproj b/DemoUtilities/DemoUtilities.csproj
index 112e72e6a..1bc2fc4ed 100644
--- a/DemoUtilities/DemoUtilities.csproj
+++ b/DemoUtilities/DemoUtilities.csproj
@@ -1,7 +1,7 @@
 <Project Sdk="Microsoft.NET.Sdk">
 
   <PropertyGroup>
-    <TargetFramework>net5.0</TargetFramework>
+    <TargetFramework>net9.0</TargetFramework>
     <LangVersion>latest</LangVersion>
     <AllowUnsafeBlocks>true</AllowUnsafeBlocks>
   </PropertyGroup>
diff --git a/DemoUtilities/Input.cs b/DemoUtilities/Input.cs
index e916fa92d..3e3e38a5e 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
 {
@@ -240,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;
             }
 
         }
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 1b9864430..652874c15 100644
--- a/DemoUtilities/TextBuilder.cs
+++ b/DemoUtilities/TextBuilder.cs
@@ -1,10 +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
 {
@@ -60,7 +57,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 +82,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/DemoUtilities/Window.cs b/DemoUtilities/Window.cs
index c70b59c9c..1cacc475b 100644
--- a/DemoUtilities/Window.cs
+++ b/DemoUtilities/Window.cs
@@ -95,15 +95,10 @@ 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;
             window.Closing += OnClosing;
-
-            window.WindowBorder = WindowBorder.Resizable;
-
             WindowMode = windowMode;
 
         }
diff --git a/Demos.GL.sln b/Demos.GL.sln
new file mode 100644
index 000000000..ed964e8a3
--- /dev/null
+++ b/Demos.GL.sln
@@ -0,0 +1,226 @@
+
+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
+	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
+		{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
+		{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
+		{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
+	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.GL/Demos.csproj b/Demos.GL/Demos.csproj
old mode 100644
new mode 100755
index b2e46843d..f4d9b1c9b
--- a/Demos.GL/Demos.csproj
+++ b/Demos.GL/Demos.csproj
@@ -1,28 +1,23 @@
 <Project Sdk="Microsoft.NET.Sdk">
   <PropertyGroup>
     <OutputType>Exe</OutputType>
-    <TargetFramework>net5.0</TargetFramework>
+    <TargetFramework>net9.0</TargetFramework>
     <AllowUnsafeBlocks>True</AllowUnsafeBlocks>
-    <Configurations>Debug;Release;ReleaseStrip</Configurations>
+    <Configurations>Debug;Release</Configurations>
     <LangVersion>latest</LangVersion>
     <!--<TieredCompilation>false</TieredCompilation>-->
   </PropertyGroup>
 
   <Target Name="PreBuild" BeforeTargets="PreBuildEvent">
-    <Exec Condition="'$(Configuration)'=='Debug'" Command="dotnet run -p &quot;$(ProjectDir)../DemoContentBuilder&quot; -- &quot;$(ProjectDir)../Demos/Demos.content&quot; -debug" />
-    <Exec Condition="'$(Configuration)'!='Debug'" Command="dotnet run -p &quot;$(ProjectDir)../DemoContentBuilder&quot; -- &quot;$(ProjectDir)../Demos/Demos.content&quot;" />
+    <Exec Condition="'$(Configuration)'=='Debug'" Command="dotnet run --project &quot;$(ProjectDir)../DemoContentBuilder&quot; -- &quot;$(ProjectDir)../Demos/Demos.content&quot; -debug" />
+    <Exec Condition="'$(Configuration)'!='Debug'" Command="dotnet run --project &quot;$(ProjectDir)../DemoContentBuilder&quot; -- &quot;$(ProjectDir)../Demos/Demos.content&quot;" />
   </Target>
 
   <PropertyGroup Condition="'$(Configuration)'=='Release'">
     <Optimize>true</Optimize>
     <DefineConstants>TRACE;RELEASE</DefineConstants>
-    <DebugType>embedded</DebugType>
     <DebugSymbols>true</DebugSymbols>
-  </PropertyGroup>
-
-  <PropertyGroup Condition="'$(Configuration)'=='ReleaseStrip'">
-    <Optimize>true</Optimize>
-    <DefineConstants>TRACE;RELEASE</DefineConstants>
+    <DebugType>full</DebugType>
   </PropertyGroup>
 
   <PropertyGroup Condition="'$(Configuration)'=='Debug'">
diff --git a/Demos.sln b/Demos.sln
index 47c988c3c..65e251f2d 100644
--- a/Demos.sln
+++ b/Demos.sln
@@ -1,259 +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
-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
-	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/Content/Sponsors/beaverboss.png b/Demos/Content/Sponsors/beaverboss.png
new file mode 100644
index 000000000..425b6d277
Binary files /dev/null and b/Demos/Content/Sponsors/beaverboss.png differ
diff --git a/Demos/Content/Sponsors/bedtimeforopossum.png b/Demos/Content/Sponsors/bedtimeforopossum.png
new file mode 100644
index 000000000..a3603b1f2
Binary files /dev/null and b/Demos/Content/Sponsors/bedtimeforopossum.png differ
diff --git a/Demos/Content/Sponsors/ladybugwouldpreferlandvaluetax.png b/Demos/Content/Sponsors/ladybugwouldpreferlandvaluetax.png
new file mode 100644
index 000000000..2eb6209fe
Binary files /dev/null and b/Demos/Content/Sponsors/ladybugwouldpreferlandvaluetax.png differ
diff --git a/Demos/Content/Sponsors/marmottourism.png b/Demos/Content/Sponsors/marmottourism.png
new file mode 100644
index 000000000..47cc0ec02
Binary files /dev/null and b/Demos/Content/Sponsors/marmottourism.png differ
diff --git a/Demos/Controls.cs b/Demos/Controls.cs
index 1e7d303dd..cdeef3726 100644
--- a/Demos/Controls.cs
+++ b/Demos/Controls.cs
@@ -1,508 +1,505 @@
 using DemoUtilities;
-using OpenTK.Audio.OpenAL;
 using OpenTK.Input;
 using System;
 using System.Collections.Generic;
 using System.Runtime.InteropServices;
-using System.Text;
 
-namespace Demos
+namespace Demos;
+
+/// <summary>
+/// Caches strings for enum values to avoid enum boxing.
+/// </summary>
+static class ControlStrings
 {
-    /// <summary>
-    /// Caches strings for enum values to avoid enum boxing.
-    /// </summary>
-    static class ControlStrings
-    {
-        static Dictionary<Key, string> keys;
-        static Dictionary<MouseButton, string> mouseButtons;
-        static Dictionary<MouseWheelAction, string> mouseWheel;
+    static Dictionary<Key, string> keys;
+    static Dictionary<MouseButton, string> mouseButtons;
+    static Dictionary<MouseWheelAction, string> 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<Key, string>();
+        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<MouseButton, string>();
+        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<MouseWheelAction, string>();
+        var wheelNames = Enum.GetNames(typeof(MouseWheelAction));
+        var wheelValues = (MouseWheelAction[])Enum.GetValues(typeof(MouseWheelAction));
+        for (int i = 0; i < wheelNames.Length; ++i)
         {
-            keys = new Dictionary<Key, string>();
-            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<MouseButton, string>();
-            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<MouseWheelAction, string>();
-            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,
-    }
-    /// <summary>
-    /// A control binding which can be held for multiple frames.
-    /// </summary>
-    [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,
+}
+/// <summary>
+/// A control binding which can be held for multiple frames.
+/// </summary>
+[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
-    }
-    /// <summary>
-    /// A control binding that supports any form of instant action, but may or may not support being held.
-    /// </summary>
-    [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
+}
+/// <summary>
+/// A control binding that supports any form of instant action, but may or may not support being held.
+/// </summary>
+[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 4b57d7df9..329db2071 100644
--- a/Demos/Demo.cs
+++ b/Demos/Demo.cs
@@ -5,115 +5,115 @@
 using System;
 using DemoRenderer.UI;
 using DemoContentLoader;
+using BepuUtilities;
 
-namespace Demos
+namespace Demos;
+
+public abstract class Demo : IDisposable
 {
-    public abstract class Demo : IDisposable
+    /// <summary>
+    /// Gets the simulation created by the demo's Initialize call.
+    /// </summary>
+    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.
+    /// <summary>
+    /// Gets the buffer pool used by the demo's simulation.
+    /// </summary>
+    public BufferPool BufferPool { get; private set; }
+
+    /// <summary>
+    /// Gets the thread dispatcher available for use by the simulation.
+    /// </summary>
+    public ThreadDispatcher ThreadDispatcher { get; private set; }
+
+    protected Demo()
     {
-        /// <summary>
-        /// Gets the simulation created by the demo's Initialize call.
-        /// </summary>
-        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.
-        /// <summary>
-        /// Gets the buffer pool used by the demo's simulation.
-        /// </summary>
-        public BufferPool BufferPool { get; private set; }
-
-        /// <summary>
-        /// Gets the thread dispatcher available for use by the simulation.
-        /// </summary>
-        public SimpleThreadDispatcher 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 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);
-        }
-
-        public virtual void LoadGraphicalContent(ContentArchive content, RenderSurface surface)
-        {
-        }
+        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 abstract void Initialize(ContentArchive content, Camera camera);
+    public virtual void LoadGraphicalContent(ContentArchive content, RenderSurface surface)
+    {
+    }
 
+    public abstract void Initialize(ContentArchive content, Camera camera);
 
-        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(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;
-            //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();
-                BufferPool.Clear();
-                ThreadDispatcher.Dispose();
-            }
+            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 504bc35fb..f2613bd5a 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;
@@ -8,123 +7,162 @@
 using System.Runtime.CompilerServices;
 using System.Numerics;
 
-namespace Demos
+namespace Demos;
+
+public struct DemoPoseIntegratorCallbacks : IPoseIntegratorCallbacks
 {
-    public struct DemoPoseIntegratorCallbacks : IPoseIntegratorCallbacks
+    /// <summary>
+    /// Gravity to apply to dynamic bodies in the simulation.
+    /// </summary>
+    public Vector3 Gravity;
+    /// <summary>
+    /// 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.
+    /// </summary>
+    public float LinearDamping;
+    /// <summary>
+    /// 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.
+    /// </summary>
+    public float AngularDamping;
+
+
+    /// <summary>
+    /// Gets how the pose integrator should handle angular velocity integration.
+    /// </summary>
+    public readonly AngularIntegrationMode AngularIntegrationMode => AngularIntegrationMode.Nonconserving;
+
+    /// <summary>
+    /// 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. 
+    /// </summary>
+    public readonly bool AllowSubstepsForUnconstrainedBodies => false;
+
+    /// <summary>
+    /// 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.
+    /// </summary>
+    public readonly bool IntegrateVelocityForKinematics => false;
+
+    public void Initialize(Simulation simulation)
     {
-        /// <summary>
-        /// Gravity to apply to dynamic bodies in the simulation.
-        /// </summary>
-        public Vector3 Gravity;
-        /// <summary>
-        /// 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.
-        /// </summary>
-        public float LinearDamping;
-        /// <summary>
-        /// 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.
-        /// </summary>
-        public float AngularDamping;
-
-        Vector3 gravityDt;
-        float linearDampingDt;
-        float angularDampingDt;
-
-        public readonly AngularIntegrationMode AngularIntegrationMode => AngularIntegrationMode.Nonconserving;
-
-        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<T> or something similar, having the simulation provided in a callback can be helpful.
-        }
-
-        /// <summary>
-        /// Creates a new set of simple callbacks for the demos.
-        /// </summary>
-        /// <param name="gravity">Gravity to apply to dynamic bodies in the simulation.</param>
-        /// <param name="linearDamping">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.</param>
-        /// <param name="angularDamping">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.</param>
-        public DemoPoseIntegratorCallbacks(Vector3 gravity, float linearDamping = .03f, float angularDamping = .03f) : this()
-        {
-            Gravity = gravity;
-            LinearDamping = linearDamping;
-            AngularDamping = angularDamping;
-        }
+        //In this demo, we don't need to initialize anything.
+        //If you had a simulation with per body gravity stored in a CollidableProperty<T> or something similar, having the simulation provided in a callback can be helpful.
+    }
 
-        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);
-        }
-        [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.
-        }
+    /// <summary>
+    /// Creates a new set of simple callbacks for the demos.
+    /// </summary>
+    /// <param name="gravity">Gravity to apply to dynamic bodies in the simulation.</param>
+    /// <param name="linearDamping">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.</param>
+    /// <param name="angularDamping">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.</param>
+    public DemoPoseIntegratorCallbacks(Vector3 gravity, float linearDamping = .03f, float angularDamping = .03f) : this()
+    {
+        Gravity = gravity;
+        LinearDamping = linearDamping;
+        AngularDamping = angularDamping;
+    }
 
+    Vector3Wide gravityWideDt;
+    Vector<float> linearDampingDt;
+    Vector<float> angularDampingDt;
+
+    /// <summary>
+    /// Callback invoked ahead of dispatches that may call into <see cref="IntegrateVelocity"/>.
+    /// 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. 
+    /// </summary>
+    /// <param name="dt">Current integration time step duration.</param>
+    /// <remarks>This is typically used for precomputing anything expensive that will be used across velocity integration.</remarks>
+    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<float>(MathF.Pow(MathHelper.Clamp(1 - LinearDamping, 0, 1), dt));
+        angularDampingDt = new Vector<float>(MathF.Pow(MathHelper.Clamp(1 - AngularDamping, 0, 1), dt));
+        gravityWideDt = Vector3Wide.Broadcast(Gravity * dt);
     }
-    public unsafe struct DemoNarrowPhaseCallbacks : INarrowPhaseCallbacks
+
+    /// <summary>
+    /// Callback for a bundle of bodies being integrated.
+    /// </summary>
+    /// <param name="bodyIndices">Indices of the bodies being integrated in this bundle.</param>
+    /// <param name="position">Current body positions.</param>
+    /// <param name="orientation">Current body orientations.</param>
+    /// <param name="localInertia">Body's current local inertia.</param>
+    /// <param name="integrationMask">Mask indicating which lanes are active in the bundle. Active lanes will contain 0xFFFFFFFF, inactive lanes will contain 0.</param>
+    /// <param name="workerIndex">Index of the worker thread processing this bundle.</param>
+    /// <param name="dt">Durations to integrate the velocity over. Can vary over lanes.</param>
+    /// <param name="velocity">Velocity of bodies in the bundle. Any changes to lanes which are not active by the integrationMask will be discarded.</param>
+    public void IntegrateVelocity(Vector<int> bodyIndices, Vector3Wide position, QuaternionWide orientation, BodyInertiaWide localInertia, Vector<int> integrationMask, int workerIndex, Vector<float> dt, ref BodyVelocityWide velocity)
     {
-        public SpringSettings ContactSpringiness;
+        //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<float>.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.
-            if (ContactSpringiness.AngularFrequency == 0 && ContactSpringiness.TwiceDampingRatio == 0)
-                ContactSpringiness = new SpringSettings(30, 1);
-        }
+    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)
+    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 unsafe bool ConfigureContactManifold<TManifold>(int workerIndex, CollidablePair pair, ref TManifold manifold, out PairMaterialProperties pairMaterial) where TManifold : unmanaged, IContactManifold<TManifold>
-        {
-            pairMaterial.FrictionCoefficient = 1f;
-            pairMaterial.MaximumRecoveryVelocity = 2f;
-            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<TManifold>(int workerIndex, CollidablePair pair, ref TManifold manifold, out PairMaterialProperties pairMaterial) where TManifold : unmanaged, IContactManifold<TManifold>
+    {
+        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 fde7751b6..f206fbc9a 100644
--- a/Demos/DemoHarness.cs
+++ b/Demos/DemoHarness.cs
@@ -7,431 +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<FontContent>(@"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<FontContent>(@"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("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();
-            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);
+    }
+
+    enum CameraMoveSpeedState
+    {
+        Regular,
+        Slow,
+        Fast
+    }
+    CameraMoveSpeedState cameraSpeedState;
+    Int2? grabberCachedMousePosition;
 
-        public void Update(float dt)
+    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), demo.BufferPool);
 
 
 
-                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 % 20 == 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 or, better yet, ReleaseStrip 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;
 
-                //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);
-                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.Extract(demo.Simulation.Bodies, demo.Simulation.Solver, demo.Simulation.BroadPhase, showConstraints, showContacts, showBoundingBoxes, 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 1b5adeeaf..131a96be8 100644
--- a/Demos/DemoMeshHelper.cs
+++ b/Demos/DemoMeshHelper.cs
@@ -3,79 +3,188 @@
 using System.Numerics;
 using BepuUtilities.Memory;
 using DemoContentLoader;
+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 void LoadModel(ContentArchive content, BufferPool pool, string contentName, in Vector3 scaling, out Mesh mesh)
+        var meshContent = content.Load<MeshContent>(contentName);
+        pool.Take<Triangle>(meshContent.Triangles.Length, out var triangles);
+        for (int i = 0; i < meshContent.Triangles.Length; ++i)
         {
-            var meshContent = content.Load<MeshContent>(contentName);
-            pool.Take<Triangle>(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);
-            }
-            mesh = 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 void CreateFan(int triangleCount, float radius, in Vector3 scaling, BufferPool pool, out Mesh mesh)
-        {
-            var anglePerTriangle = 2 * MathF.PI / triangleCount;
-            pool.Take<Triangle>(triangleCount, out var triangles);
+    public static Mesh CreateFan(int triangleCount, float radius, Vector3 scaling, BufferPool pool)
+    {
+        var anglePerTriangle = 2 * MathF.PI / triangleCount;
+        pool.Take<Triangle>(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();
-            }
-            mesh = 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 void CreateDeformedPlane(int width, int height, Func<int, int, Vector3> deformer, Vector3 scaling, BufferPool pool, out Mesh mesh)
+    public static Mesh CreateDeformedPlane(int width, int height, Func<int, int, Vector3> deformer, Vector3 scaling, BufferPool pool, IThreadDispatcher dispatcher = null)
+    {
+        pool.Take<Vector3>(width * height, out var vertices);
+        for (int i = 0; i < width; ++i)
         {
-            pool.Take<Vector3>(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<Triangle>(triangleCount, out var triangles);
+        var quadWidth = width - 1;
+        var quadHeight = height - 1;
+        var triangleCount = quadWidth * quadHeight * 2;
+        pool.Take<Triangle>(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);
-            mesh = new Mesh(triangles, scaling, pool);
+        }
+        pool.Return(ref vertices);
+        return new Mesh(triangles, scaling, pool, dispatcher);
+    }
+
+    /// <summary>
+    /// Creates a bunch of nodes and associates them with leaves with absolutely no regard for where the leaves are.
+    /// </summary>
+    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++);
         }
     }
+
+    /// <summary>
+    /// 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 <see cref="Tree.Refit"/>, <see cref="Tree.RefitAndRefine(BufferPool, int, float)"/>,
+    /// or <see cref="Tree.RefitAndRefineMultithreadedContext.RefitAndRefine(ref Tree, BufferPool, IThreadDispatcher, int, float)"/> to provide bounds and higher quality.
+    /// </summary>
+    /// <param name="triangles">Large number of triangles to build a mesh from.</param>
+    /// <param name="scaling">Scale to use for the mesh shape.</param>
+    /// <param name="pool">Buffer pool to allocate resources for the mesh.</param>
+    /// <returns>Created mesh with no bounds.</returns>
+    /// <remarks>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.</remarks>
+    public unsafe static Mesh CreateGiantMeshFastWithoutBounds(Buffer<Triangle> 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)
+        {
+            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;
+    }
+
+    /// <summary>
+    /// 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.
+    /// </summary>
+    /// <param name="triangles">Large number of triangles to build a mesh from.</param>
+    /// <param name="scaling">Scale to use for the mesh shape.</param>
+    /// <param name="pool">Buffer pool to allocate resources for the mesh.</param>
+    /// <returns>Created mesh.</returns>
+    /// <remarks>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.</remarks>
+    public static Mesh CreateGiantMeshFast(Buffer<Triangle> 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;
+    }
+
+    /// <summary>
+    /// 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 <see cref="Tree.SweepBuild(BufferPool, Buffer{BoundingBox})"/> at a much lower cost thanks to a more efficient algorithm and multithreading.
+    /// </summary>
+    /// <param name="triangles">Large number of triangles to build a mesh from.</param>
+    /// <param name="scaling">Scale to use for the mesh shape.</param>
+    /// <param name="pool">Buffer pool to allocate resources for the mesh.</param>
+    /// <returns>Created mesh.</returns>
+    /// <remarks>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.</remarks>
+    public static Mesh CreateGiantMeshWithRefinements(Buffer<Triangle> 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 cd152afdf..e2b78ac67 100644
--- a/Demos/DemoSet.cs
+++ b/Demos/DemoSet.cs
@@ -3,81 +3,87 @@
 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;
-using Demos.SpecializedTests.Media;
 using System;
 using System.Collections.Generic;
-using System.Text;
 
-namespace Demos
+namespace Demos;
+
+/// <summary>
+/// Constructs a demo from the set of available demos on demand.
+/// </summary>
+public class DemoSet
 {
-    /// <summary>
-    /// Constructs a demo from the set of available demos on demand.
-    /// </summary>
-    public class DemoSet
+    struct Option
     {
-        struct Option
-        {
-            public string Name;
-            public Func<ContentArchive, Camera, RenderSurface, Demo> Builder;
-        }
+        public string Name;
+        public Func<ContentArchive, Camera, RenderSurface, Demo> Builder;
+    }
 
-        List<Option> options = new();
-        void AddOption<T>() where T : Demo, new()
+    List<Option> options = new();
+    void AddOption<T>() where T : Demo, new()
+    {
+        options.Add(new Option
         {
-            options.Add(new Option
+            Builder = (content, camera, surface) =>
             {
-                Builder = (content, camera, surface) =>
-                {
-                    //Note that the actual work is done in the Initialize function rather than a constructor.
-                    //The 'new T()' syntax actually uses reflection and repackages exceptions in an inconvenient way.
-                    //By using Initialize instead, the stack trace and debugger will go right to the source.
-                    var demo = new T();
-                    demo.LoadGraphicalContent(content, surface);
-                    demo.Initialize(content, camera);
-                    return demo;
-                },
-                Name = typeof(T).Name
-            });
-        }
+                //Note that the actual work is done in the Initialize function rather than a constructor.
+                //The 'new T()' syntax actually uses reflection and repackages exceptions in an inconvenient way.
+                //By using Initialize instead, the stack trace and debugger will go right to the source.
+                var demo = new T();
+                demo.LoadGraphicalContent(content, surface);
+                demo.Initialize(content, camera);
+                return demo;
+            },
+            Name = typeof(T).Name
+        });
+    }
 
-        public DemoSet()
-        {
-            AddOption<CarDemo>();
-            AddOption<CharacterDemo>();
-            AddOption<TankDemo>();
-            AddOption<ColosseumDemo>();
-            AddOption<PyramidDemo>();
-            AddOption<RagdollDemo>();
-            AddOption<ContinuousCollisionDetectionDemo>();
-            AddOption<ClothDemo>();
-            AddOption<NewtDemo>();
-            AddOption<PlanetDemo>();
-            AddOption<CompoundTestDemo>();
-            AddOption<BlockChainDemo>();
-            AddOption<RayCastingDemo>();
-            AddOption<SweepDemo>();
-            AddOption<RopeStabilityDemo>();
-            AddOption<SubsteppingDemo>();
-            AddOption<ContactEventsDemo>();
-            AddOption<CollisionQueryDemo>();
-            AddOption<SolverContactEnumerationDemo>();
-            AddOption<CustomVoxelCollidableDemo>();
-            AddOption<SponsorDemo>();
-        }
+    public DemoSet()
+    {
+        AddOption<CarDemo>();
+        AddOption<TankDemo>();
+        AddOption<CharacterDemo>();
+        AddOption<RagdollTubeDemo>();
+        AddOption<PyramidDemo>();
+        AddOption<ColosseumDemo>();
+        AddOption<NewtDemo>();
+        AddOption<ClothDemo>();
+        AddOption<DancerDemo>();
+        AddOption<PlumpDancerDemo>();
+        AddOption<ContinuousCollisionDetectionDemo>();
+        AddOption<PlanetDemo>();
+        AddOption<PerBodyGravityDemo>();
+        AddOption<CompoundDemo>();
+        AddOption<RopeStabilityDemo>();
+        AddOption<SubsteppingDemo>();
+        AddOption<ChainFountainDemo>();
+        AddOption<RopeTwistDemo>();
+        AddOption<FrictionDemo>();
+        AddOption<BouncinessDemo>();
+        AddOption<RayCastingDemo>();
+        AddOption<SweepDemo>();
+        AddOption<ContactEventsDemo>();
+        AddOption<CollisionTrackingDemo>();
+        AddOption<CollisionQueryDemo>();
+        AddOption<SolverContactEnumerationDemo>();
+        AddOption<CustomVoxelCollidableDemo>();
+        AddOption<BlockChainDemo>();
+        AddOption<SponsorDemo>();
+    }
 
-        public int Count { get { return options.Count; } }
+    public int Count { get { return options.Count; } }
 
-        public string GetName(int index)
-        {
-            return options[index].Name;
-        }
+    public string GetName(int index)
+    {
+        return options[index].Name;
+    }
 
-        public Demo Build(int index, ContentArchive content, Camera camera, RenderSurface surface)
-        {
-            return options[index].Builder(content, camera, surface);
-        }
+    public Demo Build(int index, ContentArchive content, Camera camera, RenderSurface surface)
+    {
+        return options[index].Builder(content, camera, surface);
     }
 }
diff --git a/Demos/Demos.content b/Demos/Demos.content
index 2a09360b4..83608fcf1 100644
--- a/Demos/Demos.content
+++ b/Demos/Demos.content
@@ -24,4 +24,8 @@ 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
+Content\Sponsors\ladybugwouldpreferlandvaluetax.png
+Content\Sponsors\beaverboss.png
+Content\Sponsors\marmottourism.png
\ No newline at end of file
diff --git a/Demos/Demos.csproj b/Demos/Demos.csproj
index 70b980cf3..d52b685f3 100644
--- a/Demos/Demos.csproj
+++ b/Demos/Demos.csproj
@@ -1,22 +1,22 @@
 <Project Sdk="Microsoft.NET.Sdk">
   <PropertyGroup>
     <OutputType>Exe</OutputType>
-    <TargetFramework>net5.0</TargetFramework>
+    <TargetFramework>net9.0</TargetFramework>
     <AllowUnsafeBlocks>True</AllowUnsafeBlocks>
     <Configurations>Debug;Release</Configurations>
     <LangVersion>latest</LangVersion>
+    <Platforms>AnyCPU;x86;x64</Platforms>
     <!--<TieredCompilation>false</TieredCompilation>-->
   </PropertyGroup>
 
   <Target Name="PreBuild" BeforeTargets="PreBuildEvent">
-    <Exec Condition="'$(Configuration)'=='Debug'" Command="dotnet &quot;$(ProjectDir)..\DemoContentBuilder\bin\x64\$(Configuration)\net5.0\DemoContentBuilder.dll&quot; &quot;$(ProjectDir)$(ProjectName).content&quot; -debug" />
-    <Exec Condition="'$(Configuration)'!='Debug'" Command="dotnet &quot;$(ProjectDir)..\DemoContentBuilder\bin\x64\Release\net5.0\DemoContentBuilder.dll&quot; &quot;$(ProjectDir)$(ProjectName).content&quot;" />
+    <Exec Condition="'$(Configuration)'=='Debug'" Command="dotnet &quot;$(ProjectDir)..\DemoContentBuilder\bin\x64\$(Configuration)\net9.0\DemoContentBuilder.dll&quot; &quot;$(ProjectDir)$(ProjectName).content&quot; -debug" />
+    <Exec Condition="'$(Configuration)'!='Debug'" Command="dotnet &quot;$(ProjectDir)..\DemoContentBuilder\bin\x64\Release\net9.0\DemoContentBuilder.dll&quot; &quot;$(ProjectDir)$(ProjectName).content&quot;" />
   </Target>
 
   <PropertyGroup Condition="'$(Configuration)'=='Release'">
     <Optimize>true</Optimize>
     <DefineConstants>TRACE;RELEASE</DefineConstants>
-    <DebugType>embedded</DebugType>
     <DebugSymbols>true</DebugSymbols>
   </PropertyGroup>
 
diff --git a/Demos/Demos/BlockChainDemo.cs b/Demos/Demos/BlockChainDemo.cs
index c95690d12..46fad2dd4 100644
--- a/Demos/Demos/BlockChainDemo.cs
+++ b/Demos/Demos/BlockChainDemo.cs
@@ -9,99 +9,92 @@
 using System;
 using System.Numerics;
 
-namespace Demos.Demos
+namespace Demos.Demos;
+
+/// <summary>
+/// Shows how to build a decentralized ledger of transactions out of a sequence of connected blocks which could be used to revolutionize the backbone of finance while offering
+/// a new form of trust management that can be applied to a wide range of industry problems to achieve a more #secure and sustainable network of verifiable #future connections
+/// on which the accelerated development of #blockchain #solutions on the #cloud to magnify your business impact to imagine what could be achieve more by taking advantage of 
+/// truly #BigData with modern AI analytics augmented by deep-learned #blockchain #technology #to make some boxes act like a multipendulum.
+/// </summary>
+public class BlockChainDemo : Demo
 {
-    /// <summary>
-    /// Shows how to build a decentralized ledger of transactions out of a sequence of connected blocks which could be used to revolutionize the backbone of finance while offering
-    /// a new form of trust management that can be applied to a wide range of industry problems to achieve a more #secure and sustainable network of verifiable #future connections
-    /// on which the accelerated development of #blockchain #solutions on the #cloud to magnify your business impact to imagine what could be achieve more by taking advantage of 
-    /// truly #BigData with modern AI analytics augmented by deep-learned #blockchain #technology #to make some boxes act like a multipendulum.
-    /// </summary>
-    public class BlockChainDemo : Demo
+    public override void Initialize(ContentArchive content, Camera camera)
     {
-        public unsafe override void Initialize(ContentArchive content, Camera camera)
-        {
-            camera.Position = new Vector3(-30, 8, -60);
-            camera.Yaw = MathHelper.Pi * 3f / 4;
-            camera.Pitch = 0;
+        camera.Position = new Vector3(-30, 8, -60);
+        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 PositionFirstTimestepper());
+        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);
-            boxShape.ComputeInertia(1, out var boxInertia);
-            var boxIndex = Simulation.Shapes.Add(boxShape);
-            const int forkCount = 20;
-            const int blocksPerChain = 20;
-            BodyHandle[] blockHandles = new BodyHandle[blocksPerChain];
-            for (int forkIndex = 0; forkIndex < forkCount; ++forkIndex)
+        var boxShape = new Box(1, 1, 1);
+        var boxInertia = boxShape.ComputeInertia(1);
+        var boxIndex = Simulation.Shapes.Add(boxShape);
+        const int forkCount = 20;
+        const int blocksPerChain = 20;
+        BodyHandle[] blockHandles = new BodyHandle[blocksPerChain];
+        for (int forkIndex = 0; forkIndex < forkCount; ++forkIndex)
+        {
+            //Build the blocks.
+            for (int blockIndex = 0; blockIndex < blocksPerChain; ++blockIndex)
             {
-                //Build the blocks.
-                for (int blockIndex = 0; blockIndex < blocksPerChain; ++blockIndex)
-                {
-                    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, .1f),
-                        new BodyActivityDescription(.01f, 32));
-                    blockHandles[blockIndex] = Simulation.Bodies.Add(bodyDescription);
-                }
-                //Build the chains.
-                for (int i = 1; i < blocksPerChain; ++i)
+                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, boxIndex, .01f);
+                blockHandles[blockIndex] = Simulation.Bodies.Add(bodyDescription);
+            }
+            //Build the chains.
+            for (int i = 1; i < blocksPerChain; ++i)
+            {
+                var ballSocket = new BallSocket
                 {
-                    var ballSocket = new BallSocket
-                    {
-                        LocalOffsetA = new Vector3(0, 1f, 0),
-                        LocalOffsetB = new Vector3(0, -1f, 0),
-                        SpringSettings = new SpringSettings(30, 5)
-                    };
-                    Simulation.Solver.Add(blockHandles[i - 1], blockHandles[i], ref ballSocket);
-                }
+                    LocalOffsetA = new Vector3(0, 1f, 0),
+                    LocalOffsetB = new Vector3(0, -1f, 0),
+                    SpringSettings = new SpringSettings(30, 5)
+                };
+                Simulation.Solver.Add(blockHandles[i - 1], blockHandles[i], ballSocket);
             }
+        }
 
-            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), 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));
-        }
+        //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.03f);
+    }
 
-        BodyDescription coinDescription;
-        Random random = new Random(5);
-        public override void Update(Window window, Camera camera, Input input, float dt)
+    BodyDescription coinDescription;
+    Random random = new Random(5);
+    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))
+            //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(random.NextSingle(), random.NextSingle(), random.NextSingle()) * new Vector3(60, 30, 60);
+            for (int i = 0; i < 128; ++i)
             {
-                //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);
-                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 length = direction.Length();
-                    if (length > 1e-7f)
-                        direction /= length;
-                    else
-                        direction = new Vector3(0, 1, 0);
+                var direction = new Vector3(-1) + 2 * new Vector3(random.NextSingle(), random.NextSingle(), random.NextSingle());
+                var length = direction.Length();
+                if (length > 1e-7f)
+                    direction /= length;
+                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());
-                    Simulation.Bodies.Add(coinDescription);
-                }
+                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);
             }
-            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 create an ICO.");
-            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)
+    {
+        text.Clear().Append("Press Z to create an ICO.");
+        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/Demos/BouncinessDemo.cs b/Demos/Demos/BouncinessDemo.cs
index 1e33ebda5..cb86ec8a1 100644
--- a/Demos/Demos/BouncinessDemo.cs
+++ b/Demos/Demos/BouncinessDemo.cs
@@ -4,11 +4,27 @@
 using BepuPhysics.Constraints;
 using DemoContentLoader;
 using DemoRenderer;
+using DemoRenderer.UI;
+using DemoUtilities;
 using System;
 using System.Numerics;
 using System.Runtime.CompilerServices;
 
-namespace Demos.Demos
+namespace Demos.Demos;
+
+/// <summary>
+/// Shows how to configure things to bounce in the absence of a coefficient of restitution.
+/// </summary>
+/// <remarks>
+/// <para>
+/// 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.
+/// </para>
+/// <para>
+/// For a similar example of friction, see the <see cref="FrictionDemo"/>.
+/// </para>
+/// </remarks>
+public class BouncinessDemo : Demo
 {
     public struct SimpleMaterial
     {
@@ -16,8 +32,15 @@ public struct SimpleMaterial
         public float FrictionCoefficient;
         public float MaximumRecoveryVelocity;
     }
-    public unsafe struct BounceCallbacks : INarrowPhaseCallbacks
+    public struct BounceCallbacks : INarrowPhaseCallbacks
     {
+        /// <summary>
+        /// Maps <see cref="CollidableReference"/> entries to their <see cref="SimpleMaterial"/>.
+        /// </summary>
+        /// <remarks>
+        /// The narrow phase callbacks need some way to get the material data for this demo, but there's no requirement that you use the <see cref="CollidableProperty{T}"/> type.
+        /// It's just a fairly convenient and simple option.
+        /// </remarks>
         public CollidableProperty<SimpleMaterial> CollidableMaterials;
 
         public void Initialize(Simulation simulation)
@@ -27,7 +50,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
@@ -42,7 +65,7 @@ public bool AllowContactGeneration(int workerIndex, CollidablePair pair, int chi
         }
 
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public unsafe bool ConfigureContactManifold<TManifold>(int workerIndex, CollidablePair pair, ref TManifold manifold, out PairMaterialProperties pairMaterial) where TManifold : unmanaged, IContactManifold<TManifold>
+        public bool ConfigureContactManifold<TManifold>(int workerIndex, CollidablePair pair, ref TManifold manifold, out PairMaterialProperties pairMaterial) where TManifold : unmanaged, IContactManifold<TManifold>
         {
             //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];
@@ -64,48 +87,52 @@ public void Dispose()
         }
     }
 
-    /// <summary>
-    /// Shows how to configure things to bounce in the absence of a coefficient of restitution.
-    /// </summary>
-    /// <remarks>
-    /// 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.
-    /// </remarks>
-    public class BouncinessDemo : Demo
+    public override void Initialize(ContentArchive content, Camera camera)
     {
-        public unsafe override void Initialize(ContentArchive content, Camera camera)
-        {
-            camera.Position = new Vector3(0, 40, 200);
-            camera.Yaw = 0;
-            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.
-            //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<SimpleMaterial>();
-            Simulation = Simulation.Create(BufferPool, new BounceCallbacks() { CollidableMaterials = collidableMaterials }, new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SubsteppingTimestepper(4), solverIterationCount: 2);
+        camera.Position = new Vector3(0, 40, 200);
+        camera.Yaw = 0;
+        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 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. It defaults to 1 velocity iteration per substep.)
+        var collidableMaterials = new CollidableProperty<SimpleMaterial>();
+        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);
-            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 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)
+        for (int i = 0; i < 100; ++i)
+        {
+            for (int j = 0; j < 100; ++j)
             {
-                for (int j = 0; j < 100; ++j)
-                {
-                    //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.)
-                    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) };
-                }
+                //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.
+                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.Statics.Add(new StaticDescription(new Vector3(0, -15f, 0), new CollidableDescription(Simulation.Shapes.Add(new Box(2500, 30, 2500)), 0.1f)))) =
-                new SimpleMaterial { FrictionCoefficient = 1, 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("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);
     }
 }
diff --git a/Demos/Demos/Cars/CarCallbacks.cs b/Demos/Demos/Cars/CarCallbacks.cs
index 5f3bae299..fcc03b7a9 100644
--- a/Demos/Demos/Cars/CarCallbacks.cs
+++ b/Demos/Demos/Cars/CarCallbacks.cs
@@ -4,66 +4,65 @@
 using BepuPhysics.CollisionDetection;
 using BepuPhysics.Constraints;
 
-namespace Demos.Demos.Cars
+namespace Demos.Demos.Cars;
+
+struct CarBodyProperties
+{
+    public SubgroupCollisionFilter Filter;
+    public float Friction;
+}
+
+/// <summary>
+/// For the car demo, we want both wheel-body collision filtering and different friction for wheels versus the car body.
+/// </summary>
+struct CarCallbacks : INarrowPhaseCallbacks
 {
-    struct CarBodyProperties
+    public CollidableProperty<CarBodyProperties> Properties;
+    public void Initialize(Simulation simulation)
     {
-        public SubgroupCollisionFilter Filter;
-        public float Friction;
+        Properties.Initialize(simulation);
     }
 
-    /// <summary>
-    /// For the car demo, we want both wheel-body collision filtering and different friction for wheels versus the car body.
-    /// </summary>
-    struct CarCallbacks : INarrowPhaseCallbacks
+    [MethodImpl(MethodImplOptions.AggressiveInlining)]
+    public bool AllowContactGeneration(int workerIndex, CollidableReference a, CollidableReference b, ref float speculativeMargin)
     {
-        public CollidableProperty<CarBodyProperties> Properties;
-        public void Initialize(Simulation simulation)
-        {
-            Properties.Initialize(simulation);
-        }
-
-        [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public bool AllowContactGeneration(int workerIndex, CollidableReference a, CollidableReference b)
+        //It's impossible for two statics to collide, and pairs are sorted such that bodies always come before statics.
+        if (b.Mobility != CollidableMobility.Static)
         {
-            //It's impossible for two statics to collide, and pairs are sorted such that bodies always come before statics.
-            if (b.Mobility != CollidableMobility.Static)
-            {
-                return SubgroupCollisionFilter.AllowCollision(Properties[a.BodyHandle].Filter, Properties[b.BodyHandle].Filter);
-            }
-            return a.Mobility == CollidableMobility.Dynamic || b.Mobility == CollidableMobility.Dynamic;
+            return SubgroupCollisionFilter.AllowCollision(Properties[a.BodyHandle].Filter, Properties[b.BodyHandle].Filter);
         }
+        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 bool AllowContactGeneration(int workerIndex, CollidablePair pair, int childIndexA, int childIndexB)
+    {
+        return true;
+    }
 
-        [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public unsafe bool ConfigureContactManifold<TManifold>(int workerIndex, CollidablePair pair, ref TManifold manifold, out PairMaterialProperties pairMaterial) where TManifold : unmanaged, IContactManifold<TManifold>
+    [MethodImpl(MethodImplOptions.AggressiveInlining)]
+    public bool ConfigureContactManifold<TManifold>(int workerIndex, CollidablePair pair, ref TManifold manifold, out PairMaterialProperties pairMaterial) where TManifold : unmanaged, IContactManifold<TManifold>
+    {
+        pairMaterial.FrictionCoefficient = Properties[pair.A.BodyHandle].Friction;
+        if (pair.B.Mobility != CollidableMobility.Static)
         {
-            pairMaterial.FrictionCoefficient = Properties[pair.A.BodyHandle].Friction;
-            if (pair.B.Mobility != CollidableMobility.Static)
-            {
-                //If two bodies collide, just average the friction.
-                pairMaterial.FrictionCoefficient = (pairMaterial.FrictionCoefficient + Properties[pair.B.BodyHandle].Friction) * 0.5f;
-            }
-            pairMaterial.MaximumRecoveryVelocity = 2f;
-            pairMaterial.SpringSettings = new SpringSettings(30, 1);
-            return true;
+            //If two bodies collide, just average the friction.
+            pairMaterial.FrictionCoefficient = (pairMaterial.FrictionCoefficient + Properties[pair.B.BodyHandle].Friction) * 0.5f;
         }
+        pairMaterial.MaximumRecoveryVelocity = 2f;
+        pairMaterial.SpringSettings = new SpringSettings(30, 1);
+        return true;
+    }
 
 
-        [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public unsafe 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()
-        {
-            Properties.Dispose();
-        }
+    public void Dispose()
+    {
+        Properties.Dispose();
     }
 }
\ No newline at end of file
diff --git a/Demos/Demos/Cars/CarDemo.cs b/Demos/Demos/Cars/CarDemo.cs
index dd4f07a98..be03e43ad 100644
--- a/Demos/Demos/Cars/CarDemo.cs
+++ b/Demos/Demos/Cars/CarDemo.cs
@@ -11,136 +11,135 @@
 using DemoUtilities;
 using OpenTK.Input;
 
-namespace Demos.Demos.Cars
+namespace Demos.Demos.Cars;
+
+public class CarDemo : Demo
 {
-    public class CarDemo : Demo
+    SimpleCarController playerController;
+
+    struct AIController
     {
-        SimpleCarController playerController;
+        public SimpleCarController Controller;
+        public float LaneOffset;
+    }
 
-        struct AIController
+    Buffer<AIController> aiControllers;
+    RaceTrack raceTrack;
+
+    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 ToggleCar = Key.C;
+    public override void Initialize(ContentArchive content, Camera camera)
+    {
+        camera.Position = new Vector3(0, 5, 10);
+        camera.Yaw = 0;
+        camera.Pitch = 0;
+
+        var properties = new CollidableProperty<CarBodyProperties>();
+
+        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);
+        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 float x = 0.9f;
+        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 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,
+            wheelBaseLength: wheelBaseLength, wheelBaseWidth: wheelBaseWidth, ackermanSteering: 1);
+
+        //Create a bunch of AI cars to race against.
+        const int aiCount = 384;
+        BufferPool.Take(aiCount, out aiControllers);
+
+
+        const int planeWidth = 257;
+        const float scale = 3;
+        Vector2 terrainPosition = new Vector2(1 - planeWidth, 1 - planeWidth) * scale * 0.5f;
+        raceTrack = new RaceTrack { QuadrantRadius = (planeWidth - 32) * scale * 0.25f, Center = default };
+        var random = new Random(5);
+
+        //Add some building-ish landmarks in the middle of each of the four racetrack quadrants.
+        for (int i = 0; i < 4; ++i)
         {
-            public SimpleCarController Controller;
-            public float LaneOffset;
+            var landmarkCenter = new Vector3((i & 1) * raceTrack.QuadrantRadius * 2 - raceTrack.QuadrantRadius, -20, (i & 2) * raceTrack.QuadrantRadius - raceTrack.QuadrantRadius);
+            var landmarkMin = landmarkCenter - new Vector3(raceTrack.QuadrantRadius * 0.5f, 0, raceTrack.QuadrantRadius * 0.5f);
+            var landmarkSpan = new Vector3(raceTrack.QuadrantRadius, 0, raceTrack.QuadrantRadius);
+            for (int j = 0; j < 25; ++j)
+            {
+                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(random.NextSingle(), random.NextSingle(), random.NextSingle()),
+                    QuaternionEx.CreateFromAxisAngle(Vector3.UnitY, random.NextSingle() * MathF.PI),
+                    Simulation.Shapes.Add(buildingShape)));
+            }
         }
 
-        Buffer<AIController> aiControllers;
-        RaceTrack raceTrack;
-
-        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 ToggleCar = Key.C;
-        public override void Initialize(ContentArchive content, Camera camera)
+        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);
+
+        for (int i = 0; i < aiCount; ++i)
         {
-            camera.Position = new Vector3(0, 5, 10);
-            camera.Yaw = 0;
-            camera.Pitch = 0;
-
-            var properties = new CollidableProperty<CarBodyProperties>();
-            //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());
-
-            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 float x = 0.9f;
-            const float y = -0.1f;
-            const float frontZ = 1.7f;
-            const float backZ = -1.7f;
-            playerController = new SimpleCarController(SimpleCar.Create(Simulation, properties, new RigidPose(new Vector3(0, 10, 0), Quaternion.Identity), bodyShapeIndex, bodyInertia, 0.5f, wheelShapeIndex, wheelInertia, 2f,
+            //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,
                 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);
-
-            //Create a bunch of AI cars to race against.
-            const int aiCount = 384;
-            BufferPool.Take(aiCount, out aiControllers);
-
+                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);
 
-            const int planeWidth = 257;
-            const float scale = 3;
-            Vector2 terrainPosition = new Vector2(1 - planeWidth, 1 - planeWidth) * scale * 0.5f;
-            raceTrack = new RaceTrack { QuadrantRadius = (planeWidth - 32) * scale * 0.25f, Center = default };
-            var random = new Random(5);
-
-            //Add some building-ish landmarks in the middle of each of the four racetrack quadrants.
-            for (int i = 0; i < 4; ++i)
-            {
-                var landmarkCenter = new Vector3((i & 1) * raceTrack.QuadrantRadius * 2 - raceTrack.QuadrantRadius, -20, (i & 2) * raceTrack.QuadrantRadius - raceTrack.QuadrantRadius);
-                var landmarkMin = landmarkCenter - new Vector3(raceTrack.QuadrantRadius * 0.5f, 0, raceTrack.QuadrantRadius * 0.5f);
-                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);
-                    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)));
-                }
-            }
-
-            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);
+            aiControllers[i].LaneOffset = random.NextSingle() * 20 - 10;
+        }
 
-            for (int i = 0; i < aiCount; ++i)
+        var planeMesh = DemoMeshHelper.CreateDeformedPlane(planeWidth, planeWidth,
+            (int vX, int vY) =>
             {
-                //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);
-                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);
-                aiControllers[i].LaneOffset = (float)random.NextDouble() * 20 - 10;
-            }
-
-
-            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;
-                    var distanceToTrack = raceTrack.GetDistance(vertexPosition);
-                    var trackWeight = MathF.Min(1f, 3f / (distanceToTrack * 0.1f + 1f));
-                    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);
-            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 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;
+                var distanceToTrack = raceTrack.GetDistance(vertexPosition);
+                var trackWeight = MathF.Min(1f, 3f / (distanceToTrack * 0.1f + 1f));
+                var height = trackWeight * -10f + terrainHeight * (1 - trackWeight);
+                return new Vector3(vertexPosition.X, height + edgeRamp, vertexPosition.Y);
+
+            }, 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)));
+    }
 
-        bool playerControlActive = true;
-        public override void Update(Window window, Camera camera, Input input, float dt)
+    bool playerControlActive = true;
+    public override void Update(Window window, Camera camera, Input input, float dt)
+    {
+        if (input != null)
         {
             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 = 1 / 60f;
             if (playerControlActive)
             {
                 float steeringSum = 0;
@@ -154,66 +153,68 @@ public override void Update(Window window, Camera camera, Input input, float dt)
                 }
                 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 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));
             }
+        }
 
-            for (int i = 0; i < aiControllers.Length; ++i)
+        for (int i = 0; i < aiControllers.Length; ++i)
+        {
+            ref var ai = ref aiControllers[i];
+            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);
+            var predictedLocation = new Vector2(pose.Position.X, pose.Position.Z) + new Vector2(orientation.Z.X, orientation.Z.Z) * (5 + forwardVelocity * 2);
+            raceTrack.GetClosestPoint(predictedLocation, ai.LaneOffset, out var closestPoint, out var flowDirection);
+            float steeringAngle;
+            if (flowDirection.X * orientation.Z.X + flowDirection.Y * orientation.Z.Z < 0)
             {
-                ref var ai = ref aiControllers[i];
-                var body = Simulation.Bodies.GetBodyReference(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);
-                var predictedLocation = new Vector2(pose.Position.X, pose.Position.Z) + new Vector2(orientation.Z.X, orientation.Z.Z) * (5 + forwardVelocity * 2);
-                raceTrack.GetClosestPoint(predictedLocation, ai.LaneOffset, out var closestPoint, out var flowDirection);
-                float steeringAngle;
-                if (flowDirection.X * orientation.Z.X + flowDirection.Y * orientation.Z.Z < 0)
-                {
-                    //Don't drive against traffic!
-                    steeringAngle = ai.Controller.MaximumSteeringAngle;
-                }
-                else
-                {
-                    var toClosestPoint = closestPoint - new Vector2(pose.Position.X, pose.Position.Z);
-                    var horizontalOffset = orientation.X.X * toClosestPoint.X + orientation.X.Z * toClosestPoint.Y;
-                    var forwardOffset = orientation.Z.X * toClosestPoint.X + orientation.Z.Z * toClosestPoint.Y;
-                    steeringAngle = MathF.Atan2(horizontalOffset, forwardOffset);
-                }
-                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);
+                //Don't drive against traffic!
+                steeringAngle = ai.Controller.MaximumSteeringAngle;
             }
-
-            base.Update(window, camera, input, dt);
+            else
+            {
+                var toClosestPoint = closestPoint - new Vector2(pose.Position.X, pose.Position.Z);
+                var horizontalOffset = orientation.X.X * toClosestPoint.X + orientation.X.Z * toClosestPoint.Y;
+                var forwardOffset = orientation.Z.X * toClosestPoint.X + orientation.Z.Z * toClosestPoint.Y;
+                steeringAngle = MathF.Atan2(horizontalOffset, forwardOffset);
+            }
+            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, TimestepDuration, steeringAngle, speedFraction, steeringAngle < 0.05f, steeringAngle > MathF.PI * 0.2f && forwardVelocity > ai.Controller.ForwardSpeed * 0.6f);
         }
 
-        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;
-        }
+        base.Update(window, camera, input, dt);
+    }
 
-        public override void Render(Renderer renderer, Camera camera, Input input, TextBuilder text, Font font)
-        {
-            if (playerControlActive)
-            {
-                var carBody = new BodyReference(playerController.Car.Body, Simulation.Bodies);
-                QuaternionEx.TransformUnitY(carBody.Pose.Orientation, out var carUp);
-                camera.Position = carBody.Pose.Position + carUp * 1.3f + camera.Backward * 8;
-            }
+    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;
+    }
 
-            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);
-            base.Render(renderer, camera, input, text, font);
+    public override void Render(Renderer renderer, Camera camera, Input input, TextBuilder text, Font font)
+    {
+        if (playerControlActive)
+        {
+            var carBody = new BodyReference(playerController.Car.Body, Simulation.Bodies);
+            QuaternionEx.TransformUnitY(carBody.Pose.Orientation, out var carUp);
+            camera.Position = carBody.Pose.Position + carUp * 1.3f + camera.Backward * 8;
         }
+
+        var textHeight = 16;
+        var position = new Vector2(32, renderer.Surface.Resolution.Y - 128);
+        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);
     }
 }
\ No newline at end of file
diff --git a/Demos/Demos/Cars/SimpleCar.cs b/Demos/Demos/Cars/SimpleCar.cs
index 97ebb74ce..791f8c2c8 100644
--- a/Demos/Demos/Cars/SimpleCar.cs
+++ b/Demos/Demos/Cars/SimpleCar.cs
@@ -4,104 +4,103 @@
 using BepuPhysics.Constraints;
 using BepuUtilities;
 
-namespace Demos.Demos.Cars
+namespace Demos.Demos.Cars;
+
+struct SimpleCar
 {
-    struct SimpleCar
-    {
-        public BodyHandle Body;
-        public WheelHandles FrontLeftWheel;
-        public WheelHandles FrontRightWheel;
-        public WheelHandles BackLeftWheel;
-        public WheelHandles BackRightWheel;
+    public BodyHandle Body;
+    public WheelHandles FrontLeftWheel;
+    public WheelHandles FrontRightWheel;
+    public WheelHandles BackLeftWheel;
+    public WheelHandles BackRightWheel;
 
-        private Vector3 suspensionDirection;
-        private AngularHinge hingeDescription;
+    private Vector3 suspensionDirection;
+    private AngularHinge hingeDescription;
 
-        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);
-        }
+    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, steeredHinge);
+    }
 
-        public void SetSpeed(Simulation simulation, in WheelHandles wheel, float speed, float maximumForce)
+    public void SetSpeed(Simulation simulation, in WheelHandles wheel, float speed, float maximumForce)
+    {
+        simulation.Solver.ApplyDescription(wheel.Motor, new AngularAxisMotor
         {
-            simulation.Solver.ApplyDescription(wheel.Motor, new AngularAxisMotor
-            {
-                LocalAxisA = new Vector3(0, -1, 0),
-                Settings = new MotorSettings(maximumForce, 1e-6f),
-                TargetVelocity = speed
-            });
-        }
+            LocalAxisA = new Vector3(0, -1, 0),
+            Settings = new MotorSettings(maximumForce, 1e-6f),
+            TargetVelocity = speed
+        });
+    }
 
-        public static WheelHandles CreateWheel(Simulation simulation, CollidableProperty<CarBodyProperties> 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)
-        {
-            RigidPose wheelPose;
-            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)));
+    public static WheelHandles CreateWheel(Simulation simulation, CollidableProperty<CarBodyProperties> properties, in RigidPose bodyPose,
+        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);
+        QuaternionEx.ConcatenateWithoutOverlap(localWheelOrientation, bodyPose.Orientation, out wheelPose.Orientation);
+        WheelHandles handles;
+        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
-            {
-                LocalPlaneNormal = suspensionDirection,
-                TargetOffset = suspensionLength,
-                LocalOffsetA = bodyToWheelSuspension,
-                LocalOffsetB = default,
-                ServoSettings = ServoSettings.Default,
-                SpringSettings = suspensionSettings
-            });
-            handles.SuspensionTrack = simulation.Solver.Add(bodyHandle, handles.Wheel, new PointOnLineServo
-            {
-                LocalDirection = suspensionDirection,
-                LocalOffsetA = bodyToWheelSuspension,
-                LocalOffsetB = default,
-                ServoSettings = ServoSettings.Default,
-                SpringSettings = new SpringSettings(30, 1)
-            });
-            //We're treating braking and acceleration as the same thing. It is, after all, a *simple* car! Maybe it's electric or something.
-            //It would be fairly easy to split brakes and drive motors into different motors.
-            handles.Motor = simulation.Solver.Add(handles.Wheel, bodyHandle, new AngularAxisMotor
-            {
-                LocalAxisA = new Vector3(0, 1, 0),
-                Settings = default,
-                TargetVelocity = default
-            });
-            handles.Hinge = simulation.Solver.Add(bodyHandle, handles.Wheel, hingeDescription);
-            //The demos SubgroupCollisionFilter is pretty simple and only tests one direction, so we make the non-colliding relationship symmetric.
-            ref var wheelProperties = ref properties.Allocate(handles.Wheel);
-            wheelProperties = new CarBodyProperties { Filter = new SubgroupCollisionFilter(bodyHandle.Value, 1), Friction = wheelFriction };
-            SubgroupCollisionFilter.DisableCollision(ref wheelProperties.Filter, ref bodyFilter);
+        handles.SuspensionSpring = simulation.Solver.Add(bodyHandle, handles.Wheel, new LinearAxisServo
+        {
+            LocalPlaneNormal = suspensionDirection,
+            TargetOffset = suspensionLength,
+            LocalOffsetA = bodyToWheelSuspension,
+            LocalOffsetB = default,
+            ServoSettings = ServoSettings.Default,
+            SpringSettings = suspensionSettings
+        });
+        handles.SuspensionTrack = simulation.Solver.Add(bodyHandle, handles.Wheel, new PointOnLineServo
+        {
+            LocalDirection = suspensionDirection,
+            LocalOffsetA = bodyToWheelSuspension,
+            LocalOffsetB = default,
+            ServoSettings = ServoSettings.Default,
+            SpringSettings = new SpringSettings(30, 1)
+        });
+        //We're treating braking and acceleration as the same thing. It is, after all, a *simple* car! Maybe it's electric or something.
+        //It would be fairly easy to split brakes and drive motors into different motors.
+        handles.Motor = simulation.Solver.Add(handles.Wheel, bodyHandle, new AngularAxisMotor
+        {
+            LocalAxisA = new Vector3(0, 1, 0),
+            Settings = default,
+            TargetVelocity = default
+        });
+        handles.Hinge = simulation.Solver.Add(bodyHandle, handles.Wheel, hingeDescription);
+        //The demos SubgroupCollisionFilter is pretty simple and only tests one direction, so we make the non-colliding relationship symmetric.
+        ref var wheelProperties = ref properties.Allocate(handles.Wheel);
+        wheelProperties = new CarBodyProperties { Filter = new SubgroupCollisionFilter(bodyHandle.Value, 1), Friction = wheelFriction };
+        SubgroupCollisionFilter.DisableCollision(ref wheelProperties.Filter, ref bodyFilter);
 
-            return handles;
-        }
+        return handles;
+    }
 
-        public static SimpleCar Create(Simulation simulation, CollidableProperty<CarBodyProperties> 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)
+    public static SimpleCar Create(Simulation simulation, CollidableProperty<CarBodyProperties> properties, in RigidPose pose,
+        TypedIndex bodyShape, BodyInertia bodyInertia, float bodyFriction, TypedIndex wheelShape, BodyInertia wheelInertia, float wheelFriction,
+        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));
+        ref var bodyProperties = ref properties.Allocate(car.Body);
+        bodyProperties = new CarBodyProperties { Friction = bodyFriction, Filter = new SubgroupCollisionFilter(car.Body.Value, 0) };
+        QuaternionEx.TransformUnitY(localWheelOrientation, out var wheelAxis);
+        car.hingeDescription = new AngularHinge
         {
-            SimpleCar car;
-            car.Body = simulation.Bodies.Add(BodyDescription.CreateDynamic(pose, bodyInertia, new CollidableDescription(bodyShape, 0.1f), new BodyActivityDescription(0.01f)));
-            ref var bodyProperties = ref properties.Allocate(car.Body);
-            bodyProperties = new CarBodyProperties { Friction = bodyFriction, Filter = new SubgroupCollisionFilter(car.Body.Value, 0) };
-            QuaternionEx.TransformUnitY(localWheelOrientation, out var wheelAxis);
-            car.hingeDescription = new AngularHinge
-            {
-                LocalHingeAxisA = wheelAxis,
-                LocalHingeAxisB = new Vector3(0, 1, 0),
-                SpringSettings = new SpringSettings(30, 1)
-            };
-            car.suspensionDirection = suspensionDirection;
-            car.BackLeftWheel = CreateWheel(simulation, properties, pose, wheelShape, wheelInertia, wheelFriction, car.Body, ref bodyProperties.Filter, bodyToBackLeftSuspension, suspensionDirection, suspensionLength, car.hingeDescription, suspensionSettings, localWheelOrientation);
-            car.BackRightWheel = CreateWheel(simulation, properties, pose, wheelShape, wheelInertia, wheelFriction, car.Body, ref bodyProperties.Filter, bodyToBackRightSuspension, suspensionDirection, suspensionLength, car.hingeDescription, suspensionSettings, localWheelOrientation);
-            car.FrontLeftWheel = CreateWheel(simulation, properties, pose, wheelShape, wheelInertia, wheelFriction, car.Body, ref bodyProperties.Filter, bodyToFrontLeftSuspension, suspensionDirection, suspensionLength, car.hingeDescription, suspensionSettings, localWheelOrientation);
-            car.FrontRightWheel = CreateWheel(simulation, properties, pose, wheelShape, wheelInertia, wheelFriction, car.Body, ref bodyProperties.Filter, bodyToFrontRightSuspension, suspensionDirection, suspensionLength, car.hingeDescription, suspensionSettings, localWheelOrientation);
-            return car;
-        }
-
+            LocalHingeAxisA = wheelAxis,
+            LocalHingeAxisB = new Vector3(0, 1, 0),
+            SpringSettings = new SpringSettings(30, 1)
+        };
+        car.suspensionDirection = suspensionDirection;
+        car.BackLeftWheel = CreateWheel(simulation, properties, pose, wheelShape, wheelInertia, wheelFriction, car.Body, ref bodyProperties.Filter, bodyToBackLeftSuspension, suspensionDirection, suspensionLength, car.hingeDescription, suspensionSettings, localWheelOrientation);
+        car.BackRightWheel = CreateWheel(simulation, properties, pose, wheelShape, wheelInertia, wheelFriction, car.Body, ref bodyProperties.Filter, bodyToBackRightSuspension, suspensionDirection, suspensionLength, car.hingeDescription, suspensionSettings, localWheelOrientation);
+        car.FrontLeftWheel = CreateWheel(simulation, properties, pose, wheelShape, wheelInertia, wheelFriction, car.Body, ref bodyProperties.Filter, bodyToFrontLeftSuspension, suspensionDirection, suspensionLength, car.hingeDescription, suspensionSettings, localWheelOrientation);
+        car.FrontRightWheel = CreateWheel(simulation, properties, pose, wheelShape, wheelInertia, wheelFriction, car.Body, ref bodyProperties.Filter, bodyToFrontRightSuspension, suspensionDirection, suspensionLength, car.hingeDescription, suspensionSettings, localWheelOrientation);
+        return car;
     }
+
 }
\ No newline at end of file
diff --git a/Demos/Demos/Cars/SimpleCarController.cs b/Demos/Demos/Cars/SimpleCarController.cs
index b46bf2665..eeca9390e 100644
--- a/Demos/Demos/Cars/SimpleCarController.cs
+++ b/Demos/Demos/Cars/SimpleCarController.cs
@@ -1,106 +1,148 @@
 using System;
 using BepuPhysics;
 
-namespace Demos.Demos.Cars
+namespace Demos.Demos.Cars;
+
+struct SimpleCarController
 {
-    struct SimpleCarController
-    {
-        public SimpleCar Car;
+    public SimpleCar Car;
 
-        private float steeringAngle;
+    private float steeringAngle;
 
-        public readonly float SteeringAngle { get { return steeringAngle; } }
+    public readonly float SteeringAngle { get { return steeringAngle; } }
 
-        public float SteeringSpeed;
-        public float MaximumSteeringAngle;
+    public float SteeringSpeed;
+    public float MaximumSteeringAngle;
 
-        public float ForwardSpeed;
-        public float ForwardForce;
-        public float ZoomMultiplier;
-        public float BackwardSpeed;
-        public float BackwardForce;
-        public float IdleForce;
-        public float BrakeForce;
+    public float ForwardSpeed;
+    public float ForwardForce;
+    public float ZoomMultiplier;
+    public float BackwardSpeed;
+    public float BackwardForce;
+    public float IdleForce;
+    public float BrakeForce;
+    public float WheelBaseLength;
+    public float WheelBaseWidth;
+    /// <summary>
+    /// 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.
+    /// </summary>
+    public float AckermanSteering;
 
-        //Track the previous state to force wakeups if the constraint targets have changed.
-        private float previousTargetSpeed;
-        private float previousTargetForce;
+    //Track the previous state to force wakeups if the constraint targets have changed.
+    private float previousTargetSpeed;
+    private float previousTargetForce;
 
-        public SimpleCarController(SimpleCar car,
-            float forwardSpeed, float forwardForce, float zoomMultiplier, float backwardSpeed, float backwardForce, float idleForce, float brakeForce,
-            float steeringSpeed, float maximumSteeringAngle)
-        {
-            Car = car;
-            ForwardSpeed = forwardSpeed;
-            ForwardForce = forwardForce;
-            ZoomMultiplier = zoomMultiplier;
-            BackwardSpeed = backwardSpeed;
-            BackwardForce = backwardForce;
-            IdleForce = idleForce;
-            BrakeForce = brakeForce;
-            SteeringSpeed = steeringSpeed;
-            MaximumSteeringAngle = maximumSteeringAngle;
+    public SimpleCarController(SimpleCar car,
+        float forwardSpeed, float forwardForce, float zoomMultiplier, float backwardSpeed, float backwardForce, float idleForce, float brakeForce,
+        float steeringSpeed, float maximumSteeringAngle, float wheelBaseLength, float wheelBaseWidth, float ackermanSteering)
+    {
+        Car = car;
+        ForwardSpeed = forwardSpeed;
+        ForwardForce = forwardForce;
+        ZoomMultiplier = zoomMultiplier;
+        BackwardSpeed = backwardSpeed;
+        BackwardForce = backwardForce;
+        IdleForce = idleForce;
+        BrakeForce = brakeForce;
+        SteeringSpeed = steeringSpeed;
+        MaximumSteeringAngle = maximumSteeringAngle;
+        WheelBaseLength = wheelBaseLength;
+        WheelBaseWidth = wheelBaseWidth;
+        AckermanSteering = ackermanSteering;
 
-            steeringAngle = 0;
-            previousTargetForce = 0;
-            previousTargetSpeed = 0;
-        }
-        public void Update(Simulation simulation, float dt, float targetSteeringAngle, float targetSpeedFraction, bool zoom, bool brake)
+        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)
         {
-            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)
-            {
-                //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);
-            }
-            float newTargetSpeed, newTargetForce;
-            bool allWheels;
-            if (brake)
+            float leftSteeringAngle;
+            float rightSteeringAngle;
+
+            float steeringAngleAbs = MathF.Abs(steeringAngle);
+
+            if (AckermanSteering > 0 && steeringAngleAbs > 1e-6)
             {
-                newTargetSpeed = 0;
-                newTargetForce = BrakeForce;
-                allWheels = true;
+                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));
+                    rightSteeringAngle = steeringAngle + (rightSteeringAngle - steeringAngleAbs) * AckermanSteering;
+                    
+                    leftSteeringAngle = MathF.Atan(WheelBaseLength / (turnRadius + wheelBaseHalfWidth));
+                    leftSteeringAngle = steeringAngle + (leftSteeringAngle - steeringAngleAbs) * AckermanSteering;
+                }
+                else
+                {
+                    rightSteeringAngle = MathF.Atan(WheelBaseLength / (turnRadius + wheelBaseHalfWidth));
+                    rightSteeringAngle = steeringAngle - (rightSteeringAngle - steeringAngleAbs) * AckermanSteering;
+
+                    leftSteeringAngle = MathF.Atan(WheelBaseLength / (turnRadius - wheelBaseHalfWidth));
+                    leftSteeringAngle = steeringAngle - (leftSteeringAngle - steeringAngleAbs) * AckermanSteering;
+                }
             }
-            else if (targetSpeedFraction > 0)
+            else
             {
-                newTargetForce = zoom ? ForwardForce * ZoomMultiplier : ForwardForce;
-                newTargetSpeed = targetSpeedFraction * (zoom ? ForwardSpeed * ZoomMultiplier : ForwardSpeed);
-                allWheels = false;
+                leftSteeringAngle = steeringAngle;
+                rightSteeringAngle = steeringAngle;
             }
-            else if (targetSpeedFraction < 0)
+
+            //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, leftSteeringAngle);
+            Car.Steer(simulation, Car.FrontRightWheel, rightSteeringAngle);
+        }
+        float newTargetSpeed, newTargetForce;
+        bool allWheels;
+        if (brake)
+        {
+            newTargetSpeed = 0;
+            newTargetForce = BrakeForce;
+            allWheels = true;
+        }
+        else if (targetSpeedFraction > 0)
+        {
+            newTargetForce = zoom ? ForwardForce * ZoomMultiplier : ForwardForce;
+            newTargetSpeed = targetSpeedFraction * (zoom ? ForwardSpeed * ZoomMultiplier : ForwardSpeed);
+            allWheels = false;
+        }
+        else if (targetSpeedFraction < 0)
+        {
+            newTargetForce = zoom ? BackwardForce * ZoomMultiplier : BackwardForce;
+            newTargetSpeed = targetSpeedFraction * (zoom ? BackwardSpeed * ZoomMultiplier : BackwardSpeed);
+            allWheels = false;
+        }
+        else
+        {
+            newTargetForce = IdleForce;
+            newTargetSpeed = 0;
+            allWheels = true;
+        }
+        if (previousTargetSpeed != newTargetSpeed || previousTargetForce != newTargetForce)
+        {
+            previousTargetSpeed = newTargetSpeed;
+            previousTargetForce = newTargetForce;
+            Car.SetSpeed(simulation, Car.FrontLeftWheel, newTargetSpeed, newTargetForce);
+            Car.SetSpeed(simulation, Car.FrontRightWheel, newTargetSpeed, newTargetForce);
+            if (allWheels)
             {
-                newTargetForce = zoom ? BackwardForce * ZoomMultiplier : BackwardForce;
-                newTargetSpeed = targetSpeedFraction * (zoom ? BackwardSpeed * ZoomMultiplier : BackwardSpeed);
-                allWheels = false;
+                Car.SetSpeed(simulation, Car.BackLeftWheel, newTargetSpeed, newTargetForce);
+                Car.SetSpeed(simulation, Car.BackRightWheel, newTargetSpeed, newTargetForce);
             }
             else
             {
-                newTargetForce = IdleForce;
-                newTargetSpeed = 0;
-                allWheels = true;
-            }
-            if (previousTargetSpeed != newTargetSpeed || previousTargetForce != newTargetForce)
-            {
-                previousTargetSpeed = newTargetSpeed;
-                previousTargetForce = newTargetForce;
-                Car.SetSpeed(simulation, Car.FrontLeftWheel, newTargetSpeed, newTargetForce);
-                Car.SetSpeed(simulation, Car.FrontRightWheel, newTargetSpeed, newTargetForce);
-                if (allWheels)
-                {
-                    Car.SetSpeed(simulation, Car.BackLeftWheel, newTargetSpeed, newTargetForce);
-                    Car.SetSpeed(simulation, Car.BackRightWheel, newTargetSpeed, newTargetForce);
-                }
-                else
-                {
-                    Car.SetSpeed(simulation, Car.BackLeftWheel, 0, 0);
-                    Car.SetSpeed(simulation, Car.BackRightWheel, 0, 0);
-                }
+                Car.SetSpeed(simulation, Car.BackLeftWheel, 0, 0);
+                Car.SetSpeed(simulation, Car.BackRightWheel, 0, 0);
             }
         }
     }
diff --git a/Demos/Demos/Cars/WheelHandles.cs b/Demos/Demos/Cars/WheelHandles.cs
index 9a9ea421f..6f10329bc 100644
--- a/Demos/Demos/Cars/WheelHandles.cs
+++ b/Demos/Demos/Cars/WheelHandles.cs
@@ -1,13 +1,12 @@
 using BepuPhysics;
 
-namespace Demos.Demos.Cars
+namespace Demos.Demos.Cars;
+
+struct WheelHandles
 {
-    struct WheelHandles
-    {
-        public BodyHandle Wheel;
-        public ConstraintHandle SuspensionSpring;
-        public ConstraintHandle SuspensionTrack;
-        public ConstraintHandle Hinge;
-        public ConstraintHandle Motor;
-    }
+    public BodyHandle Wheel;
+    public ConstraintHandle SuspensionSpring;
+    public ConstraintHandle SuspensionTrack;
+    public ConstraintHandle Hinge;
+    public ConstraintHandle Motor;
 }
\ No newline at end of file
diff --git a/Demos/Demos/ChainFountainDemo.cs b/Demos/Demos/ChainFountainDemo.cs
new file mode 100644
index 000000000..a0aae7ea5
--- /dev/null
+++ b/Demos/Demos/ChainFountainDemo.cs
@@ -0,0 +1,124 @@
+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;
+
+/// <summary>
+/// A segmented rope chain thing launches itself out of a container. See also: https://en.wikipedia.org/wiki/Chain_fountain
+/// </summary>
+public class ChainFountainDemo : Demo
+{
+    public override void Initialize(ContentArchive content, Camera camera)
+    {
+        camera.Position = new Vector3(5.65f, 3, -23);
+        camera.Yaw = MathF.PI;
+        camera.Pitch = 0;
+
+        var filters = new CollidableProperty<RopeFilter>();
+        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);
+        var beadDescription = BodyDescription.CreateDynamic(new Vector3(), beadShape.ComputeInertia(1), Simulation.Shapes.Add(beadShape), 0.01f);
+
+        const int beadCount = 4096;
+        var handles = new BodyHandle[beadCount];
+        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 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;
+
+            //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 };
+
+            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.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)
+    {
+        //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();
+        //}
+        //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);
+        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/Characters/CharacterControllers.cs b/Demos/Demos/Characters/CharacterControllers.cs
index bcb474824..8417e37fd 100644
--- a/Demos/Demos/Characters/CharacterControllers.cs
+++ b/Demos/Demos/Characters/CharacterControllers.cs
@@ -10,346 +10,297 @@
 using System.Runtime.CompilerServices;
 using System.Threading;
 
-namespace Demos.Demos.Characters
+namespace Demos.Demos.Characters;
+
+/// <summary>
+/// Raw data for a dynamic character controller instance.
+/// </summary>
+public struct CharacterController
 {
     /// <summary>
-    /// Raw data for a dynamic character controller instance.
+    /// Direction the character is looking in world space. Defines the forward direction for movement.
+    /// </summary>
+    public Vector3 ViewDirection;
+    /// <summary>
+    /// Target horizontal velocity. 
+    /// X component refers to desired velocity along the strafing direction (perpendicular to the view direction projected down to the surface), 
+    /// Y component refers to the desired velocity along the forward direction (aligned with the view direction projected down to the surface).
     /// </summary>
-    public struct CharacterController
+    public Vector2 TargetVelocity;
+    /// <summary>
+    /// If true, the character will try to jump on the next time step. Will be reset to false after being processed.
+    /// </summary>
+    public bool TryJump;
+
+    /// <summary>
+    /// Handle of the body associated with the character.
+    /// </summary>
+    public BodyHandle BodyHandle;
+    /// <summary>
+    /// Character's up direction in the local space of the character's body.
+    /// </summary>
+    public Vector3 LocalUp;
+    /// <summary>
+    /// Velocity at which the character pushes off the support during a jump.
+    /// </summary>
+    public float JumpVelocity;
+    /// <summary>
+    /// Maximum force the character can apply tangent to the supporting surface to move.
+    /// </summary>
+    public float MaximumHorizontalForce;
+    /// <summary>
+    /// Maximum force the character can apply to glue itself to the supporting surface.
+    /// </summary>
+    public float MaximumVerticalForce;
+    /// <summary>
+    /// Cosine of the maximum slope angle that the character can treat as a support.
+    /// </summary>
+    public float CosMaximumSlope;
+    /// <summary>
+    /// Depth threshold beyond which a contact is considered a support if it the normal allows it.
+    /// </summary>
+    public float MinimumSupportDepth;
+    /// <summary>
+    /// Depth threshold beyond which a contact is considered a support if the previous frame had support, even if it isn't deep enough to meet the MinimumSupportDepth.
+    /// </summary>
+    public float MinimumSupportContinuationDepth;
+
+    /// <summary>
+    /// Whether the character is currently supported.
+    /// </summary>
+    public bool Supported;
+    /// <summary>
+    /// Collidable supporting the character, if any. Only valid if Supported is true.
+    /// </summary>
+    public CollidableReference Support;
+    /// <summary>
+    /// Handle of the character's motion constraint, if any. Only valid if Supported is true.
+    /// </summary>
+    public ConstraintHandle MotionConstraintHandle;
+}
+
+/// <summary>
+/// System that manages all the characters in a simulation. Responsible for updating movement constraints based on character goals and contact states.
+/// </summary>
+public unsafe class CharacterControllers : IDisposable
+{
+    /// <summary>
+    /// Gets the simulation to which this set of characters belongs.
+    /// </summary>
+    public Simulation Simulation { get; private set; }
+    BufferPool pool;
+
+    Buffer<int> bodyHandleToCharacterIndex;
+    QuickList<CharacterController> characters;
+
+    /// <summary>
+    /// Gets the number of characters being controlled.
+    /// </summary>
+    public int CharacterCount { get { return characters.Count; } }
+
+    /// <summary>
+    /// Creates a character controller system.
+    /// </summary>
+    /// <param name="pool">Pool to allocate resources from.</param>
+    /// <param name="initialCharacterCapacity">Number of characters to initially allocate space for.</param>
+    /// <param name="initialBodyHandleCapacity">Number of body handles to initially allocate space for in the body handle->character mapping.</param>
+    public CharacterControllers(BufferPool pool, int initialCharacterCapacity = 4096, int initialBodyHandleCapacity = 4096)
     {
-        /// <summary>
-        /// Direction the character is looking in world space. Defines the forward direction for movement.
-        /// </summary>
-        public Vector3 ViewDirection;
-        /// <summary>
-        /// Target horizontal velocity. 
-        /// X component refers to desired velocity along the strafing direction (perpendicular to the view direction projected down to the surface), 
-        /// Y component refers to the desired velocity along the forward direction (aligned with the view direction projected down to the surface).
-        /// </summary>
-        public Vector2 TargetVelocity;
-        /// <summary>
-        /// If true, the character will try to jump on the next time step. Will be reset to false after being processed.
-        /// </summary>
-        public bool TryJump;
-
-        /// <summary>
-        /// Handle of the body associated with the character.
-        /// </summary>
-        public BodyHandle BodyHandle;
-        /// <summary>
-        /// Character's up direction in the local space of the character's body.
-        /// </summary>
-        public Vector3 LocalUp;
-        /// <summary>
-        /// Velocity at which the character pushes off the support during a jump.
-        /// </summary>
-        public float JumpVelocity;
-        /// <summary>
-        /// Maximum force the character can apply tangent to the supporting surface to move.
-        /// </summary>
-        public float MaximumHorizontalForce;
-        /// <summary>
-        /// Maximum force the character can apply to glue itself to the supporting surface.
-        /// </summary>
-        public float MaximumVerticalForce;
-        /// <summary>
-        /// Cosine of the maximum slope angle that the character can treat as a support.
-        /// </summary>
-        public float CosMaximumSlope;
-        /// <summary>
-        /// Depth threshold beyond which a contact is considered a support if it the normal allows it.
-        /// </summary>
-        public float MinimumSupportDepth;
-        /// <summary>
-        /// Depth threshold beyond which a contact is considered a support if the previous frame had support, even if it isn't deep enough to meet the MinimumSupportDepth.
-        /// </summary>
-        public float MinimumSupportContinuationDepth;
-
-        /// <summary>
-        /// Whether the character is currently supported.
-        /// </summary>
-        public bool Supported;
-        /// <summary>
-        /// Collidable supporting the character, if any. Only valid if Supported is true.
-        /// </summary>
-        public CollidableReference Support;
-        /// <summary>
-        /// Handle of the character's motion constraint, if any. Only valid if Supported is true.
-        /// </summary>
-        public ConstraintHandle MotionConstraintHandle;
+        this.pool = pool;
+        characters = new QuickList<CharacterController>(initialCharacterCapacity, pool);
+        ResizeBodyHandleCapacity(initialBodyHandleCapacity);
+        analyzeContactsWorker = AnalyzeContactsWorker;
+        expandBoundingBoxesWorker = ExpandBoundingBoxesWorker;
     }
 
     /// <summary>
-    /// System that manages all the characters in a simulation. Responsible for updating movement constraints based on character goals and contact states.
+    /// Caches the simulation associated with the characters.
     /// </summary>
-    public class CharacterControllers : IDisposable
+    /// <param name="simulation">Simulation to be associated with the characters.</param>
+    public void Initialize(Simulation simulation)
     {
-        /// <summary>
-        /// Gets the simulation to which this set of chracters belongs.
-        /// </summary>
-        public Simulation Simulation { get; private set; }
-        BufferPool pool;
-
-        Buffer<int> bodyHandleToCharacterIndex;
-        QuickList<CharacterController> characters;
-
-        /// <summary>
-        /// Gets the number of characters being controlled.
-        /// </summary>
-        public int CharacterCount { get { return characters.Count; } }
-
-        /// <summary>
-        /// Creates a character controller systme.
-        /// </summary>
-        /// <param name="pool">Pool to allocate resources from.</param>
-        /// <param name="initialCharacterCapacity">Number of characters to initially allocate space for.</param>
-        /// <param name="initialBodyHandleCapacity">Number of body handles to initially allocate space for in the body handle->character mapping.</param>
-        public CharacterControllers(BufferPool pool, int initialCharacterCapacity = 4096, int initialBodyHandleCapacity = 4096)
-        {
-            this.pool = pool;
-            characters = new QuickList<CharacterController>(initialCharacterCapacity, pool);
-            ResizeBodyHandleCapacity(initialBodyHandleCapacity);
-            analyzeContactsWorker = AnalyzeContactsWorker;
-            expandBoundingBoxesWorker = ExpandBoundingBoxesWorker;
-        }
+        Simulation = simulation;
+        simulation.Solver.Register<DynamicCharacterMotionConstraint>();
+        simulation.Solver.Register<StaticCharacterMotionConstraint>();
+        simulation.Timestepper.BeforeCollisionDetection += PrepareForContacts;
+        simulation.Timestepper.CollisionsDetected += AnalyzeContacts;
+    }
 
-        /// <summary>
-        /// Caches the simulation associated with the characters.
-        /// </summary>
-        /// <param name="simulation">Simulation to be associated with the characters.</param>
-        public void Initialize(Simulation simulation)
+    private void ResizeBodyHandleCapacity(int bodyHandleCapacity)
+    {
+        var oldCapacity = bodyHandleToCharacterIndex.Length;
+        pool.ResizeToAtLeast(ref bodyHandleToCharacterIndex, bodyHandleCapacity, bodyHandleToCharacterIndex.Length);
+        if (bodyHandleToCharacterIndex.Length > oldCapacity)
         {
-            Simulation = simulation;
-            simulation.Solver.Register<DynamicCharacterMotionConstraint>();
-            simulation.Solver.Register<StaticCharacterMotionConstraint>();
-            simulation.Timestepper.BeforeCollisionDetection += PrepareForContacts;
-            simulation.Timestepper.CollisionsDetected += AnalyzeContacts;
+            Unsafe.InitBlockUnaligned(ref Unsafe.As<int, byte>(ref bodyHandleToCharacterIndex[oldCapacity]), 0xFF, (uint)((bodyHandleToCharacterIndex.Length - oldCapacity) * sizeof(int)));
         }
+    }
 
-        private void ResizeBodyHandleCapacity(int bodyHandleCapacity)
-        {
-            var oldCapacity = bodyHandleToCharacterIndex.Length;
-            pool.ResizeToAtLeast(ref bodyHandleToCharacterIndex, bodyHandleCapacity, bodyHandleToCharacterIndex.Length);
-            if (bodyHandleToCharacterIndex.Length > oldCapacity)
-            {
-                Unsafe.InitBlockUnaligned(ref Unsafe.As<int, byte>(ref bodyHandleToCharacterIndex[oldCapacity]), 0xFF, (uint)((bodyHandleToCharacterIndex.Length - oldCapacity) * sizeof(int)));
-            }
-        }
+    /// <summary>
+    /// Gets the current memory slot index of a character using its associated body handle.
+    /// </summary>
+    /// <param name="bodyHandle">Body handle associated with the character to look up the index of.</param>
+    /// <returns>Index of the character associated with the body handle.</returns>
+    [MethodImpl(MethodImplOptions.AggressiveInlining)]
+    public int GetCharacterIndexForBodyHandle(int bodyHandle)
+    {
+        Debug.Assert(bodyHandle >= 0 && bodyHandle < bodyHandleToCharacterIndex.Length && bodyHandleToCharacterIndex[bodyHandle] >= 0, "Can only look up indices for body handles associated with characters in this CharacterControllers instance.");
+        return bodyHandleToCharacterIndex[bodyHandle];
+    }
 
-        /// <summary>
-        /// Gets the current memory slot index of a character using its associated body handle.
-        /// </summary>
-        /// <param name="bodyHandle">Body handle associated with the character to look up the index of.</param>
-        /// <returns>Index of the character associated with the body handle.</returns>
-        [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public int GetCharacterIndexForBodyHandle(int bodyHandle)
-        {
-            Debug.Assert(bodyHandle >= 0 && bodyHandle < bodyHandleToCharacterIndex.Length && bodyHandleToCharacterIndex[bodyHandle] >= 0, "Can only look up indices for body handles associated with characters in this CharacterControllers instance.");
-            return bodyHandleToCharacterIndex[bodyHandle];
-        }
+    /// <summary>
+    /// Gets a reference to the character at the given memory slot index.
+    /// </summary>
+    /// <param name="index">Index of the character to retrieve.</param>
+    /// <returns>Reference to the character at the given memory slot index.</returns>
+    [MethodImpl(MethodImplOptions.AggressiveInlining)]
+    public ref CharacterController GetCharacterByIndex(int index)
+    {
+        return ref characters[index];
+    }
 
-        /// <summary>
-        /// Gets a reference to the character at the given memory slot index.
-        /// </summary>
-        /// <param name="index">Index of the character to retrieve.</param>
-        /// <returns>Reference to the character at the given memory slot index.</returns>
-        [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public ref CharacterController GetCharacterByIndex(int index)
-        {
-            return ref characters[index];
-        }
+    /// <summary>
+    /// Gets a reference to the character using the handle of the character's body.
+    /// </summary>
+    /// <param name="bodyHandle">Body handle of the character to look up.</param>
+    /// <returns>Reference to the character associated with the given body handle.</returns>
+    [MethodImpl(MethodImplOptions.AggressiveInlining)]
+    public ref CharacterController GetCharacterByBodyHandle(BodyHandle bodyHandle)
+    {
+        Debug.Assert(bodyHandle.Value >= 0 && bodyHandle.Value < bodyHandleToCharacterIndex.Length && bodyHandleToCharacterIndex[bodyHandle.Value] >= 0, "Can only look up indices for body handles associated with characters in this CharacterControllers instance.");
+        return ref characters[bodyHandleToCharacterIndex[bodyHandle.Value]];
+    }
 
-        /// <summary>
-        /// Gets a reference to the character using the handle of the character's body.
-        /// </summary>
-        /// <param name="bodyHandle">Body handle of the character to look up.</param>
-        /// <returns>Reference to the character associated with the given body handle.</returns>
-        [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public ref CharacterController GetCharacterByBodyHandle(BodyHandle bodyHandle)
-        {
-            Debug.Assert(bodyHandle.Value >= 0 && bodyHandle.Value < bodyHandleToCharacterIndex.Length && bodyHandleToCharacterIndex[bodyHandle.Value] >= 0, "Can only look up indices for body handles associated with characters in this CharacterControllers instance.");
-            return ref characters[bodyHandleToCharacterIndex[bodyHandle.Value]];
-        }
+    /// <summary>
+    /// Allocates a character.
+    /// </summary>
+    /// <param name="bodyHandle">Body handle associated with the character.</param>
+    /// <returns>Reference to the allocated character.</returns>
+    public ref CharacterController AllocateCharacter(BodyHandle bodyHandle)
+    {
+        Debug.Assert(bodyHandle.Value >= 0 && (bodyHandle.Value >= bodyHandleToCharacterIndex.Length || bodyHandleToCharacterIndex[bodyHandle.Value] == -1),
+            "Cannot allocate more than one character for the same body handle.");
+        if (bodyHandle.Value >= bodyHandleToCharacterIndex.Length)
+            ResizeBodyHandleCapacity(Math.Max(bodyHandle.Value + 1, bodyHandleToCharacterIndex.Length * 2));
+        var characterIndex = characters.Count;
+        ref var character = ref characters.Allocate(pool);
+        character = default;
+        character.BodyHandle = bodyHandle;
+        bodyHandleToCharacterIndex[bodyHandle.Value] = characterIndex;
+        return ref character;
+    }
 
-        /// <summary>
-        /// Allocates a character.
-        /// </summary>
-        /// <param name="bodyHandle">Body handle associated with the character.</param>
-        /// <returns>Reference to the allocated character.</returns>
-        public ref CharacterController AllocateCharacter(BodyHandle bodyHandle)
+    /// <summary>
+    /// Removes a character from the character controllers set by the character's index.
+    /// </summary>
+    /// <param name="characterIndex">Index of the character to remove.</param>
+    public void RemoveCharacterByIndex(int characterIndex)
+    {
+        Debug.Assert(characterIndex >= 0 && characterIndex < characters.Count, "Character index must exist in the set of characters.");
+        ref var character = ref characters[characterIndex];
+        Debug.Assert(character.BodyHandle.Value >= 0 && character.BodyHandle.Value < bodyHandleToCharacterIndex.Length && bodyHandleToCharacterIndex[character.BodyHandle.Value] == characterIndex,
+            "Character must exist in the set of characters.");
+        bodyHandleToCharacterIndex[character.BodyHandle.Value] = -1;
+        characters.FastRemoveAt(characterIndex);
+        //If the removal moved a character, update the body handle mapping.
+        if (characters.Count > characterIndex)
         {
-            Debug.Assert(bodyHandle.Value >= 0 && (bodyHandle.Value >= bodyHandleToCharacterIndex.Length || bodyHandleToCharacterIndex[bodyHandle.Value] == -1),
-                "Cannot allocate more than one character for the same body handle.");
-            if (bodyHandle.Value >= bodyHandleToCharacterIndex.Length)
-                ResizeBodyHandleCapacity(Math.Max(bodyHandle.Value + 1, bodyHandleToCharacterIndex.Length * 2));
-            var characterIndex = characters.Count;
-            ref var character = ref characters.Allocate(pool);
-            character = default;
-            character.BodyHandle = bodyHandle;
-            bodyHandleToCharacterIndex[bodyHandle.Value] = characterIndex;
-            return ref character;
+            bodyHandleToCharacterIndex[characters[characterIndex].BodyHandle.Value] = characterIndex;
         }
+    }
+
+    /// <summary>
+    /// Removes a character from the character controllers set by the body handle associated with the character.
+    /// </summary>
+    /// <param name="bodyHandle">Body handle associated with the character to remove.</param>
+    public void RemoveCharacterByBodyHandle(BodyHandle bodyHandle)
+    {
+        Debug.Assert(bodyHandle.Value >= 0 && bodyHandle.Value < bodyHandleToCharacterIndex.Length && bodyHandleToCharacterIndex[bodyHandle.Value] >= 0,
+            "Removing a character by body handle requires that a character associated with the given body handle actually exists.");
+        RemoveCharacterByIndex(bodyHandleToCharacterIndex[bodyHandle.Value]);
+    }
 
-        /// <summary>
-        /// Removes a character from the character controllers set by the character's index.
-        /// </summary>
-        /// <param name="characterIndex">Index of the character to remove.</param>
-        public void RemoveCharacterByIndex(int characterIndex)
+    struct SupportCandidate
+    {
+        public Vector3 OffsetFromCharacter;
+        public float Depth;
+        public Vector3 OffsetFromSupport;
+        public Vector3 Normal;
+        public CollidableReference Support;
+    }
+
+    struct ContactCollectionWorkerCache
+    {
+        public Buffer<SupportCandidate> SupportCandidates;
+
+        public ContactCollectionWorkerCache(int maximumCharacterCount, BufferPool pool)
         {
-            Debug.Assert(characterIndex >= 0 && characterIndex < characters.Count, "Character index must exist in the set of characters.");
-            ref var character = ref characters[characterIndex];
-            Debug.Assert(character.BodyHandle.Value >= 0 && character.BodyHandle.Value < bodyHandleToCharacterIndex.Length && bodyHandleToCharacterIndex[character.BodyHandle.Value] == characterIndex,
-                "Character must exist in the set of characters.");
-            bodyHandleToCharacterIndex[character.BodyHandle.Value] = -1;
-            characters.FastRemoveAt(characterIndex);
-            //If the removal moved a character, update the body handle mapping.
-            if (characters.Count > characterIndex)
+            pool.Take(maximumCharacterCount, out SupportCandidates);
+            for (int i = 0; i < maximumCharacterCount; ++i)
             {
-                bodyHandleToCharacterIndex[characters[characterIndex].BodyHandle.Value] = characterIndex;
+                //Initialize the depths to a value that guarantees replacement.
+                SupportCandidates[i].Depth = float.MinValue;
             }
         }
 
-        /// <summary>
-        /// Removes a character from the character controllers set by the body handle associated with the character.
-        /// </summary>
-        /// <param name="bodyHandle">Body handle associated with the character to remove.</param>
-        public void RemoveCharacterByBodyHandle(BodyHandle bodyHandle)
-        {
-            Debug.Assert(bodyHandle.Value >= 0 && bodyHandle.Value < bodyHandleToCharacterIndex.Length && bodyHandleToCharacterIndex[bodyHandle.Value] >= 0,
-                "Removing a character by body handle requires that a character associated with the given body handle actually exists.");
-            RemoveCharacterByIndex(bodyHandleToCharacterIndex[bodyHandle.Value]);
-        }
-
-        struct SupportCandidate
+        public void Dispose(BufferPool pool)
         {
-            public Vector3 OffsetFromCharacter;
-            public float Depth;
-            public Vector3 OffsetFromSupport;
-            public Vector3 Normal;
-            public CollidableReference Support;
+            pool.Return(ref SupportCandidates);
         }
+    }
 
-        struct ContactCollectionWorkerCache
-        {
-            public Buffer<SupportCandidate> SupportCandidates;
 
-            public unsafe ContactCollectionWorkerCache(int maximumCharacterCount, BufferPool pool)
-            {
-                pool.Take(maximumCharacterCount, out SupportCandidates);
-                for (int i = 0; i < maximumCharacterCount; ++i)
-                {
-                    //Initialize the depths to a value that guarantees replacement.
-                    SupportCandidates[i].Depth = float.MinValue;
-                }
-            }
+    Buffer<ContactCollectionWorkerCache> contactCollectionWorkerCaches;
 
-            public void Dispose(BufferPool pool)
+    [MethodImpl(MethodImplOptions.AggressiveInlining)]
+    bool TryReportContacts<TManifold>(CollidableReference characterCollidable, CollidableReference supportCollidable, CollidablePair pair, ref TManifold manifold, int workerIndex) where TManifold : struct, IContactManifold<TManifold>
+    {
+        if (characterCollidable.Mobility == CollidableMobility.Dynamic && characterCollidable.BodyHandle.Value < bodyHandleToCharacterIndex.Length)
+        {
+            var characterBodyHandle = characterCollidable.BodyHandle;
+            var characterIndex = bodyHandleToCharacterIndex[characterBodyHandle.Value];
+            if (characterIndex >= 0)
             {
-                pool.Return(ref SupportCandidates);
-            }
-        }
+                //This is actually a character.
+                ref var character = ref characters[characterIndex];
+                //Our job here is to process the manifold into a support representation. That means a single point, normal, and importance heuristic.
+                //Note that we cannot safely pick from the candidates in this function- it is likely executed from a multithreaded context, so all we can do is
+                //output the pair's result into a worker-exclusive buffer.
 
+                //Contacts with sufficiently negative depth will not be considered support candidates.
+                //Contacts with intermediate depth (above minimum threshold, but still below negative epsilon) may be candidates if the character previously had support.
+                //Contacts with depth above negative epsilon always pass the depth test.
 
-        Buffer<ContactCollectionWorkerCache> contactCollectionWorkerCaches;
+                //Maximum depth is used to heuristically choose which contact represents the support.
+                //Note that this could be changed to subtly modify the behavior- for example, dotting the movement direction with the support normal and such.
+                //A more careful choice of heuristic could make the character more responsive when trying to 'step' up obstacles.
 
-        [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        bool TryReportContacts<TManifold>(CollidableReference characterCollidable, CollidableReference supportCollidable, CollidablePair pair, ref TManifold manifold, int workerIndex) where TManifold : struct, IContactManifold<TManifold>
-        {
-            if (characterCollidable.Mobility == CollidableMobility.Dynamic && characterCollidable.BodyHandle.Value < bodyHandleToCharacterIndex.Length)
-            {
-                var characterBodyHandle = characterCollidable.BodyHandle;
-                var characterIndex = bodyHandleToCharacterIndex[characterBodyHandle.Value];
-                if (characterIndex >= 0)
+                //Note that the body may be inactive during this callback even though it will be activated by new constraints after the narrow phase flushes.
+                //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.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)
                 {
-                    //This is actually a character.
-                    ref var character = ref characters[characterIndex];
-                    //Our job here is to process the manifold into a support representation. That means a single point, normal, and importance heuristic.
-                    //Note that we cannot safely pick from the candidates in this function- it is likely executed from a multithreaded context, so all we can do is
-                    //output the pair's result into a worker-exclusive buffer.
-
-                    //Contacts with sufficiently negative depth will not be considered support candidates.
-                    //Contacts with intermediate depth (above minimum threshold, but still below negative epsilon) may be candidates if the character previously had support.
-                    //Contacts with depth above negative epsilon always pass the depth test.
-
-                    //Maximum depth is used to heuristically choose which contact represents the support.
-                    //Note that this could be changed to subtly modify the behavior- for example, dotting the movement direction with the support normal and such.
-                    //A more careful choice of heuristic could make the character more responsive when trying to 'step' up obstacles.
-
-                    //Note that the body may be inactive during this callback even though it will be activated by new constraints after the narrow phase flushes.
-                    //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];
-                    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)
-                    {
-                        ref var convexManifold = ref Unsafe.As<TManifold, ConvexContactManifold>(ref manifold);
-                        var normalUpDot = Vector3.Dot(convexManifold.Normal, up);
-                        //The narrow phase generates contacts with normals pointing from B to A by convention.
-                        //If the character is collidable B, then we need to negate the comparison.
-                        if ((pair.B.Packed == characterCollidable.Packed ? -normalUpDot : normalUpDot) > character.CosMaximumSlope)
-                        {
-                            //This manifold has a slope that is potentially supportive.
-                            //Can the maximum depth contact be used as a support?
-                            var maximumDepth = convexManifold.Contact0.Depth;
-                            var maximumDepthIndex = 0;
-                            for (int i = 1; i < convexManifold.Count; ++i)
-                            {
-                                ref var candidateDepth = ref Unsafe.Add(ref convexManifold.Contact0, i).Depth;
-                                if (candidateDepth > maximumDepth)
-                                {
-                                    maximumDepth = candidateDepth;
-                                    maximumDepthIndex = i;
-                                }
-                            }
-                            if (maximumDepth >= character.MinimumSupportDepth || (character.Supported && maximumDepth > character.MinimumSupportContinuationDepth))
-                            {
-                                ref var supportCandidate = ref contactCollectionWorkerCaches[workerIndex].SupportCandidates[characterIndex];
-                                if (supportCandidate.Depth < maximumDepth)
-                                {
-                                    //This support candidate should be replaced.
-                                    supportCandidate.Depth = maximumDepth;
-                                    ref var deepestContact = ref Unsafe.Add(ref convexManifold.Contact0, maximumDepthIndex);
-                                    var offsetFromB = deepestContact.Offset - convexManifold.OffsetB;
-                                    if (pair.B.Packed == characterCollidable.Packed)
-                                    {
-                                        supportCandidate.Normal = -convexManifold.Normal;
-                                        supportCandidate.OffsetFromCharacter = offsetFromB;
-                                        supportCandidate.OffsetFromSupport = deepestContact.Offset;
-                                    }
-                                    else
-                                    {
-                                        supportCandidate.Normal = convexManifold.Normal;
-                                        supportCandidate.OffsetFromCharacter = deepestContact.Offset;
-                                        supportCandidate.OffsetFromSupport = offsetFromB;
-                                    }
-                                    supportCandidate.Support = supportCollidable;
-                                }
-                            }
-                        }
-                    }
-                    else
+                    ref var convexManifold = ref Unsafe.As<TManifold, ConvexContactManifold>(ref manifold);
+                    var normalUpDot = Vector3.Dot(convexManifold.Normal, up);
+                    //The narrow phase generates contacts with normals pointing from B to A by convention.
+                    //If the character is collidable B, then we need to negate the comparison.
+                    if ((pair.B.Packed == characterCollidable.Packed ? -normalUpDot : normalUpDot) > character.CosMaximumSlope)
                     {
-                        ref var nonconvexManifold = ref Unsafe.As<TManifold, NonconvexContactManifold>(ref manifold);
-                        //The narrow phase generates contacts with normals pointing from B to A by convention.
-                        //If the character is collidable B, then we need to negate the comparison.
                         //This manifold has a slope that is potentially supportive.
                         //Can the maximum depth contact be used as a support?
-                        var maximumDepth = float.MinValue;
-                        var maximumDepthIndex = -1;
-                        for (int i = 0; i < nonconvexManifold.Count; ++i)
+                        var maximumDepth = convexManifold.Contact0.Depth;
+                        var maximumDepthIndex = 0;
+                        for (int i = 1; i < convexManifold.Count; ++i)
                         {
-                            ref var candidate = ref Unsafe.Add(ref nonconvexManifold.Contact0, i);
-                            if (candidate.Depth > maximumDepth)
+                            ref var candidateDepth = ref Unsafe.Add(ref convexManifold.Contact0, i).Depth;
+                            if (candidateDepth > maximumDepth)
                             {
-                                //All the nonconvex candidates can have different normals, so we have to perform the (calibrated) normal test on every single one.
-                                var upDot = Vector3.Dot(candidate.Normal, up);
-                                if ((pair.B.Packed == characterCollidable.Packed ? -upDot : upDot) > character.CosMaximumSlope)
-                                {
-                                    maximumDepth = candidate.Depth;
-                                    maximumDepthIndex = i;
-                                }
+                                maximumDepth = candidateDepth;
+                                maximumDepthIndex = i;
                             }
                         }
                         if (maximumDepth >= character.MinimumSupportDepth || (character.Supported && maximumDepth > character.MinimumSupportContinuationDepth))
@@ -358,18 +309,18 @@ bool TryReportContacts<TManifold>(CollidableReference characterCollidable, Colli
                             if (supportCandidate.Depth < maximumDepth)
                             {
                                 //This support candidate should be replaced.
-                                ref var deepestContact = ref Unsafe.Add(ref nonconvexManifold.Contact0, maximumDepthIndex);
                                 supportCandidate.Depth = maximumDepth;
-                                var offsetFromB = deepestContact.Offset - nonconvexManifold.OffsetB;
+                                ref var deepestContact = ref Unsafe.Add(ref convexManifold.Contact0, maximumDepthIndex);
+                                var offsetFromB = deepestContact.Offset - convexManifold.OffsetB;
                                 if (pair.B.Packed == characterCollidable.Packed)
                                 {
-                                    supportCandidate.Normal = -deepestContact.Normal;
+                                    supportCandidate.Normal = -convexManifold.Normal;
                                     supportCandidate.OffsetFromCharacter = offsetFromB;
                                     supportCandidate.OffsetFromSupport = deepestContact.Offset;
                                 }
                                 else
                                 {
-                                    supportCandidate.Normal = deepestContact.Normal;
+                                    supportCandidate.Normal = convexManifold.Normal;
                                     supportCandidate.OffsetFromCharacter = deepestContact.Offset;
                                     supportCandidate.OffsetFromSupport = offsetFromB;
                                 }
@@ -377,525 +328,576 @@ bool TryReportContacts<TManifold>(CollidableReference characterCollidable, Colli
                             }
                         }
                     }
-                    return true;
                 }
+                else
+                {
+                    ref var nonconvexManifold = ref Unsafe.As<TManifold, NonconvexContactManifold>(ref manifold);
+                    //The narrow phase generates contacts with normals pointing from B to A by convention.
+                    //If the character is collidable B, then we need to negate the comparison.
+                    //This manifold has a slope that is potentially supportive.
+                    //Can the maximum depth contact be used as a support?
+                    var maximumDepth = float.MinValue;
+                    var maximumDepthIndex = -1;
+                    for (int i = 0; i < nonconvexManifold.Count; ++i)
+                    {
+                        ref var candidate = ref Unsafe.Add(ref nonconvexManifold.Contact0, i);
+                        if (candidate.Depth > maximumDepth)
+                        {
+                            //All the nonconvex candidates can have different normals, so we have to perform the (calibrated) normal test on every single one.
+                            var upDot = Vector3.Dot(candidate.Normal, up);
+                            if ((pair.B.Packed == characterCollidable.Packed ? -upDot : upDot) > character.CosMaximumSlope)
+                            {
+                                maximumDepth = candidate.Depth;
+                                maximumDepthIndex = i;
+                            }
+                        }
+                    }
+                    if (maximumDepth >= character.MinimumSupportDepth || (character.Supported && maximumDepth > character.MinimumSupportContinuationDepth))
+                    {
+                        ref var supportCandidate = ref contactCollectionWorkerCaches[workerIndex].SupportCandidates[characterIndex];
+                        if (supportCandidate.Depth < maximumDepth)
+                        {
+                            //This support candidate should be replaced.
+                            ref var deepestContact = ref Unsafe.Add(ref nonconvexManifold.Contact0, maximumDepthIndex);
+                            supportCandidate.Depth = maximumDepth;
+                            var offsetFromB = deepestContact.Offset - nonconvexManifold.OffsetB;
+                            if (pair.B.Packed == characterCollidable.Packed)
+                            {
+                                supportCandidate.Normal = -deepestContact.Normal;
+                                supportCandidate.OffsetFromCharacter = offsetFromB;
+                                supportCandidate.OffsetFromSupport = deepestContact.Offset;
+                            }
+                            else
+                            {
+                                supportCandidate.Normal = deepestContact.Normal;
+                                supportCandidate.OffsetFromCharacter = deepestContact.Offset;
+                                supportCandidate.OffsetFromSupport = offsetFromB;
+                            }
+                            supportCandidate.Support = supportCollidable;
+                        }
+                    }
+                }
+                return true;
             }
-            return false;
         }
+        return false;
+    }
 
 
-        /// <summary>
-        /// Reports contacts about a collision to the character system. If the pair does not involve a character or there are no contacts, does nothing and returns false.
-        /// </summary>
-        /// <param name="pair">Pair of objects associated with the contact manifold.</param>
-        /// <param name="manifold">Contact manifold between the colliding objects.</param>
-        /// <param name="workerIndex">Index of the currently executing worker thread.</param>
-        /// <param name="materialProperties">Material properties for this pair. Will be modified if the pair involves a character.</param>
-        /// <returns>True if the pair involved a character pair and has contacts, false otherwise.</returns>
-        [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public bool TryReportContacts<TManifold>(in CollidablePair pair, ref TManifold manifold, int workerIndex, ref PairMaterialProperties materialProperties) where TManifold : struct, IContactManifold<TManifold>
-        {
-            Debug.Assert(contactCollectionWorkerCaches.Allocated && workerIndex < contactCollectionWorkerCaches.Length && contactCollectionWorkerCaches[workerIndex].SupportCandidates.Allocated,
-                "Worker caches weren't properly allocated; did you forget to call PrepareForContacts before collision detection?");
-            if (manifold.Count == 0)
-                return false;
-            //It's possible for neither, one, or both collidables to be a character. Check each one, treating the other as a potential support.
-            var aIsCharacter = TryReportContacts(pair.A, pair.B, pair, ref manifold, workerIndex);
-            var bIsCharacter = TryReportContacts(pair.B, pair.A, pair, ref manifold, workerIndex);
-            if (aIsCharacter || bIsCharacter)
-            {
-                //The character's motion over the surface should be controlled entirely by the horizontal motion constraint.
-                //Note- you could use the friction coefficient to change the horizontal motion constraint's maximum force to simulate different environments if you want.
-                //That would just require caching a bit more information for the AnalyzeContacts function to use.
-                materialProperties.FrictionCoefficient = 0;
-                return true;
-            }
+    /// <summary>
+    /// Reports contacts about a collision to the character system. If the pair does not involve a character or there are no contacts, does nothing and returns false.
+    /// </summary>
+    /// <param name="pair">Pair of objects associated with the contact manifold.</param>
+    /// <param name="manifold">Contact manifold between the colliding objects.</param>
+    /// <param name="workerIndex">Index of the currently executing worker thread.</param>
+    /// <param name="materialProperties">Material properties for this pair. Will be modified if the pair involves a character.</param>
+    /// <returns>True if the pair involved a character pair and has contacts, false otherwise.</returns>
+    [MethodImpl(MethodImplOptions.AggressiveInlining)]
+    public bool TryReportContacts<TManifold>(in CollidablePair pair, ref TManifold manifold, int workerIndex, ref PairMaterialProperties materialProperties) where TManifold : struct, IContactManifold<TManifold>
+    {
+        Debug.Assert(contactCollectionWorkerCaches.Allocated && workerIndex < contactCollectionWorkerCaches.Length && contactCollectionWorkerCaches[workerIndex].SupportCandidates.Allocated,
+            "Worker caches weren't properly allocated; did you forget to call PrepareForContacts before collision detection?");
+        if (manifold.Count == 0)
             return false;
-        }
-
-        Buffer<(int Start, int Count)> boundingBoxExpansionJobs;
-        unsafe void ExpandBoundingBoxes(int start, int count)
+        //It's possible for neither, one, or both collidables to be a character. Check each one, treating the other as a potential support.
+        var aIsCharacter = TryReportContacts(pair.A, pair.B, pair, ref manifold, workerIndex);
+        var bIsCharacter = TryReportContacts(pair.B, pair.A, pair, ref manifold, workerIndex);
+        if (aIsCharacter || bIsCharacter)
         {
-            var end = start + count;
-            for (int i = start; i < end; ++i)
-            {
-                ref var character = ref characters[i];
-                var characterBody = Simulation.Bodies.GetBodyReference(character.BodyHandle);
-                if (characterBody.Awake)
-                {
-                    Simulation.BroadPhase.GetActiveBoundsPointers(characterBody.Collidable.BroadPhaseIndex, out var min, out var max);
-                    QuaternionEx.Transform(character.LocalUp, characterBody.Pose.Orientation, out var characterUp);
-                    var supportExpansion = character.MinimumSupportContinuationDepth * characterUp;
-                    *min += Vector3.Min(Vector3.Zero, supportExpansion);
-                    *max += Vector3.Max(Vector3.Zero, supportExpansion);
-                }
-            }
+            //The character's motion over the surface should be controlled entirely by the horizontal motion constraint.
+            //Note- you could use the friction coefficient to change the horizontal motion constraint's maximum force to simulate different environments if you want.
+            //That would just require caching a bit more information for the AnalyzeContacts function to use.
+            materialProperties.FrictionCoefficient = 0;
+            return true;
         }
+        return false;
+    }
+
+    Buffer<(int Start, int Count)> boundingBoxExpansionJobs;
 
-        int boundingBoxExpansionJobIndex;
-        Action<int> expandBoundingBoxesWorker;
-        void ExpandBoundingBoxesWorker(int workerIndex)
+    void ExpandBoundingBoxes(int start, int count)
+    {
+        var end = start + count;
+        for (int i = start; i < end; ++i)
         {
-            while (true)
+            ref var character = ref characters[i];
+            var characterBody = Simulation.Bodies[character.BodyHandle];
+            if (characterBody.Awake)
             {
-                var jobIndex = Interlocked.Increment(ref boundingBoxExpansionJobIndex);
-                if (jobIndex < boundingBoxExpansionJobs.Length)
-                {
-                    ref var job = ref boundingBoxExpansionJobs[jobIndex];
-                    ExpandBoundingBoxes(job.Start, job.Count);
-                }
-                else
-                {
-                    break;
-                }
+                Simulation.BroadPhase.GetActiveBoundsPointers(characterBody.Collidable.BroadPhaseIndex, out var min, out var max);
+                QuaternionEx.Transform(character.LocalUp, characterBody.Pose.Orientation, out var characterUp);
+                var supportExpansion = character.MinimumSupportContinuationDepth * characterUp;
+                *min += Vector3.Min(Vector3.Zero, supportExpansion);
+                *max += Vector3.Max(Vector3.Zero, supportExpansion);
             }
         }
+    }
+
+    int boundingBoxExpansionJobIndex;
+    Action<int> expandBoundingBoxesWorker;
 
-        /// <summary>
-        /// Preallocates space for support data collected during the narrow phase. Should be called before the narrow phase executes.
-        /// </summary>
-        void PrepareForContacts(float dt, IThreadDispatcher threadDispatcher = null)
+    void ExpandBoundingBoxesWorker(int workerIndex)
+    {
+        while (true)
         {
-            Debug.Assert(!contactCollectionWorkerCaches.Allocated, "Worker caches were already allocated; did you forget to call AnalyzeContacts after collision detection to flush the previous frame's results?");
-            var threadCount = threadDispatcher == null ? 1 : threadDispatcher.ThreadCount;
-            pool.Take(threadCount, out contactCollectionWorkerCaches);
-            for (int i = 0; i < contactCollectionWorkerCaches.Length; ++i)
-            {
-                contactCollectionWorkerCaches[i] = new ContactCollectionWorkerCache(characters.Count, pool);
-            }
-            //While the character will retain support with contacts with depths above the MinimumSupportContinuationDepth if there was support in the previous frame,
-            //it's possible for the contacts to be lost because the bounding box isn't expanded by MinimumSupportContinuationDepth and the broad phase doesn't see the support collidable.
-            //Here, we expand the bounding boxes to compensate.
-            if (threadCount == 1 || characters.Count < 256)
+            var jobIndex = Interlocked.Increment(ref boundingBoxExpansionJobIndex);
+            if (jobIndex < boundingBoxExpansionJobs.Length)
             {
-                ExpandBoundingBoxes(0, characters.Count);
+                ref var job = ref boundingBoxExpansionJobs[jobIndex];
+                ExpandBoundingBoxes(job.Start, job.Count);
             }
             else
             {
-                var jobCount = Math.Min(characters.Count, threadCount);
-                var charactersPerJob = characters.Count / jobCount;
-                var baseCharacterCount = charactersPerJob * jobCount;
-                var remainder = characters.Count - baseCharacterCount;
-                pool.Take(jobCount, out boundingBoxExpansionJobs);
-                var previousEnd = 0;
-                for (int jobIndex = 0; jobIndex < jobCount; ++jobIndex)
-                {
-                    var charactersForJob = jobIndex < remainder ? charactersPerJob + 1 : charactersPerJob;
-                    ref var job = ref boundingBoxExpansionJobs[jobIndex];
-                    job.Start = previousEnd;
-                    job.Count = charactersForJob;
-                    previousEnd += job.Count;
-                }
-
-                boundingBoxExpansionJobIndex = -1;
-                threadDispatcher.DispatchWorkers(expandBoundingBoxesWorker);
-                pool.Return(ref boundingBoxExpansionJobs);
-
+                break;
             }
         }
+    }
 
-        struct PendingDynamicConstraint
+    /// <summary>
+    /// Preallocates space for support data collected during the narrow phase. Should be called before the narrow phase executes.
+    /// </summary>
+    void PrepareForContacts(float dt, IThreadDispatcher threadDispatcher = null)
+    {
+        Debug.Assert(!contactCollectionWorkerCaches.Allocated, "Worker caches were already allocated; did you forget to call AnalyzeContacts after collision detection to flush the previous frame's results?");
+        var threadCount = threadDispatcher == null ? 1 : threadDispatcher.ThreadCount;
+        pool.Take(threadCount, out contactCollectionWorkerCaches);
+        for (int i = 0; i < contactCollectionWorkerCaches.Length; ++i)
         {
-            public int CharacterIndex;
-            public DynamicCharacterMotionConstraint Description;
+            contactCollectionWorkerCaches[i] = new ContactCollectionWorkerCache(characters.Count, pool);
         }
-        struct PendingStaticConstraint
+        //While the character will retain support with contacts with depths above the MinimumSupportContinuationDepth if there was support in the previous frame,
+        //it's possible for the contacts to be lost because the bounding box isn't expanded by MinimumSupportContinuationDepth and the broad phase doesn't see the support collidable.
+        //Here, we expand the bounding boxes to compensate.
+        if (threadCount == 1 || characters.Count < 256)
         {
-            public int CharacterIndex;
-            public StaticCharacterMotionConstraint Description;
+            ExpandBoundingBoxes(0, characters.Count);
         }
-        struct Jump
+        else
         {
-            //Note that not every jump will contain a support body, so this can waste memory.
-            //That's not really a concern- jumps are very rare (relatively speaking), so all we're wasting is capacity, not bandwidth.
-            public int CharacterBodyIndex;
-            public Vector3 CharacterVelocityChange;
-            public int SupportBodyIndex;
-            public Vector3 SupportImpulseOffset;
+            var jobCount = Math.Min(characters.Count, threadCount);
+            var charactersPerJob = characters.Count / jobCount;
+            var baseCharacterCount = charactersPerJob * jobCount;
+            var remainder = characters.Count - baseCharacterCount;
+            pool.Take(jobCount, out boundingBoxExpansionJobs);
+            var previousEnd = 0;
+            for (int jobIndex = 0; jobIndex < jobCount; ++jobIndex)
+            {
+                var charactersForJob = jobIndex < remainder ? charactersPerJob + 1 : charactersPerJob;
+                ref var job = ref boundingBoxExpansionJobs[jobIndex];
+                job.Start = previousEnd;
+                job.Count = charactersForJob;
+                previousEnd += job.Count;
+            }
+
+            boundingBoxExpansionJobIndex = -1;
+            threadDispatcher.DispatchWorkers(expandBoundingBoxesWorker, boundingBoxExpansionJobs.Length);
+            pool.Return(ref boundingBoxExpansionJobs);
+
         }
+    }
 
-        struct AnalyzeContactsWorkerCache
-        {
-            //The solver does not permit multithreaded removals and additions. We handle all of them in a sequential postpass.
-            public QuickList<ConstraintHandle> ConstraintHandlesToRemove;
-            public QuickList<PendingDynamicConstraint> DynamicConstraintsToAdd;
-            public QuickList<PendingStaticConstraint> StaticConstraintsToAdd;
-            public QuickList<Jump> Jumps;
+    struct PendingDynamicConstraint
+    {
+        public int CharacterIndex;
+        public DynamicCharacterMotionConstraint Description;
+    }
+    struct PendingStaticConstraint
+    {
+        public int CharacterIndex;
+        public StaticCharacterMotionConstraint Description;
+    }
+    struct Jump
+    {
+        //Note that not every jump will contain a support body, so this can waste memory.
+        //That's not really a concern- jumps are very rare (relatively speaking), so all we're wasting is capacity, not bandwidth.
+        public int CharacterBodyIndex;
+        public Vector3 CharacterVelocityChange;
+        public int SupportBodyIndex;
+        public Vector3 SupportImpulseOffset;
+    }
 
-            public AnalyzeContactsWorkerCache(int maximumCharacterCount, BufferPool pool)
-            {
-                ConstraintHandlesToRemove = new QuickList<ConstraintHandle>(maximumCharacterCount, pool);
-                DynamicConstraintsToAdd = new QuickList<PendingDynamicConstraint>(maximumCharacterCount, pool);
-                StaticConstraintsToAdd = new QuickList<PendingStaticConstraint>(maximumCharacterCount, pool);
-                Jumps = new QuickList<Jump>(maximumCharacterCount, pool);
-            }
+    struct AnalyzeContactsWorkerCache
+    {
+        //The solver does not permit multithreaded removals and additions. We handle all of them in a sequential postpass.
+        public QuickList<ConstraintHandle> ConstraintHandlesToRemove;
+        public QuickList<PendingDynamicConstraint> DynamicConstraintsToAdd;
+        public QuickList<PendingStaticConstraint> StaticConstraintsToAdd;
+        public QuickList<Jump> Jumps;
 
-            public void Dispose(BufferPool pool)
-            {
-                ConstraintHandlesToRemove.Dispose(pool);
-                DynamicConstraintsToAdd.Dispose(pool);
-                StaticConstraintsToAdd.Dispose(pool);
-                Jumps.Dispose(pool);
-            }
+        public AnalyzeContactsWorkerCache(int maximumCharacterCount, BufferPool pool)
+        {
+            ConstraintHandlesToRemove = new QuickList<ConstraintHandle>(maximumCharacterCount, pool);
+            DynamicConstraintsToAdd = new QuickList<PendingDynamicConstraint>(maximumCharacterCount, pool);
+            StaticConstraintsToAdd = new QuickList<PendingStaticConstraint>(maximumCharacterCount, pool);
+            Jumps = new QuickList<Jump>(maximumCharacterCount, pool);
+        }
+
+        public void Dispose(BufferPool pool)
+        {
+            ConstraintHandlesToRemove.Dispose(pool);
+            DynamicConstraintsToAdd.Dispose(pool);
+            StaticConstraintsToAdd.Dispose(pool);
+            Jumps.Dispose(pool);
         }
+    }
 
-        Buffer<AnalyzeContactsWorkerCache> analyzeContactsWorkerCaches;
+    Buffer<AnalyzeContactsWorkerCache> analyzeContactsWorkerCaches;
 
-        void AnalyzeContactsForCharacterRegion(int start, int exclusiveEnd, int workerIndex)
+    void AnalyzeContactsForCharacterRegion(int start, int exclusiveEnd, int workerIndex)
+    {
+        ref var analyzeContactsWorkerCache = ref analyzeContactsWorkerCaches[workerIndex];
+        for (int characterIndex = start; characterIndex < exclusiveEnd; ++characterIndex)
         {
-            ref var analyzeContactsWorkerCache = ref analyzeContactsWorkerCaches[workerIndex];
-            for (int characterIndex = start; characterIndex < exclusiveEnd; ++characterIndex)
+            //Note that this iterates over both active and inactive characters rather than segmenting inactive characters into their own collection.
+            //This demands branching, but the expectation is that the vast majority of characters will be active, so there is less value in copying them into stasis.                
+            ref var character = ref characters[characterIndex];
+            ref var bodyLocation = ref Simulation.Bodies.HandleToLocation[character.BodyHandle.Value];
+            if (bodyLocation.SetIndex == 0)
             {
-                //Note that this iterates over both active and inactive characters rather than segmenting inactive characters into their own collection.
-                //This demands branching, but the expectation is that the vast majority of characters will be active, so there is less value in copying them into stasis.                
-                ref var character = ref characters[characterIndex];
-                ref var bodyLocation = ref Simulation.Bodies.HandleToLocation[character.BodyHandle.Value];
-                if (bodyLocation.SetIndex == 0)
+                var supportCandidate = contactCollectionWorkerCaches[0].SupportCandidates[characterIndex];
+                for (int j = 1; j < contactCollectionWorkerCaches.Length; ++j)
                 {
-                    var supportCandidate = contactCollectionWorkerCaches[0].SupportCandidates[characterIndex];
-                    for (int j = 1; j < contactCollectionWorkerCaches.Length; ++j)
+                    ref var workerCandidate = ref contactCollectionWorkerCaches[j].SupportCandidates[characterIndex];
+                    if (workerCandidate.Depth > supportCandidate.Depth)
                     {
-                        ref var workerCandidate = ref contactCollectionWorkerCaches[j].SupportCandidates[characterIndex];
-                        if (workerCandidate.Depth > supportCandidate.Depth)
-                        {
-                            supportCandidate = workerCandidate;
-                        }
+                        supportCandidate = workerCandidate;
                     }
-                    //We need to protect against one possible corner case: if the body supporting the character was removed, the associated motion constraint was also removed.
-                    //Arbitrarily un-support the character if we detect this.      
-                    if (character.Supported)
+                }
+                //We need to protect against one possible corner case: if the body supporting the character was removed, the associated motion constraint was also removed.
+                //Arbitrarily un-support the character if we detect this.      
+                if (character.Supported)
+                {
+                    //If the constraint no longer exists at all, 
+                    if (!Simulation.Solver.ConstraintExists(character.MotionConstraintHandle) ||
+                        //or if the constraint does exist but is now used by a different constraint type,
+                        (Simulation.Solver.HandleToConstraint[character.MotionConstraintHandle.Value].TypeId != DynamicCharacterMotionTypeProcessor.BatchTypeId &&
+                        Simulation.Solver.HandleToConstraint[character.MotionConstraintHandle.Value].TypeId != StaticCharacterMotionTypeProcessor.BatchTypeId))
                     {
-                        //If the constraint no longer exists at all, 
-                        if (!Simulation.Solver.ConstraintExists(character.MotionConstraintHandle) ||
-                            //or if the constraint does exist but is now used by a different constraint type,
-                            (Simulation.Solver.HandleToConstraint[character.MotionConstraintHandle.Value].TypeId != DynamicCharacterMotionTypeProcessor.BatchTypeId &&
-                            Simulation.Solver.HandleToConstraint[character.MotionConstraintHandle.Value].TypeId != StaticCharacterMotionTypeProcessor.BatchTypeId))
-                        {
-                            //then the character isn't actually supported anymore.
-                            character.Supported = false;
-                        }
-                        //Note that it's sufficient to only check that the type matches the dynamic motion constraint type id because no other systems ever create dynamic character motion constraints.
-                        //Other systems may result in the constraint's removal, but no other system will ever *create* it.
-                        //Further, during this analysis loop, we do not create any constraints. We only set up pending additions to be processed after the multithreaded analysis completes.
+                        //then the character isn't actually supported anymore.
+                        character.Supported = false;
                     }
+                    //Note that it's sufficient to only check that the type matches the dynamic motion constraint type id because no other systems ever create dynamic character motion constraints.
+                    //Other systems may result in the constraint's removal, but no other system will ever *create* it.
+                    //Further, during this analysis loop, we do not create any constraints. We only set up pending additions to be processed after the multithreaded analysis completes.
+                }
 
-                    //The body is active. We may need to remove the associated constraint from the solver. Remove if any of the following hold:
-                    //1) The character was previously supported but is no longer.
-                    //2) The character was previously supported by a body, and is now supported by a different body.
-                    //3) The character was previously supported by a static, and is now supported by a body.
-                    //4) The character was previously supported by a body, and is now supported by a static.
-                    var shouldRemove = character.Supported && (character.TryJump || supportCandidate.Depth == float.MinValue || character.Support.Packed != supportCandidate.Support.Packed);
-                    if (shouldRemove)
-                    {
-                        //Mark the constraint for removal.
-                        analyzeContactsWorkerCache.ConstraintHandlesToRemove.AllocateUnsafely() = character.MotionConstraintHandle;
-                    }
+                //The body is active. We may need to remove the associated constraint from the solver. Remove if any of the following hold:
+                //1) The character was previously supported but is no longer.
+                //2) The character was previously supported by a body, and is now supported by a different body.
+                //3) The character was previously supported by a static, and is now supported by a body.
+                //4) The character was previously supported by a body, and is now supported by a static.
+                var shouldRemove = character.Supported && (character.TryJump || supportCandidate.Depth == float.MinValue || character.Support.Packed != supportCandidate.Support.Packed);
+                if (shouldRemove)
+                {
+                    //Mark the constraint for removal.
+                    analyzeContactsWorkerCache.ConstraintHandlesToRemove.AllocateUnsafely() = character.MotionConstraintHandle;
+                }
 
-                    //If the character is jumping, don't create a constraint.
-                    if (supportCandidate.Depth > float.MinValue && character.TryJump)
+                //If the character is jumping, don't create a constraint.
+                if (supportCandidate.Depth > float.MinValue && character.TryJump)
+                {
+                    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.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)
                     {
-                        QuaternionEx.Transform(character.LocalUp, Simulation.Bodies.ActiveSet.Poses[bodyLocation.Index].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);
-                        //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.DynamicsState[supportingBodyLocation.Index].Motion.Velocity;
+                        var wxr = Vector3.Cross(supportVelocity.Angular, supportCandidate.OffsetFromSupport);
+                        var supportContactVelocity = supportVelocity.Linear + wxr;
+                        var supportUpVelocity = Vector3.Dot(supportContactVelocity, characterUp);
+
+                        //If the support is dynamic, apply an opposing impulse. Note that velocity changes cannot safely be applied during multithreaded execution;
+                        //characters could share support bodies, and a character might be a support of another character.
+                        //That's really not concerning from a performance perspective- characters don't jump many times per frame.
+                        ref var jump = ref analyzeContactsWorkerCache.Jumps.AllocateUnsafely();
+                        jump.CharacterBodyIndex = bodyLocation.Index;
+                        jump.CharacterVelocityChange = characterUp * MathF.Max(0, character.JumpVelocity - (characterUpVelocity - supportUpVelocity));
+                        if (character.Support.Mobility == CollidableMobility.Dynamic)
                         {
-                            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];
-                            var wxr = Vector3.Cross(supportVelocity.Angular, supportCandidate.OffsetFromSupport);
-                            var supportContactVelocity = supportVelocity.Linear + wxr;
-                            var supportUpVelocity = Vector3.Dot(supportContactVelocity, characterUp);
-
-                            //If the support is dynamic, apply an opposing impulse. Note that velocity changes cannot safely be applied during multithreaded execution;
-                            //characters could share support bodies, and a character might be a support of another character.
-                            //That's really not concerning from a performance perspective- characters don't jump many times per frame.
-                            ref var jump = ref analyzeContactsWorkerCache.Jumps.AllocateUnsafely();
-                            jump.CharacterBodyIndex = bodyLocation.Index;
-                            jump.CharacterVelocityChange = characterUp * MathF.Max(0, character.JumpVelocity - (characterUpVelocity - supportUpVelocity));
-                            if (character.Support.Mobility == CollidableMobility.Dynamic)
-                            {
-                                jump.SupportBodyIndex = supportingBodyLocation.Index;
-                                jump.SupportImpulseOffset = supportCandidate.OffsetFromSupport;
-                            }
-                            else
-                            {
-                                //No point in applying impulses to kinematics.
-                                jump.SupportBodyIndex = -1;
-                            }
+                            jump.SupportBodyIndex = supportingBodyLocation.Index;
+                            jump.SupportImpulseOffset = supportCandidate.OffsetFromSupport;
                         }
                         else
                         {
-                            //Static bodies have no velocity, so we don't have to consider the support.
-                            ref var jump = ref analyzeContactsWorkerCache.Jumps.AllocateUnsafely();
-                            jump.CharacterBodyIndex = bodyLocation.Index;
-                            jump.CharacterVelocityChange = characterUp * MathF.Max(0, character.JumpVelocity - characterUpVelocity);
+                            //No point in applying impulses to kinematics.
                             jump.SupportBodyIndex = -1;
                         }
-                        character.Supported = false;
                     }
-                    else if (supportCandidate.Depth > float.MinValue)
+                    else
+                    {
+                        //Static bodies have no velocity, so we don't have to consider the support.
+                        ref var jump = ref analyzeContactsWorkerCache.Jumps.AllocateUnsafely();
+                        jump.CharacterBodyIndex = bodyLocation.Index;
+                        jump.CharacterVelocityChange = characterUp * MathF.Max(0, character.JumpVelocity - characterUpVelocity);
+                        jump.SupportBodyIndex = -1;
+                    }
+                    character.Supported = false;
+                }
+                else if (supportCandidate.Depth > float.MinValue)
+                {
+                    //If a support currently exists and there is still an old constraint, then update it.
+                    //If a support currently exists and there is not an old constraint, add the new constraint.
+
+                    //Project the view direction down onto the surface as represented by the contact normal.
+                    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.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,
+                        "The calibrated support normal and the character's up direction should have a positive dot product if the maximum slope is working properly. Is the maximum slope >= pi/2?");
+                    surfaceBasis.Z = up * (rayDistance / rayVelocity) - character.ViewDirection;
+                    var zLengthSquared = surfaceBasis.Z.LengthSquared();
+                    if (zLengthSquared > 1e-12f)
+                    {
+                        surfaceBasis.Z /= MathF.Sqrt(zLengthSquared);
+                    }
+                    else
+                    {
+                        QuaternionEx.GetQuaternionBetweenNormalizedVectors(Vector3.UnitY, surfaceBasis.Y, out var rotation);
+                        QuaternionEx.TransformUnitZ(rotation, out surfaceBasis.Z);
+                    }
+                    surfaceBasis.X = Vector3.Cross(surfaceBasis.Y, surfaceBasis.Z);
+                    QuaternionEx.CreateFromRotationMatrix(surfaceBasis, out var surfaceBasisQuaternion);
+                    if (supportCandidate.Support.Mobility != CollidableMobility.Static)
                     {
-                        //If a support currently exists and there is still an old constraint, then update it.
-                        //If a support currently exists and there is not an old constraint, add the new constraint.
-
-                        //Project the view direction down onto the surface as represented by the contact normal.
-                        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);
-                        var rayDistance = Vector3.Dot(character.ViewDirection, surfaceBasis.Y);
-                        var rayVelocity = Vector3.Dot(up, surfaceBasis.Y);
-                        Debug.Assert(rayVelocity > 0,
-                            "The calibrated support normal and the character's up direction should have a positive dot product if the maximum slope is working properly. Is the maximum slope >= pi/2?");
-                        surfaceBasis.Z = up * (rayDistance / rayVelocity) - character.ViewDirection;
-                        var zLengthSquared = surfaceBasis.Z.LengthSquared();
-                        if (zLengthSquared > 1e-12f)
+                        //The character is supported by a body.
+                        var motionConstraint = new DynamicCharacterMotionConstraint
                         {
-                            surfaceBasis.Z /= MathF.Sqrt(zLengthSquared);
+                            MaximumHorizontalForce = character.MaximumHorizontalForce,
+                            MaximumVerticalForce = character.MaximumVerticalForce,
+                            OffsetFromCharacterToSupportPoint = supportCandidate.OffsetFromCharacter,
+                            OffsetFromSupportToSupportPoint = supportCandidate.OffsetFromSupport,
+                            SurfaceBasis = surfaceBasisQuaternion,
+                            TargetVelocity = character.TargetVelocity,
+                            Depth = supportCandidate.Depth
+                        };
+                        if (character.Supported && !shouldRemove)
+                        {
+                            //Already exists, update it.
+                            Simulation.Solver.ApplyDescriptionWithoutWaking(character.MotionConstraintHandle, motionConstraint);
                         }
                         else
                         {
-                            QuaternionEx.GetQuaternionBetweenNormalizedVectors(Vector3.UnitY, surfaceBasis.Y, out var rotation);
-                            QuaternionEx.TransformUnitZ(rotation, out surfaceBasis.Z);
+                            //Doesn't exist, mark it for addition.
+                            ref var pendingConstraint = ref analyzeContactsWorkerCache.DynamicConstraintsToAdd.AllocateUnsafely();
+                            pendingConstraint.Description = motionConstraint;
+                            pendingConstraint.CharacterIndex = characterIndex;
                         }
-                        surfaceBasis.X = Vector3.Cross(surfaceBasis.Y, surfaceBasis.Z);
-                        QuaternionEx.CreateFromRotationMatrix(surfaceBasis, out var surfaceBasisQuaternion);
-                        if (supportCandidate.Support.Mobility != CollidableMobility.Static)
+                    }
+                    else
+                    {
+                        //The character is supported by a static.
+                        var motionConstraint = new StaticCharacterMotionConstraint
                         {
-                            //The character is supported by a body.
-                            var motionConstraint = new DynamicCharacterMotionConstraint
-                            {
-                                MaximumHorizontalForce = character.MaximumHorizontalForce,
-                                MaximumVerticalForce = character.MaximumVerticalForce,
-                                OffsetFromCharacterToSupportPoint = supportCandidate.OffsetFromCharacter,
-                                OffsetFromSupportToSupportPoint = supportCandidate.OffsetFromSupport,
-                                SurfaceBasis = surfaceBasisQuaternion,
-                                TargetVelocity = character.TargetVelocity,
-                                Depth = supportCandidate.Depth
-                            };
-                            if (character.Supported && !shouldRemove)
-                            {
-                                //Already exists, update it.
-                                Simulation.Solver.ApplyDescriptionWithoutWaking(character.MotionConstraintHandle, ref motionConstraint);
-                            }
-                            else
-                            {
-                                //Doesn't exist, mark it for addition.
-                                ref var pendingConstraint = ref analyzeContactsWorkerCache.DynamicConstraintsToAdd.AllocateUnsafely();
-                                pendingConstraint.Description = motionConstraint;
-                                pendingConstraint.CharacterIndex = characterIndex;
-                            }
+                            MaximumHorizontalForce = character.MaximumHorizontalForce,
+                            MaximumVerticalForce = character.MaximumVerticalForce,
+                            OffsetFromCharacterToSupportPoint = supportCandidate.OffsetFromCharacter,
+                            SurfaceBasis = surfaceBasisQuaternion,
+                            TargetVelocity = character.TargetVelocity,
+                            Depth = supportCandidate.Depth
+                        };
+                        if (character.Supported && !shouldRemove)
+                        {
+                            //Already exists, update it.
+                            Simulation.Solver.ApplyDescriptionWithoutWaking(character.MotionConstraintHandle, motionConstraint);
                         }
                         else
                         {
-                            //The character is supported by a static.
-                            var motionConstraint = new StaticCharacterMotionConstraint
-                            {
-                                MaximumHorizontalForce = character.MaximumHorizontalForce,
-                                MaximumVerticalForce = character.MaximumVerticalForce,
-                                OffsetFromCharacterToSupportPoint = supportCandidate.OffsetFromCharacter,
-                                SurfaceBasis = surfaceBasisQuaternion,
-                                TargetVelocity = character.TargetVelocity,
-                                Depth = supportCandidate.Depth
-                            };
-                            if (character.Supported && !shouldRemove)
-                            {
-                                //Already exists, update it.
-                                Simulation.Solver.ApplyDescriptionWithoutWaking(character.MotionConstraintHandle, ref motionConstraint);
-                            }
-                            else
-                            {
-                                //Doesn't exist, mark it for addition.
-                                ref var pendingConstraint = ref analyzeContactsWorkerCache.StaticConstraintsToAdd.AllocateUnsafely();
-                                pendingConstraint.Description = motionConstraint;
-                                pendingConstraint.CharacterIndex = characterIndex;
-                            }
+                            //Doesn't exist, mark it for addition.
+                            ref var pendingConstraint = ref analyzeContactsWorkerCache.StaticConstraintsToAdd.AllocateUnsafely();
+                            pendingConstraint.Description = motionConstraint;
+                            pendingConstraint.CharacterIndex = characterIndex;
                         }
-                        character.Supported = true;
-                        character.Support = supportCandidate.Support;
-                    }
-                    else
-                    {
-                        character.Supported = false;
                     }
+                    character.Supported = true;
+                    character.Support = supportCandidate.Support;
+                }
+                else
+                {
+                    character.Supported = false;
                 }
-                //The TryJump flag is always reset even if the attempt failed.
-                character.TryJump = false;
             }
+            //The TryJump flag is always reset even if the attempt failed.
+            character.TryJump = false;
         }
+    }
 
-        struct AnalyzeContactsJob
+    struct AnalyzeContactsJob
+    {
+        public int Start;
+        public int ExclusiveEnd;
+    }
+
+    int analysisJobIndex;
+    int analysisJobCount;
+    Buffer<AnalyzeContactsJob> jobs;
+    Action<int> analyzeContactsWorker;
+
+    void AnalyzeContactsWorker(int workerIndex)
+    {
+        int jobIndex;
+        while ((jobIndex = Interlocked.Increment(ref analysisJobIndex)) < analysisJobCount)
         {
-            public int Start;
-            public int ExclusiveEnd;
+            ref var job = ref jobs[jobIndex];
+            AnalyzeContactsForCharacterRegion(job.Start, job.ExclusiveEnd, workerIndex);
         }
+    }
+
+
+    /// <summary>
+    /// Updates all character support states and motion constraints based on the current character goals and all the contacts collected since the last call to AnalyzeContacts. 
+    /// Attach to a simulation callback where the most recent contact is available and before the solver executes.
+    /// </summary>
+    void AnalyzeContacts(float dt, IThreadDispatcher threadDispatcher)
+    {
+        //var start = Stopwatch.GetTimestamp();
+        Debug.Assert(contactCollectionWorkerCaches.Allocated, "Worker caches weren't properly allocated; did you forget to call PrepareForContacts before collision detection?");
 
-        int analysisJobIndex;
-        int analysisJobCount;
-        Buffer<AnalyzeContactsJob> jobs;
-        Action<int> analyzeContactsWorker;
-        void AnalyzeContactsWorker(int workerIndex)
+        if (threadDispatcher == null)
+        {
+            pool.Take(1, out analyzeContactsWorkerCaches);
+            analyzeContactsWorkerCaches[0] = new AnalyzeContactsWorkerCache(characters.Count, pool);
+            AnalyzeContactsForCharacterRegion(0, characters.Count, 0);
+        }
+        else
         {
-            int jobIndex;
-            while ((jobIndex = Interlocked.Increment(ref analysisJobIndex)) < analysisJobCount)
+            analysisJobCount = Math.Min(characters.Count, threadDispatcher.ThreadCount * 4);
+            if (analysisJobCount > 0)
             {
-                ref var job = ref jobs[jobIndex];
-                AnalyzeContactsForCharacterRegion(job.Start, job.ExclusiveEnd, workerIndex);
+                pool.Take(threadDispatcher.ThreadCount, out analyzeContactsWorkerCaches);
+                pool.Take(analysisJobCount, out jobs);
+                for (int i = 0; i < threadDispatcher.ThreadCount; ++i)
+                {
+                    analyzeContactsWorkerCaches[i] = new AnalyzeContactsWorkerCache(characters.Count, pool);
+                }
+                var baseCount = characters.Count / analysisJobCount;
+                var remainder = characters.Count - baseCount * analysisJobCount;
+                var previousEnd = 0;
+                for (int i = 0; i < analysisJobCount; ++i)
+                {
+                    ref var job = ref jobs[i];
+                    job.Start = previousEnd;
+                    job.ExclusiveEnd = job.Start + (i < remainder ? baseCount + 1 : baseCount);
+                    previousEnd = job.ExclusiveEnd;
+                }
+                analysisJobIndex = -1;
+                threadDispatcher.DispatchWorkers(analyzeContactsWorker, analysisJobCount);
+                pool.Return(ref jobs);
             }
         }
-
-
-        /// <summary>
-        /// Updates all character support states and motion constraints based on the current character goals and all the contacts collected since the last call to AnalyzeContacts. 
-        /// Attach to a simulation callback where the most recent contact is available and before the solver executes.
-        /// </summary>
-        void AnalyzeContacts(float dt, IThreadDispatcher threadDispatcher)
+        //We're done with all the contact collection worker caches.
+        for (int i = 0; i < contactCollectionWorkerCaches.Length; ++i)
         {
-            //var start = Stopwatch.GetTimestamp();
-            Debug.Assert(contactCollectionWorkerCaches.Allocated, "Worker caches weren't properly allocated; did you forget to call PrepareForContacts before collision detection?");
+            contactCollectionWorkerCaches[i].Dispose(pool);
+        }
+        pool.Return(ref contactCollectionWorkerCaches);
 
-            if (threadDispatcher == null)
-            {
-                pool.Take(1, out analyzeContactsWorkerCaches);
-                analyzeContactsWorkerCaches[0] = new AnalyzeContactsWorkerCache(characters.Count, pool);
-                AnalyzeContactsForCharacterRegion(0, characters.Count, 0);
-            }
-            else
+        if (analyzeContactsWorkerCaches.Allocated)
+        {
+            //Flush all the worker caches. Note that we perform all removals before moving onto any additions to avoid unnecessary constraint batches
+            //caused by the new and old constraint affecting the same bodies.
+            for (int threadIndex = 0; threadIndex < analyzeContactsWorkerCaches.Length; ++threadIndex)
             {
-                analysisJobCount = Math.Min(characters.Count, threadDispatcher.ThreadCount * 4);
-                if (analysisJobCount > 0)
+                ref var cache = ref analyzeContactsWorkerCaches[threadIndex];
+                for (int i = 0; i < cache.ConstraintHandlesToRemove.Count; ++i)
                 {
-                    pool.Take(threadDispatcher.ThreadCount, out analyzeContactsWorkerCaches);
-                    pool.Take(analysisJobCount, out jobs);
-                    for (int i = 0; i < threadDispatcher.ThreadCount; ++i)
-                    {
-                        analyzeContactsWorkerCaches[i] = new AnalyzeContactsWorkerCache(characters.Count, pool);
-                    }
-                    var baseCount = characters.Count / analysisJobCount;
-                    var remainder = characters.Count - baseCount * analysisJobCount;
-                    var previousEnd = 0;
-                    for (int i = 0; i < analysisJobCount; ++i)
-                    {
-                        ref var job = ref jobs[i];
-                        job.Start = previousEnd;
-                        job.ExclusiveEnd = job.Start + (i < remainder ? baseCount + 1 : baseCount);
-                        previousEnd = job.ExclusiveEnd;
-                    }
-                    analysisJobIndex = -1;
-                    threadDispatcher.DispatchWorkers(analyzeContactsWorker);
-                    pool.Return(ref jobs);
+                    Simulation.Solver.Remove(cache.ConstraintHandlesToRemove[i]);
                 }
             }
-            //We're done with all the contact collection worker caches.
-            for (int i = 0; i < contactCollectionWorkerCaches.Length; ++i)
+            for (int threadIndex = 0; threadIndex < analyzeContactsWorkerCaches.Length; ++threadIndex)
             {
-                contactCollectionWorkerCaches[i].Dispose(pool);
-            }
-            pool.Return(ref contactCollectionWorkerCaches);
-
-            if (analyzeContactsWorkerCaches.Allocated)
-            {
-                //Flush all the worker caches. Note that we perform all removals before moving onto any additions to avoid unnecessary constraint batches
-                //caused by the new and old constraint affecting the same bodies.
-                for (int threadIndex = 0; threadIndex < analyzeContactsWorkerCaches.Length; ++threadIndex)
+                ref var workerCache = ref analyzeContactsWorkerCaches[threadIndex];
+                for (int i = 0; i < workerCache.StaticConstraintsToAdd.Count; ++i)
                 {
-                    ref var cache = ref analyzeContactsWorkerCaches[threadIndex];
-                    for (int i = 0; i < cache.ConstraintHandlesToRemove.Count; ++i)
-                    {
-                        Simulation.Solver.Remove(cache.ConstraintHandlesToRemove[i]);
-                    }
+                    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, pendingConstraint.Description);
                 }
-                for (int threadIndex = 0; threadIndex < analyzeContactsWorkerCaches.Length; ++threadIndex)
+                for (int i = 0; i < workerCache.DynamicConstraintsToAdd.Count; ++i)
                 {
-                    ref var workerCache = ref analyzeContactsWorkerCaches[threadIndex];
-                    for (int i = 0; i < workerCache.StaticConstraintsToAdd.Count; ++i)
-                    {
-                        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);
-                    }
-                    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);
-                    }
-                    ref var activeSet = ref Simulation.Bodies.ActiveSet;
-                    for (int i = 0; i < workerCache.Jumps.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, pendingConstraint.Description);
+                }
+                ref var activeSet = ref Simulation.Bodies.ActiveSet;
+                for (int i = 0; i < workerCache.Jumps.Count; ++i)
+                {
+                    ref var jump = ref workerCache.Jumps[i];
+                    activeSet.DynamicsState[jump.CharacterBodyIndex].Motion.Velocity.Linear += jump.CharacterVelocityChange;
+                    if (jump.SupportBodyIndex >= 0)
                     {
-                        ref var jump = ref workerCache.Jumps[i];
-                        activeSet.Velocities[jump.CharacterBodyIndex].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.DynamicsState[jump.CharacterBodyIndex].Inertia.Local.InverseMass, jump.SupportImpulseOffset);
                     }
-                    workerCache.Dispose(pool);
                 }
-                pool.Return(ref analyzeContactsWorkerCaches);
+                workerCache.Dispose(pool);
             }
-
-            //var end = Stopwatch.GetTimestamp();
-            //Console.WriteLine($"Time (ms): {(end - start) / (1e-3 * Stopwatch.Frequency)}");
+            pool.Return(ref analyzeContactsWorkerCaches);
         }
 
-        /// <summary>
-        /// Ensures that the internal structures of the character controllers system can handle the given number of characters and body handles, resizing if necessary.
-        /// </summary>
-        /// <param name="characterCapacity">Minimum character capacity to require.</param>
-        /// <param name="bodyHandleCapacity">Minimum number of body handles to allocate space for.</param>
-        public void EnsureCapacity(int characterCapacity, int bodyHandleCapacity)
+        //var end = Stopwatch.GetTimestamp();
+        //Console.WriteLine($"Time (ms): {(end - start) / (1e-3 * Stopwatch.Frequency)}");
+    }
+
+    /// <summary>
+    /// Ensures that the internal structures of the character controllers system can handle the given number of characters and body handles, resizing if necessary.
+    /// </summary>
+    /// <param name="characterCapacity">Minimum character capacity to require.</param>
+    /// <param name="bodyHandleCapacity">Minimum number of body handles to allocate space for.</param>
+    public void EnsureCapacity(int characterCapacity, int bodyHandleCapacity)
+    {
+        characters.EnsureCapacity(characterCapacity, pool);
+        if (bodyHandleToCharacterIndex.Length < bodyHandleCapacity)
         {
-            characters.EnsureCapacity(characterCapacity, pool);
-            if (bodyHandleToCharacterIndex.Length < bodyHandleCapacity)
-            {
-                ResizeBodyHandleCapacity(bodyHandleCapacity);
-            }
+            ResizeBodyHandleCapacity(bodyHandleCapacity);
         }
+    }
 
-        /// <summary>
-        /// Resizes the internal structures of the character controllers system for the target sizes. Will not shrink below the currently active data size.
-        /// </summary>
-        /// <param name="characterCapacity">Target character capacity to allocate space for.</param>
-        /// <param name="bodyHandleCapacity">Target number of body handles to allocate space for.</param>
-        public void Resize(int characterCapacity, int bodyHandleCapacity)
+    /// <summary>
+    /// Resizes the internal structures of the character controllers system for the target sizes. Will not shrink below the currently active data size.
+    /// </summary>
+    /// <param name="characterCapacity">Target character capacity to allocate space for.</param>
+    /// <param name="bodyHandleCapacity">Target number of body handles to allocate space for.</param>
+    public void Resize(int characterCapacity, int bodyHandleCapacity)
+    {
+        int lastOccupiedIndex = -1;
+        for (int i = bodyHandleToCharacterIndex.Length - 1; i >= 0; --i)
         {
-            int lastOccupiedIndex = -1;
-            for (int i = bodyHandleToCharacterIndex.Length - 1; i >= 0; --i)
+            if (bodyHandleToCharacterIndex[i] != -1)
             {
-                if (bodyHandleToCharacterIndex[i] != -1)
-                {
-                    lastOccupiedIndex = i;
-                    break;
-                }
+                lastOccupiedIndex = i;
+                break;
             }
-            var targetHandleCapacity = BufferPool.GetCapacityForCount<int>(Math.Max(lastOccupiedIndex + 1, bodyHandleCapacity));
-            if (targetHandleCapacity != bodyHandleToCharacterIndex.Length)
-                ResizeBodyHandleCapacity(targetHandleCapacity);
-
-            var targetCharacterCapacity = BufferPool.GetCapacityForCount<int>(Math.Max(characters.Count, characterCapacity));
-            if (targetCharacterCapacity != characters.Span.Length)
-                characters.Resize(targetCharacterCapacity, pool);
         }
+        var targetHandleCapacity = BufferPool.GetCapacityForCount<int>(Math.Max(lastOccupiedIndex + 1, bodyHandleCapacity));
+        if (targetHandleCapacity != bodyHandleToCharacterIndex.Length)
+            ResizeBodyHandleCapacity(targetHandleCapacity);
+
+        var targetCharacterCapacity = BufferPool.GetCapacityForCount<int>(Math.Max(characters.Count, characterCapacity));
+        if (targetCharacterCapacity != characters.Span.Length)
+            characters.Resize(targetCharacterCapacity, pool);
+    }
 
-        bool disposed;
-        /// <summary>
-        /// Returns pool-allocated resources.
-        /// </summary>
-        public void Dispose()
+    bool disposed;
+    /// <summary>
+    /// Returns pool-allocated resources.
+    /// </summary>
+    public void Dispose()
+    {
+        if (!disposed)
         {
-            if (!disposed)
-            {
-                disposed = true;
-                Simulation.Timestepper.BeforeCollisionDetection -= PrepareForContacts;
-                Simulation.Timestepper.CollisionsDetected -= AnalyzeContacts;
-                characters.Dispose(pool);
-                pool.Return(ref bodyHandleToCharacterIndex);
-            }
+            disposed = true;
+            Simulation.Timestepper.BeforeCollisionDetection -= PrepareForContacts;
+            Simulation.Timestepper.CollisionsDetected -= AnalyzeContacts;
+            characters.Dispose(pool);
+            pool.Return(ref bodyHandleToCharacterIndex);
         }
     }
 }
\ No newline at end of file
diff --git a/Demos/Demos/Characters/CharacterDemo.cs b/Demos/Demos/Characters/CharacterDemo.cs
index 497b2a3b7..b6879329e 100644
--- a/Demos/Demos/Characters/CharacterDemo.cs
+++ b/Demos/Demos/Characters/CharacterDemo.cs
@@ -10,237 +10,231 @@
 using DemoRenderer.UI;
 using OpenTK.Input;
 
-namespace Demos.Demos.Characters
+namespace Demos.Demos.Characters;
+
+/// <summary>
+/// Shows one way of using the dynamic character controller in the context of a giant newt and levitating pads.
+/// </summary>
+public class CharacterDemo : Demo
 {
-    /// <summary>
-    /// Shows one way of using the dynamic character controller in the context of a giant newt and levitating pads.
-    /// </summary>
-    public class CharacterDemo : Demo
+    CharacterControllers characters;
+    public override void Initialize(ContentArchive content, Camera camera)
     {
-        CharacterControllers characters;
-        public unsafe override void Initialize(ContentArchive content, Camera camera)
+        camera.Position = new Vector3(20, 10, 20);
+        camera.Yaw = MathF.PI;
+        camera.Pitch = 0;
+
+        characters = new CharacterControllers(BufferPool);
+        Simulation = Simulation.Create(BufferPool, new CharacterNarrowphaseCallbacks(characters), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(8, 1));
+
+        CreateCharacter(new Vector3(0, 2, -4));
+
+        //Create a bunch of legos to hurt your feet on.
+        var random = new Random(5);
+        var origin = new Vector3(-3f, 0.5f, 0);
+        var spacing = new Vector3(0.5f, 0, -0.5f);
+        for (int i = 0; i < 12; ++i)
         {
-            camera.Position = new Vector3(20, 10, 20);
-            camera.Yaw = MathF.PI;
-            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 PositionFirstTimestepper());
-
-            CreateCharacter(new Vector3(0, 2, -4));
-
-            //Create a bunch of legos to hurt your feet on.
-            var random = new Random(5);
-            var origin = new Vector3(-3f, 0.5f, 0);
-            var spacing = new Vector3(0.5f, 0, -0.5f);
-            for (int i = 0; i < 12; ++i)
+            for (int j = 0; j < 12; ++j)
             {
-                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(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);
+                var choice = (i + j) % 3;
+                switch (choice)
                 {
-                    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 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;
-
-                    }
+                    case 0:
+                        Simulation.Bodies.Add(BodyDescription.CreateDynamic((position, orientation), shape.ComputeInertia(1), shapeIndex, 0.01f));
+                        break;
+                    case 1:
+                        Simulation.Bodies.Add(BodyDescription.CreateKinematic((position, orientation), shapeIndex, 0.01f));
+                        break;
+                    case 2:
+                        Simulation.Statics.Add(new StaticDescription(position, orientation, shapeIndex));
+                        break;
+
                 }
             }
+        }
 
-            //Add some spinning fans to get slapped by.
-            var bladeDescription = BodyDescription.CreateConvexDynamic(new Vector3(), 3, Simulation.Shapes, new Box(10, 0.2f, 2));
-            var bladeBaseDescription = BodyDescription.CreateConvexKinematic(new Vector3(), Simulation.Shapes, new Box(0.2f, 1, 0.2f));
-            for (int i = 0; i < 3; ++i)
-            {
-                bladeBaseDescription.Pose.Position = new Vector3(-22, 1, i * 11);
-                bladeDescription.Pose.Position = new Vector3(-22, 1.7f, i * 11);
-                var baseHandle = Simulation.Bodies.Add(bladeBaseDescription);
-                var bladeHandle = Simulation.Bodies.Add(bladeDescription);
-                Simulation.Solver.Add(baseHandle, bladeHandle,
-                    new Hinge
-                    {
-                        LocalHingeAxisA = Vector3.UnitY,
-                        LocalHingeAxisB = Vector3.UnitY,
-                        LocalOffsetA = new Vector3(0, 0.7f, 0),
-                        LocalOffsetB = new Vector3(0, 0, 0),
-                        SpringSettings = new SpringSettings(30, 1)
-                    });
-                Simulation.Solver.Add(baseHandle, bladeHandle,
-                    new AngularAxisMotor
-                    {
-                        LocalAxisA = Vector3.UnitY,
-                        TargetVelocity = (i + 1) * (i + 1) * (i + 1) * (i + 1) * 0.2f,
-                        Settings = new MotorSettings(5 * (i + 1), 0.0001f)
-                    });
-            }
+        //Add some spinning fans to get slapped by.
+        var bladeDescription = BodyDescription.CreateConvexDynamic(new Vector3(), 3, Simulation.Shapes, new Box(10, 0.2f, 2));
+        var bladeBaseDescription = BodyDescription.CreateConvexKinematic(new Vector3(), Simulation.Shapes, new Box(0.2f, 1, 0.2f));
+        for (int i = 0; i < 3; ++i)
+        {
+            bladeBaseDescription.Pose.Position = new Vector3(-22, 1, i * 11);
+            bladeDescription.Pose.Position = new Vector3(-22, 1.7f, i * 11);
+            var baseHandle = Simulation.Bodies.Add(bladeBaseDescription);
+            var bladeHandle = Simulation.Bodies.Add(bladeDescription);
+            Simulation.Solver.Add(baseHandle, bladeHandle,
+                new Hinge
+                {
+                    LocalHingeAxisA = Vector3.UnitY,
+                    LocalHingeAxisB = Vector3.UnitY,
+                    LocalOffsetA = new Vector3(0, 0.7f, 0),
+                    LocalOffsetB = new Vector3(0, 0, 0),
+                    SpringSettings = new SpringSettings(30, 1)
+                });
+            Simulation.Solver.Add(baseHandle, bladeHandle,
+                new AngularAxisMotor
+                {
+                    LocalAxisA = Vector3.UnitY,
+                    TargetVelocity = (i + 1) * (i + 1) * (i + 1) * (i + 1) * 0.2f,
+                    Settings = new MotorSettings(5 * (i + 1), 0.0001f)
+                });
+        }
 
-            //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);
-            Simulation.Statics.Add(new StaticDescription(new Vector3(0, 0.5f, 0), new CollidableDescription(Simulation.Shapes.Add(newtMesh), 0.1f)));
+        //Include a giant newt to test character-newt behavior and to ensure thematic consistency.
+        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.
-            var tongueBase = Simulation.Bodies.Add(BodyDescription.CreateKinematic(new Vector3(0, 8.4f, 24), default, default));
-            var tongue = Simulation.Bodies.Add(BodyDescription.CreateConvexDynamic(new Vector3(0, 8.4f, 27.5f), 1, Simulation.Shapes, new Box(1, 0.1f, 6f)));
-            Simulation.Solver.Add(tongueBase, tongue, new Hinge
-            {
-                LocalHingeAxisA = Vector3.UnitX,
-                LocalHingeAxisB = Vector3.UnitX,
-                LocalOffsetB = new Vector3(0, 0, -3f),
-                SpringSettings = new SpringSettings(30, 1)
-            });
-            Simulation.Solver.Add(tongueBase, tongue, new AngularServo
-            {
-                TargetRelativeRotationLocalA = Quaternion.Identity,
-                ServoSettings = ServoSettings.Default,
-                SpringSettings = new SpringSettings(2, 0)
-            });
-
-            //And a seesaw thing?
-            var seesawBase = Simulation.Bodies.Add(BodyDescription.CreateKinematic(new Vector3(0, 1f, 34f), new CollidableDescription(Simulation.Shapes.Add(new Box(0.2f, 1, 0.2f)), 0.1f), new BodyActivityDescription(0.01f)));
-            var seesaw = Simulation.Bodies.Add(BodyDescription.CreateConvexDynamic(new Vector3(0, 1.7f, 34f), 1, Simulation.Shapes, new Box(1, 0.1f, 6f)));
-            Simulation.Solver.Add(seesawBase, seesaw, new Hinge
-            {
-                LocalHingeAxisA = Vector3.UnitX,
-                LocalHingeAxisB = Vector3.UnitX,
-                LocalOffsetA = new Vector3(0, 0.7f, 0),
-                LocalOffsetB = new Vector3(0, 0, 0),
-                SpringSettings = new SpringSettings(30, 1)
-            });
-
-            Simulation.Bodies.Add(BodyDescription.CreateConvexDynamic(new Vector3(0, 2.25f, 35.5f), 0.5f, Simulation.Shapes, new Box(1f, 1f, 1f)));
-
-            //Create some moving platforms to jump on.
-            movingPlatforms = new MovingPlatform[16];
-            Func<double, RigidPose> poseCreator = time =>
-            {
-                RigidPose pose;
-                var horizontalScale = (float)(45 + 10 * Math.Sin(time * 0.015));
-                //Float in a noisy ellipse around the newt.
-                pose.Position = new Vector3(0.7f * horizontalScale * (float)Math.Sin(time * 0.1), 10 + 4 * (float)Math.Sin((time + Math.PI * 0.5f) * 0.25), horizontalScale * (float)Math.Cos(time * 0.1));
-                //As the platform goes behind the newt, dip toward the ground. Use smoothstep for a less jerky ride.
-                var x = MathF.Max(0f, MathF.Min(1f, 1f - (pose.Position.Z + 20f) / -20f));
-                var smoothStepped = 3 * x * x - 2 * x * x * x;
-                pose.Position.Y = smoothStepped * (pose.Position.Y - 0.025f) + 0.025f;
-                pose.Orientation = Quaternion.Identity;
-                return pose;
-            };
-            var platformCollidable = new CollidableDescription(Simulation.Shapes.Add(new Box(5, 1, 5)), 0.1f);
-            for (int i = 0; i < movingPlatforms.Length; ++i)
-            {
-                movingPlatforms[i] = new MovingPlatform(platformCollidable, i * 3559, 1f / 60f, Simulation, poseCreator);
-            }
-            var box = new Box(4, 1, 4);
-            var boxCollidable = new CollidableDescription(Simulation.Shapes.Add(box), 0.1f);
-            const int width = 8;
-            for (int i = 0; i < width; ++i)
+        //Give the newt a tongue, I guess.
+        var tongueBase = Simulation.Bodies.Add(BodyDescription.CreateKinematic(new Vector3(0, 8.4f, 24), default, default));
+        var tongue = Simulation.Bodies.Add(BodyDescription.CreateConvexDynamic(new Vector3(0, 8.4f, 27.5f), 1, Simulation.Shapes, new Box(1, 0.1f, 6f)));
+        Simulation.Solver.Add(tongueBase, tongue, new Hinge
+        {
+            LocalHingeAxisA = Vector3.UnitX,
+            LocalHingeAxisB = Vector3.UnitX,
+            LocalOffsetB = new Vector3(0, 0, -3f),
+            SpringSettings = new SpringSettings(30, 1)
+        });
+        Simulation.Solver.Add(tongueBase, tongue, new AngularServo
+        {
+            TargetRelativeRotationLocalA = Quaternion.Identity,
+            ServoSettings = ServoSettings.Default,
+            SpringSettings = new SpringSettings(2, 0)
+        });
+
+        //And a seesaw thing?
+        var seesawBase = Simulation.Bodies.Add(BodyDescription.CreateKinematic(new Vector3(0, 1f, 34f), Simulation.Shapes.Add(new Box(0.2f, 1, 0.2f)), 0.01f));
+        var seesaw = Simulation.Bodies.Add(BodyDescription.CreateConvexDynamic(new Vector3(0, 1.7f, 34f), 1, Simulation.Shapes, new Box(1, 0.1f, 6f)));
+        Simulation.Solver.Add(seesawBase, seesaw, new Hinge
+        {
+            LocalHingeAxisA = Vector3.UnitX,
+            LocalHingeAxisB = Vector3.UnitX,
+            LocalOffsetA = new Vector3(0, 0.7f, 0),
+            LocalOffsetB = new Vector3(0, 0, 0),
+            SpringSettings = new SpringSettings(30, 1)
+        });
+
+        Simulation.Bodies.Add(BodyDescription.CreateConvexDynamic(new Vector3(0, 2.25f, 35.5f), 0.5f, Simulation.Shapes, new Box(1f, 1f, 1f)));
+
+        //Create some moving platforms to jump on.
+        movingPlatforms = new MovingPlatform[16];
+        Func<double, RigidPose> poseCreator = time =>
+        {
+            RigidPose pose;
+            var horizontalScale = (float)(45 + 10 * Math.Sin(time * 0.015));
+            //Float in a noisy ellipse around the newt.
+            pose.Position = new Vector3(0.7f * horizontalScale * (float)Math.Sin(time * 0.1), 10 + 4 * (float)Math.Sin((time + Math.PI * 0.5f) * 0.25), horizontalScale * (float)Math.Cos(time * 0.1));
+            //As the platform goes behind the newt, dip toward the ground. Use smoothstep for a less jerky ride.
+            var x = MathF.Max(0f, MathF.Min(1f, 1f - (pose.Position.Z + 20f) / -20f));
+            var smoothStepped = 3 * x * x - 2 * x * x * x;
+            pose.Position.Y = smoothStepped * (pose.Position.Y - 0.025f) + 0.025f;
+            pose.Orientation = Quaternion.Identity;
+            return pose;
+        };
+        var platformShapeIndex = Simulation.Shapes.Add(new Box(5, 1, 5));
+        for (int i = 0; i < movingPlatforms.Length; ++i)
+        {
+            movingPlatforms[i] = new MovingPlatform(platformShapeIndex, i * 3559, 1f / 60f, Simulation, poseCreator);
+        }
+        var box = new Box(4, 1, 4);
+        var boxShapeIndex = Simulation.Shapes.Add(box);
+        const int width = 8;
+        for (int i = 0; i < width; ++i)
+        {
+            for (int j = 0; j < width; ++j)
             {
-                for (int j = 0; j < width; ++j)
-                {
-                    Simulation.Statics.Add(new StaticDescription(new Vector3(box.Width, 0, box.Length) * new Vector3(i, 0, j) + new Vector3(32f, 1, 0), boxCollidable));
-                }
-
+                Simulation.Statics.Add(new StaticDescription(new Vector3(box.Width, 0, box.Length) * new Vector3(i, 0, j) + new Vector3(32f, 1, 0), boxShapeIndex));
             }
 
-            //Prevent the character from falling into the void.
-            Simulation.Statics.Add(new StaticDescription(new Vector3(0, 0, 0), new CollidableDescription(Simulation.Shapes.Add(new Box(200, 1, 200)), 0.1f)));
         }
 
+        //Prevent the character from falling into the void.
+        Simulation.Statics.Add(new StaticDescription(new Vector3(0, 0, 0), Simulation.Shapes.Add(new Box(200, 1, 200))));
+    }
 
-        struct MovingPlatform
-        {
-            public BodyHandle BodyHandle;
-            public float InverseGoalSatisfactionTime;
-            public double TimeOffset;
-            public Func<double, RigidPose> PoseCreator;
 
-            public MovingPlatform(CollidableDescription collidable, double timeOffset, float goalSatisfactionTime, Simulation simulation, Func<double, RigidPose> poseCreator)
-            {
-                PoseCreator = poseCreator;
-                BodyHandle = simulation.Bodies.Add(BodyDescription.CreateKinematic(poseCreator(timeOffset), collidable, new BodyActivityDescription(-1)));
-                InverseGoalSatisfactionTime = 1f / goalSatisfactionTime;
-                TimeOffset = timeOffset;
-            }
+    struct MovingPlatform
+    {
+        public BodyHandle BodyHandle;
+        public float InverseGoalSatisfactionTime;
+        public double TimeOffset;
+        public Func<double, RigidPose> PoseCreator;
 
-            public void Update(Simulation simulation, double time)
-            {
-                var body = simulation.Bodies.GetBodyReference(BodyHandle);
-                ref var pose = ref body.Pose;
-                ref var velocity = ref body.Velocity;
-                var targetPose = PoseCreator(time + TimeOffset);
-                velocity.Linear = (targetPose.Position - pose.Position) * InverseGoalSatisfactionTime;
-                QuaternionEx.GetRelativeRotationWithoutOverlap(pose.Orientation, targetPose.Orientation, out var rotation);
-                QuaternionEx.GetAxisAngleFromQuaternion(rotation, out var axis, out var angle);
-                velocity.Angular = axis * (angle * InverseGoalSatisfactionTime);
-            }
+        public MovingPlatform(CollidableDescription collidable, double timeOffset, float goalSatisfactionTime, Simulation simulation, Func<double, RigidPose> poseCreator)
+        {
+            PoseCreator = poseCreator;
+            BodyHandle = simulation.Bodies.Add(BodyDescription.CreateKinematic(poseCreator(timeOffset), collidable, -1));
+            InverseGoalSatisfactionTime = 1f / goalSatisfactionTime;
+            TimeOffset = timeOffset;
         }
-        MovingPlatform[] movingPlatforms;
 
-        bool characterActive;
-        CharacterInput character;
-        double time;
-        void CreateCharacter(Vector3 position)
+        public void Update(Simulation simulation, double time)
         {
-            characterActive = true;
-            character = new CharacterInput(characters, position, new Capsule(0.5f, 1), 0.1f, 1, 20, 100, 6, 4, MathF.PI * 0.4f);
+            var body = simulation.Bodies[BodyHandle];
+            ref var pose = ref body.Pose;
+            ref var velocity = ref body.Velocity;
+            var targetPose = PoseCreator(time + TimeOffset);
+            velocity.Linear = (targetPose.Position - pose.Position) * InverseGoalSatisfactionTime;
+            QuaternionEx.GetRelativeRotationWithoutOverlap(pose.Orientation, targetPose.Orientation, out var rotation);
+            QuaternionEx.GetAxisAngleFromQuaternion(rotation, out var axis, out var angle);
+            velocity.Angular = axis * (angle * InverseGoalSatisfactionTime);
         }
+    }
+    MovingPlatform[] movingPlatforms;
 
-        public override void Update(Window window, Camera camera, Input input, float dt)
+    bool characterActive;
+    CharacterInput character;
+    double time;
+    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);
+    }
+
+    public override void Update(Window window, Camera camera, Input input, float dt)
+    {
+        if (input.WasPushed(Key.C))
         {
-            const float simulationDt = 1 / 60f;
-            if (input.WasPushed(Key.C))
-            {
-                if (characterActive)
-                {
-                    character.Dispose();
-                    characterActive = false;
-                }
-                else
-                {
-                    CreateCharacter(camera.Position);
-                }
-            }
             if (characterActive)
             {
-                //Console.WriteLine($"Supported: {characters.GetCharacterByBodyHandle(character.BodyHandle).Supported}");
-                character.UpdateCharacterGoals(input, camera, simulationDt);
+                character.Dispose();
+                characterActive = false;
             }
-            //Using a fixed time per update to match the demos simulation update rate.
-            time += 1 / 60f;
-            for (int i = 0; i < movingPlatforms.Length; ++i)
+            else
             {
-                movingPlatforms[i].Update(Simulation, time);
+                CreateCharacter(camera.Position);
             }
-            base.Update(window, camera, input, dt);
         }
+        if (characterActive)
+        {
+            //Console.WriteLine($"Supported: {characters.GetCharacterByBodyHandle(character.BodyHandle).Supported}");
+            character.UpdateCharacterGoals(input, camera, TimestepDuration);
+        }
+        //Using a fixed time per update to match the demos simulation update rate.
+        time += TimestepDuration;
+        for (int i = 0; i < movingPlatforms.Length; ++i)
+        {
+            movingPlatforms[i].Update(Simulation, time);
+        }
+        base.Update(window, camera, input, dt);
+    }
 
-        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)
+    {
+        float textHeight = 16;
+        var position = new Vector2(32, renderer.Surface.Resolution.Y - textHeight * 9);
+        renderer.TextBatcher.Write(text.Clear().Append("Toggle character: C"), position, textHeight, new Vector3(1), font);
+        position.Y += textHeight * 1.2f;
+        character.RenderControls(position, textHeight, renderer.TextBatcher, text, font);
+        if (characterActive)
         {
-            float textHeight = 16;
-            var position = new Vector2(32, renderer.Surface.Resolution.Y - textHeight * 9);
-            renderer.TextBatcher.Write(text.Clear().Append("Toggle character: C"), position, textHeight, new Vector3(1), font);
-            position.Y += textHeight * 1.2f;
-            character.RenderControls(position, textHeight, renderer.TextBatcher, text, font);
-            if (characterActive)
-            {
-                character.UpdateCameraPosition(camera);
-            }
-            base.Render(renderer, camera, input, text, font);
+            character.UpdateCameraPosition(camera);
         }
+        base.Render(renderer, camera, input, text, font);
     }
 }
 
diff --git a/Demos/Demos/Characters/CharacterInput.cs b/Demos/Demos/Characters/CharacterInput.cs
index 314f88052..46016b659 100644
--- a/Demos/Demos/Characters/CharacterInput.cs
+++ b/Demos/Demos/Characters/CharacterInput.cs
@@ -9,171 +9,172 @@
 using OpenTK.Input;
 using BepuUtilities;
 
-namespace Demos.Demos.Characters
+namespace Demos.Demos.Characters;
+
+/// <summary>
+/// Convenience structure that wraps a CharacterController reference and its associated body.
+/// </summary>
+/// <remarks>
+/// <para>This should be treated as an example- nothing here is intended to suggest how you *must* handle characters. 
+/// On the contrary, this does some fairly inefficient stuff if you're dealing with hundreds of characters in a predictable way.
+/// It's just a fairly convenient interface for demos usage.</para>
+/// <para>Note that all characters are dynamic and respond to constraints and forces in the simulation.</para>
+/// </remarks>
+public struct CharacterInput
 {
-    /// <summary>
-    /// Convenience structure that wraps a CharacterController reference and its associated body.
-    /// </summary>
-    /// <remarks>
-    /// <para>This should be treated as an example- nothing here is intended to suggest how you *must* handle characters. 
-    /// On the contrary, this does some fairly inefficient stuff if you're dealing with hundreds of characters in a predictable way.
-    /// It's just a fairly convenient interface for demos usage.</para>
-    /// <para>Note that all characters are dynamic and respond to constraints and forces in the simulation.</para>
-    /// </remarks>
-    public struct CharacterInput
-    {
-        BodyHandle bodyHandle;
-        CharacterControllers characters;
-        float speed;
-        Capsule shape;
+    BodyHandle bodyHandle;
+    CharacterControllers characters;
+    float speed;
+    Capsule shape;
 
-        public BodyHandle BodyHandle { get { return bodyHandle; } }
+    public BodyHandle BodyHandle { get { return bodyHandle; } }
 
-        public CharacterInput(CharacterControllers characters, Vector3 initialPosition, Capsule shape,
-            float speculativeMargin, float mass, float maximumHorizontalForce, float maximumVerticalGlueForce,
-            float jumpVelocity, float speed, float maximumSlope = MathF.PI * 0.25f)
-        {
-            this.characters = characters;
-            var shapeIndex = characters.Simulation.Shapes.Add(shape);
+    public CharacterInput(CharacterControllers characters, Vector3 initialPosition, Capsule shape,
+        float minimumSpeculativeMargin, float mass, float maximumHorizontalForce, float maximumVerticalGlueForce,
+        float jumpVelocity, float speed, float maximumSlope = MathF.PI * 0.25f)
+    {
+        this.characters = characters;
+        var shapeIndex = characters.Simulation.Shapes.Add(shape);
 
-            //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, speculativeMargin), new BodyActivityDescription(shape.Radius * 0.02f)));
-            ref var character = ref characters.AllocateCharacter(bodyHandle);
-            character.LocalUp = new Vector3(0, 1, 0);
-            character.CosMaximumSlope = MathF.Cos(maximumSlope);
-            character.JumpVelocity = jumpVelocity;
-            character.MaximumVerticalForce = maximumVerticalGlueForce;
-            character.MaximumHorizontalForce = maximumHorizontalForce;
-            character.MinimumSupportDepth = shape.Radius * -0.01f;
-            character.MinimumSupportContinuationDepth = -speculativeMargin;
-            this.speed = speed;
-            this.shape = shape;
-        }
+        //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(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);
+        character.JumpVelocity = jumpVelocity;
+        character.MaximumVerticalForce = maximumVerticalGlueForce;
+        character.MaximumHorizontalForce = maximumHorizontalForce;
+        character.MinimumSupportDepth = shape.Radius * -0.01f;
+        character.MinimumSupportContinuationDepth = -minimumSpeculativeMargin;
+        this.speed = speed;
+        this.shape = shape;
+    }
 
-        static Key MoveForward = Key.W;
-        static Key MoveBackward = Key.S;
-        static Key MoveRight = Key.D;
-        static Key MoveLeft = Key.A;
-        static Key Sprint = Key.LShift;
-        static Key Jump = Key.Space;
-        static Key JumpAlternate = Key.BackSpace; //I have a weird keyboard.
+    static Key MoveForward = Key.W;
+    static Key MoveBackward = Key.S;
+    static Key MoveRight = Key.D;
+    static Key MoveLeft = Key.A;
+    static Key Sprint = Key.LShift;
+    static Key Jump = Key.Space;
+    static Key JumpAlternate = Key.BackSpace; //I have a weird keyboard.
 
-        public void UpdateCharacterGoals(Input input, Camera camera, float simulationTimestepDuration)
+    public void UpdateCharacterGoals(Input input, Camera camera, float simulationTimestepDuration)
+    {
+        Vector2 movementDirection = default;
+        if (input.IsDown(MoveForward))
         {
-            Vector2 movementDirection = default;
-            if (input.IsDown(MoveForward))
-            {
-                movementDirection = new Vector2(0, 1);
-            }
-            if (input.IsDown(MoveBackward))
-            {
-                movementDirection += new Vector2(0, -1);
-            }
-            if (input.IsDown(MoveLeft))
-            {
-                movementDirection += new Vector2(-1, 0);
-            }
-            if (input.IsDown(MoveRight))
-            {
-                movementDirection += new Vector2(1, 0);
-            }
-            var movementDirectionLengthSquared = movementDirection.LengthSquared();
-            if (movementDirectionLengthSquared > 0)
-            {
-                movementDirection /= MathF.Sqrt(movementDirectionLengthSquared);
-            }
-
-            ref var character = ref characters.GetCharacterByBodyHandle(bodyHandle);
-            character.TryJump = input.WasPushed(Jump) || input.WasPushed(JumpAlternate);
-            var characterBody = new BodyReference(bodyHandle, characters.Simulation.Bodies);
-            var effectiveSpeed = input.IsDown(Sprint) ? speed * 1.75f : speed;
-            var newTargetVelocity = movementDirection * effectiveSpeed;
-            var viewDirection = camera.Forward;
-            //Modifying the character's raw data does not automatically wake the character up, so we do so explicitly if necessary.
-            //If you don't explicitly wake the character up, it won't respond to the changed motion goals.
-            //(You can also specify a negative deactivation threshold in the BodyActivityDescription to prevent the character from sleeping at all.)
-            if (!characterBody.Awake &&
-                ((character.TryJump && character.Supported) ||
-                newTargetVelocity != character.TargetVelocity ||
-                (newTargetVelocity != Vector2.Zero && character.ViewDirection != viewDirection)))
-            {
-                characters.Simulation.Awakener.AwakenBody(character.BodyHandle);
-            }
-            character.TargetVelocity = newTargetVelocity;
-            character.ViewDirection = viewDirection;
-
-            //The character's motion constraints aren't active while the character is in the air, so if we want air control, we'll need to apply it ourselves.
-            //(You could also modify the constraints to do this, but the robustness of solved constraints tends to be a lot less important for air control.)
-            //There isn't any one 'correct' way to implement air control- it's a nonphysical gameplay thing, and this is just one way to do it.
-            //Note that this permits accelerating along a particular direction, and never attempts to slow down the character.
-            //This allows some movement quirks common in some game character controllers.
-            //Consider what happens if, starting from a standstill, you accelerate fully along X, then along Z- your full velocity magnitude will be sqrt(2) * maximumAirSpeed.
-            //Feel free to try alternative implementations. Again, there is no one correct approach.
-            if (!character.Supported && movementDirectionLengthSquared > 0)
-            {
-                QuaternionEx.Transform(character.LocalUp, characterBody.Pose.Orientation, out var characterUp);
-                var characterRight = Vector3.Cross(character.ViewDirection, characterUp);
-                var rightLengthSquared = characterRight.LengthSquared();
-                if (rightLengthSquared > 1e-10f)
-                {
-                    characterRight /= MathF.Sqrt(rightLengthSquared);
-                    var characterForward = Vector3.Cross(characterUp, characterRight);
-                    var worldMovementDirection = characterRight * movementDirection.X + characterForward * movementDirection.Y;
-                    var currentVelocity = Vector3.Dot(characterBody.Velocity.Linear, worldMovementDirection);
-                    //We'll arbitrarily set air control to be a fraction of supported movement's speed/force.
-                    const float airControlForceScale = .2f;
-                    const float airControlSpeedScale = .2f;
-                    var airAccelerationDt = characterBody.LocalInertia.InverseMass * character.MaximumHorizontalForce * airControlForceScale * simulationTimestepDuration;
-                    var maximumAirSpeed = effectiveSpeed * airControlSpeedScale;
-                    var targetVelocity = MathF.Min(currentVelocity + airAccelerationDt, maximumAirSpeed);
-                    //While we shouldn't allow the character to continue accelerating in the air indefinitely, trying to move in a given direction should never slow us down in that direction.
-                    var velocityChangeAlongMovementDirection = MathF.Max(0, targetVelocity - currentVelocity);
-                    characterBody.Velocity.Linear += worldMovementDirection * velocityChangeAlongMovementDirection;
-                    Debug.Assert(characterBody.Awake, "Velocity changes don't automatically update objects; the character should have already been woken up before applying air control.");
-                }
-            }
+            movementDirection = new Vector2(0, 1);
         }
-
-        public void UpdateCameraPosition(Camera camera, float cameraBackwardOffsetScale = 4)
+        if (input.IsDown(MoveBackward))
         {
-            //We'll override the demo harness's camera control by attaching the camera to the character controller body.
-            ref var character = ref characters.GetCharacterByBodyHandle(bodyHandle);
-            var characterBody = new BodyReference(bodyHandle, characters.Simulation.Bodies);
-            //Use a simple sorta-neck model so that when the camera looks down, the center of the screen sees past the character.
-            //Makes mouselocked ray picking easier.
-            camera.Position = characterBody.Pose.Position + new Vector3(0, shape.HalfLength, 0) +
-                camera.Up * (shape.Radius * 1.2f) -
-                camera.Forward * (shape.HalfLength + shape.Radius) * cameraBackwardOffsetScale;
+            movementDirection += new Vector2(0, -1);
         }
-
-        void RenderControl(ref Vector2 position, float textHeight, string controlName, string controlValue, TextBuilder text, TextBatcher textBatcher, Font font)
+        if (input.IsDown(MoveLeft))
         {
-            text.Clear().Append(controlName).Append(": ").Append(controlValue);
-            textBatcher.Write(text, position, textHeight, new Vector3(1), font);
-            position.Y += textHeight * 1.1f;
+            movementDirection += new Vector2(-1, 0);
         }
-        public void RenderControls(Vector2 position, float textHeight, TextBatcher textBatcher, TextBuilder text, Font font)
+        if (input.IsDown(MoveRight))
         {
-            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);
+            movementDirection += new Vector2(1, 0);
+        }
+        var movementDirectionLengthSquared = movementDirection.LengthSquared();
+        if (movementDirectionLengthSquared > 0)
+        {
+            movementDirection /= MathF.Sqrt(movementDirectionLengthSquared);
         }
 
+        ref var character = ref characters.GetCharacterByBodyHandle(bodyHandle);
+        character.TryJump = input.WasPushed(Jump) || input.WasPushed(JumpAlternate);
+        var characterBody = new BodyReference(bodyHandle, characters.Simulation.Bodies);
+        var effectiveSpeed = input.IsDown(Sprint) ? speed * 1.75f : speed;
+        var newTargetVelocity = movementDirection * effectiveSpeed;
+        var viewDirection = camera.Forward;
+        //Modifying the character's raw data does not automatically wake the character up, so we do so explicitly if necessary.
+        //If you don't explicitly wake the character up, it won't respond to the changed motion goals.
+        //(You can also specify a negative deactivation threshold in the BodyActivityDescription to prevent the character from sleeping at all.)
+        if (!characterBody.Awake &&
+            ((character.TryJump && character.Supported) ||
+            newTargetVelocity != character.TargetVelocity ||
+            (newTargetVelocity != Vector2.Zero && character.ViewDirection != viewDirection)))
+        {
+            characters.Simulation.Awakener.AwakenBody(character.BodyHandle);
+        }
+        character.TargetVelocity = newTargetVelocity;
+        character.ViewDirection = viewDirection;
 
-        /// <summary>
-        /// Removes the character's body from the simulation and the character from the associated characters set.
-        /// </summary>
-        public void Dispose()
+        //The character's motion constraints aren't active while the character is in the air, so if we want air control, we'll need to apply it ourselves.
+        //(You could also modify the constraints to do this, but the robustness of solved constraints tends to be a lot less important for air control.)
+        //There isn't any one 'correct' way to implement air control- it's a nonphysical gameplay thing, and this is just one way to do it.
+        //Note that this permits accelerating along a particular direction, and never attempts to slow down the character.
+        //This allows some movement quirks common in some game character controllers.
+        //Consider what happens if, starting from a standstill, you accelerate fully along X, then along Z- your full velocity magnitude will be sqrt(2) * maximumAirSpeed.
+        //Feel free to try alternative implementations. Again, there is no one correct approach.
+        if (!character.Supported && movementDirectionLengthSquared > 0)
         {
-            characters.Simulation.Shapes.Remove(new BodyReference(bodyHandle, characters.Simulation.Bodies).Collidable.Shape);
-            characters.Simulation.Bodies.Remove(bodyHandle);
-            characters.RemoveCharacterByBodyHandle(bodyHandle);
+            QuaternionEx.Transform(character.LocalUp, characterBody.Pose.Orientation, out var characterUp);
+            var characterRight = Vector3.Cross(character.ViewDirection, characterUp);
+            var rightLengthSquared = characterRight.LengthSquared();
+            if (rightLengthSquared > 1e-10f)
+            {
+                characterRight /= MathF.Sqrt(rightLengthSquared);
+                var characterForward = Vector3.Cross(characterUp, characterRight);
+                var worldMovementDirection = characterRight * movementDirection.X + characterForward * movementDirection.Y;
+                var currentVelocity = Vector3.Dot(characterBody.Velocity.Linear, worldMovementDirection);
+                //We'll arbitrarily set air control to be a fraction of supported movement's speed/force.
+                const float airControlForceScale = .2f;
+                const float airControlSpeedScale = .2f;
+                var airAccelerationDt = characterBody.LocalInertia.InverseMass * character.MaximumHorizontalForce * airControlForceScale * simulationTimestepDuration;
+                var maximumAirSpeed = effectiveSpeed * airControlSpeedScale;
+                var targetVelocity = MathF.Min(currentVelocity + airAccelerationDt, maximumAirSpeed);
+                //While we shouldn't allow the character to continue accelerating in the air indefinitely, trying to move in a given direction should never slow us down in that direction.
+                var velocityChangeAlongMovementDirection = MathF.Max(0, targetVelocity - currentVelocity);
+                characterBody.Velocity.Linear += worldMovementDirection * velocityChangeAlongMovementDirection;
+                Debug.Assert(characterBody.Awake, "Velocity changes don't automatically update objects; the character should have already been woken up before applying air control.");
+            }
         }
     }
+
+    public void UpdateCameraPosition(Camera camera, float cameraBackwardOffsetScale = 4)
+    {
+        //We'll override the demo harness's camera control by attaching the camera to the character controller body.
+        ref var character = ref characters.GetCharacterByBodyHandle(bodyHandle);
+        var characterBody = new BodyReference(bodyHandle, characters.Simulation.Bodies);
+        //Use a simple sorta-neck model so that when the camera looks down, the center of the screen sees past the character.
+        //Makes mouselocked ray picking easier.
+        camera.Position = characterBody.Pose.Position + new Vector3(0, shape.HalfLength, 0) +
+            camera.Up * (shape.Radius * 1.2f) -
+            camera.Forward * (shape.HalfLength + shape.Radius) * cameraBackwardOffsetScale;
+    }
+
+    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 void RenderControls(Vector2 position, float textHeight, TextBatcher textBatcher, TextBuilder text, Font 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);
+    }
+
+
+    /// <summary>
+    /// Removes the character's body from the simulation and the character from the associated characters set.
+    /// </summary>
+    public void Dispose()
+    {
+        characters.Simulation.Shapes.Remove(new BodyReference(bodyHandle, characters.Simulation.Bodies).Collidable.Shape);
+        characters.Simulation.Bodies.Remove(bodyHandle);
+        characters.RemoveCharacterByBodyHandle(bodyHandle);
+    }
 }
 
 
diff --git a/Demos/Demos/Characters/CharacterMotionConstraint.cs b/Demos/Demos/Characters/CharacterMotionConstraint.cs
index b86ffabba..b1dc03152 100644
--- a/Demos/Demos/Characters/CharacterMotionConstraint.cs
+++ b/Demos/Demos/Characters/CharacterMotionConstraint.cs
@@ -1,4 +1,10 @@
-using BepuUtilities;
+
+
+
+
+
+
+using BepuUtilities;
 using BepuPhysics;
 using System.Numerics;
 using System;
@@ -7,645 +13,647 @@
 using BepuUtilities.Memory;
 using static BepuUtilities.GatherScatter;
 
-namespace Demos.Demos.Characters
-{ 
-    public struct CharacterMotionAccumulatedImpulse
-    {
-        public Vector2Wide Horizontal;
-        public Vector<float> Vertical;
-    }
+namespace Demos.Demos.Characters;
+
+public struct CharacterMotionAccumulatedImpulse
+{
+    public Vector2Wide Horizontal;
+    public Vector<float> Vertical;
+}
+
+//IMPORTANT NOTE: The static and dynamic motion constraint variants were autogenerated from a text template. To modify the constraint, modify the text template instead.
+//Using code generators to generate these sorts of constraint variants can work around some of the limitations in compile time expressiveness-
+//here, we avoid creating a bunch of duplicate math for the one and two body constraint cases which would have to be separately maintained.
+//There are other cases where this is pushed even further. Within the engine, the dozens of contact constraint variants are generated by text templates.
+//This isn't the most wonderful solution in terms of development experience or tooling, 
+//but it maximizes performance and avoids the need for in-language turing complete metaprogramming, so I'm okay with it.
+
+
+//Constraint descriptions provide an explicit mapping from the array-of-structures format to the internal array-of-structures-of-arrays format used by the solver.
+//Note that there is a separate description for the one and two body case- constraint implementations take advantage of the lack of a second body to reduce data gathering requirements.
+/// <summary>
+/// Description of a character motion constraint where the support is static.
+/// </summary>
+public struct StaticCharacterMotionConstraint : IOneBodyConstraintDescription<StaticCharacterMotionConstraint>
+{
+    /// <summary>
+    /// Maximum force that the horizontal motion constraint can apply to reach the current velocity goal.
+    /// </summary>
+    public float MaximumHorizontalForce;
+    /// <summary>
+    /// Maximum force that the vertical motion constraint can apply to fight separation.
+    /// </summary>
+    public float MaximumVerticalForce;
+    /// <summary>
+    /// Target horizontal velocity in terms of the basis X and -Z axes.
+    /// </summary>
+    public Vector2 TargetVelocity;
+    /// <summary>
+    /// Depth of the supporting contact. The vertical motion constraint permits separating velocity if, after a frame, the objects will still be touching.
+    /// </summary>
+	public float Depth;
+    /// <summary>
+    /// Stores the quaternion-packed orthonormal basis for the motion constraint. When expanded into a matrix, X and Z will represent the Right and Backward directions respectively. Y will represent Up.
+    /// In other words, a target tangential velocity of (4, 2) will result in a goal velocity of 4 along the (1, 0, 0) * Basis direction and a goal velocity of 2 along the (0, 0, -1) * Basis direction.
+    /// All motion moving along the (0, 1, 0) * Basis axis will be fought against by the vertical motion constraint.
+    /// </summary>
+    public Quaternion SurfaceBasis;
+    /// <summary>
+    /// World space offset from the character's center to apply impulses at.
+    /// </summary>
+    public Vector3 OffsetFromCharacterToSupportPoint;
 
-    //IMPORTANT NOTE: The static and dynamic motion constraint variants were autogenerated from a text template. To modify the constraint, modify the text template instead.
-    //Using code generators to generate these sorts of constraint variants can work around some of the limitations in compile time expressiveness-
-    //here, we avoid creating a bunch of duplicate math for the one and two body constraint cases which would have to be separately maintained.
-    //There are other cases where this is pushed even further. Within the engine, the dozens of contact constraint variants are generated by text templates.
-    //This isn't the most wonderful solution in terms of development experience or tooling, 
-    //but it maximizes performance and avoids the need for in-language turing complete metaprogramming, so I'm okay with it.
 
-    //Constraint descriptions provide an explicit mapping from the array-of-structures format to the internal array-of-structures-of-arrays format used by the solver.
-    //Note that there is a separate description for the one and two body case- constraint implementations take advantage of the lack of a second body to reduce data gathering requirements.
+    //It's possible to create multiple descriptions for the same underlying constraint type id which can update different parts of the constraint data.
+    //This functionality isn't used very often, though- you'll notice that the engine has a 1:1 mapping (at least at the time of this writing).
+    //But in principle, it doesn't have to be that way. So, the description must provide information about the type and type id.
     /// <summary>
-    /// Description of a character motion constraint where the support is static.
+    /// Gets the constraint type id that this description is associated with. 
     /// </summary>
-    public struct StaticCharacterMotionConstraint : IOneBodyConstraintDescription<StaticCharacterMotionConstraint>
+    public static int ConstraintTypeId => StaticCharacterMotionTypeProcessor.BatchTypeId;
+
+    /// <summary>
+    /// Gets the TypeProcessor type that is associated with this description.
+    /// </summary>
+    public static Type TypeProcessorType => typeof(StaticCharacterMotionTypeProcessor);    
+    /// <summary>
+    /// Creates a type processor for this constraint type.
+    /// </summary>
+    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,
+    //we refer to that slot using the same struct and then write only to the first slot.
+    //(Note that accessing slots after the first may result in access violations; the 'offset instance' is not guaranteed to refer to valid data beyond the first slot!)
+    public readonly void ApplyDescription(ref TypeBatch batch, int bundleIndex, int innerIndex)
     {
-        /// <summary>
-        /// Maximum force that the horizontal motion constraint can apply to reach the current velocity goal.
-        /// </summary>
-        public float MaximumHorizontalForce;
-        /// <summary>
-        /// Maximum force that the vertical motion constraint can apply to fight separation.
-        /// </summary>
-        public float MaximumVerticalForce;
-        /// <summary>
-        /// Target horizontal velocity in terms of the basis X and -Z axes.
-        /// </summary>
-        public Vector2 TargetVelocity;
-        /// <summary>
-        /// Depth of the supporting contact. The vertical motion constraint permits separating velocity if, after a frame, the objects will still be touching.
-        /// </summary>
-		public float Depth;
-        /// <summary>
-        /// Stores the quaternion-packed orthonormal basis for the motion constraint. When expanded into a matrix, X and Z will represent the Right and Backward directions respectively. Y will represent Up.
-        /// In other words, a target tangential velocity of (4, 2) will result in a goal velocity of 4 along the (1, 0, 0) * Basis direction and a goal velocity of 2 along the (0, 0, -1) * Basis direction.
-        /// All motion moving along the (0, 1, 0) * Basis axis will be fought against by the vertical motion constraint.
-        /// </summary>
-        public Quaternion SurfaceBasis;
-        /// <summary>
-        /// World space offset from the character's center to apply impulses at.
-        /// </summary>
-        public Vector3 OffsetFromCharacterToSupportPoint;
-
-        //It's possible to create multiple descriptions for the same underlying constraint type id which can update different parts of the constraint data.
-        //This functionality isn't used very often, though- you'll notice that the engine has a 1:1 mapping (at least at the time of this writing).
-        //But in principle, it doesn't have to be that way. So, the description must provide information about the type and type id.
-        /// <summary>
-        /// Gets the constraint type id that this description is associated with. 
-        /// </summary>
-        public readonly int ConstraintTypeId => StaticCharacterMotionTypeProcessor.BatchTypeId;
-
-        /// <summary>
-        /// Gets the TypeProcessor type that is associated with this description.
-        /// </summary>
-        public readonly Type TypeProcessorType => typeof(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,
-        //we refer to that slot using the same struct and then write only to the first slot.
-        //(Note that accessing slots after the first may result in access violations; the 'offset instance' is not guaranteed to refer to valid data beyond the first slot!)
-        public readonly void ApplyDescription(ref TypeBatch batch, int bundleIndex, int innerIndex)
-        {
-            ref var target = ref GetOffsetInstance(ref Buffer<StaticCharacterMotionPrestep>.Get(ref batch.PrestepData, bundleIndex), innerIndex);
-            QuaternionWide.WriteFirst(SurfaceBasis, ref target.SurfaceBasis);
-            GetFirst(ref target.MaximumHorizontalForce) = MaximumHorizontalForce;
-            GetFirst(ref target.MaximumVerticalForce) = MaximumVerticalForce;
-            Vector2Wide.WriteFirst(TargetVelocity, ref target.TargetVelocity);
-			GetFirst(ref target.Depth) = Depth;
-            Vector3Wide.WriteFirst(OffsetFromCharacterToSupportPoint, ref target.OffsetFromCharacter);
-        }
-
-        public readonly void BuildDescription(ref TypeBatch batch, int bundleIndex, int innerIndex, out StaticCharacterMotionConstraint description)
-        {
-            ref var source = ref GetOffsetInstance(ref Buffer<StaticCharacterMotionPrestep>.Get(ref batch.PrestepData, bundleIndex), innerIndex);
-            QuaternionWide.ReadFirst(source.SurfaceBasis, out description.SurfaceBasis);
-            description.MaximumHorizontalForce = GetFirst(ref source.MaximumHorizontalForce);
-            description.MaximumVerticalForce = GetFirst(ref source.MaximumVerticalForce);
-            Vector2Wide.ReadFirst(source.TargetVelocity, out description.TargetVelocity);
-			description.Depth = GetFirst(ref source.Depth);
-            Vector3Wide.ReadFirst(source.OffsetFromCharacter, out description.OffsetFromCharacterToSupportPoint);
-        }
+        ref var target = ref GetOffsetInstance(ref Buffer<StaticCharacterMotionPrestep>.Get(ref batch.PrestepData, bundleIndex), innerIndex);
+        QuaternionWide.WriteFirst(SurfaceBasis, ref target.SurfaceBasis);
+        GetFirst(ref target.MaximumHorizontalForce) = MaximumHorizontalForce;
+        GetFirst(ref target.MaximumVerticalForce) = MaximumVerticalForce;
+        Vector2Wide.WriteFirst(TargetVelocity, ref target.TargetVelocity);
+		GetFirst(ref target.Depth) = Depth;
+        Vector3Wide.WriteFirst(OffsetFromCharacterToSupportPoint, ref target.OffsetFromCharacter);
+
     }
 
-    //Note that all the solver-side data is in terms of 'Wide' data types- the solver never works on just one constraint at a time. Instead,
-    //it executes them in bundles of width equal to the runtime/hardware exposed SIMD unit width. This lets the solver scale with wider compute units.
-    //(This is important for machines that can perform 8 or more operations per instruction- there's no good way to map a single constraint instance's 
-    //computation onto such a wide instruction, so if the solver tried to do such a thing, it would leave a huge amount of performance on the table.)
+    public static void BuildDescription(ref TypeBatch batch, int bundleIndex, int innerIndex, out StaticCharacterMotionConstraint description)
+    {
+        ref var source = ref GetOffsetInstance(ref Buffer<StaticCharacterMotionPrestep>.Get(ref batch.PrestepData, bundleIndex), innerIndex);
+        QuaternionWide.ReadFirst(source.SurfaceBasis, out description.SurfaceBasis);
+        description.MaximumHorizontalForce = GetFirst(ref source.MaximumHorizontalForce);
+        description.MaximumVerticalForce = GetFirst(ref source.MaximumVerticalForce);
+        Vector2Wide.ReadFirst(source.TargetVelocity, out description.TargetVelocity);
+		description.Depth = GetFirst(ref source.Depth);
+        Vector3Wide.ReadFirst(source.OffsetFromCharacter, out description.OffsetFromCharacterToSupportPoint);
 
-    //"Prestep" data can be thought of as the input to the solver. It describes everything the solver needs to know about.
-    /// <summary>
-    /// AOSOA formatted bundle of prestep data for multiple static-supported character motion constraints.
-    /// </summary>
-    public struct StaticCharacterMotionPrestep
+    }
+}
+
+//Note that all the solver-side data is in terms of 'Wide' data types- the solver never works on just one constraint at a time. Instead,
+//it executes them in bundles of width equal to the runtime/hardware exposed SIMD unit width. This lets the solver scale with wider compute units.
+//(This is important for machines that can perform 8 or more operations per instruction- there's no good way to map a single constraint instance's 
+//computation onto such a wide instruction, so if the solver tried to do such a thing, it would leave a huge amount of performance on the table.)
+
+//"Prestep" data can be thought of as the input to the solver. It describes everything the solver needs to know about.
+/// <summary>
+/// AOSOA formatted bundle of prestep data for multiple static-supported character motion constraints.
+/// </summary>
+public struct StaticCharacterMotionPrestep
+{
+    //Note that the prestep data layout is important. The solver tends to be severely memory bandwidth bound, so using a minimal representation is valuable.
+    //That's why the Basis is stored as a quaternion and not a full Matrix- the cost of the arithmetic operations to expand it back into the original matrix form is far less
+    //than the cost of loading all the extra lanes of data when scaled up to many cores.
+    public QuaternionWide SurfaceBasis;
+    public Vector<float> MaximumHorizontalForce;
+    public Vector<float> MaximumVerticalForce;
+	public Vector<float> Depth;
+    public Vector2Wide TargetVelocity;
+    public Vector3Wide OffsetFromCharacter;
+
+}
+
+public struct StaticCharacterMotionFunctions : IOneBodyConstraintFunctions<StaticCharacterMotionPrestep, CharacterMotionAccumulatedImpulse>
+{
+    [MethodImpl(MethodImplOptions.AggressiveInlining)]
+    static void ComputeJacobians(in Vector3Wide offsetA, in QuaternionWide basisQuaternion,
+        out Matrix3x3Wide basis,
+        out Matrix2x3Wide horizontalAngularJacobianA, 
+
+        out Vector3Wide verticalAngularJacobianA)
     {
-        //Note that the prestep data layout is important. The solver tends to be severely memory bandwidth bound, so using a minimal representation is valuable.
-        //That's why the Basis is stored as a quaternion and not a full Matrix- the cost of the arithmetic operations to expand it back into the original matrix form is far less
-        //than the cost of loading all the extra lanes of data when scaled up to many cores.
-        public QuaternionWide SurfaceBasis;
-        public Vector<float> MaximumHorizontalForce;
-        public Vector<float> MaximumVerticalForce;
-		public Vector<float> Depth;
-        public Vector2Wide TargetVelocity;
-        public Vector3Wide OffsetFromCharacter;
+        //Both of the motion constraints are velocity motors, like tangent friction. They don't actually have a position level goal.
+        //But if we did want to make such a position level goal, it could be expressed as:
+        //dot(basis.X, constrainedPointOnA - constrainedPointOnB) = 0
+        //dot(basis.Y, constrainedPointOnA - constrainedPointOnB) <= 0 
+        //dot(basis.Z, constrainedPointOnA - constrainedPointOnB) = 0
+        //Note that the Y axis, corresponding to the vertical motion constraint, is an inequality. It pulls toward the surface, but never pushes away.
+        //It also has a separate maximum force and acts on an independent axis; that's why we solve it as a separate constraint.
+        //To get a velocity constraint out of these position goals, differentiate with respect to time:
+        //d/dt(dot(basis.X, constrainedPointOnA - constrainedPointOnB)) = dot(basis.X, d/dt(constrainedPointOnA - constrainedPointOnB))
+        //                                                              = dot(basis.X, a.LinearVelocity + a.AngularVelocity x offsetToConstrainedPointOnA - b.linearVelocity - b.AngularVelocity x offsetToConstrainedPointOnB)
+        //Throwing some algebra and identities at it:
+        //dot(basis.X, a.LinearVelocity) + dot(basis.X, a.AngularVelocity x offsetToConstrainedPointOnA) + dot(-basis.X, b.LinearVelocity) + dot(basis.X, offsetToConstrainedPointOnB x b.AngularVelocity)
+        //dot(basis.X, a.LinearVelocity) + dot(a.AngularVelocity, offsetToConstrainedPointOnA x basis.X) + dot(-basis.X, b.LinearVelocity) + dot(b.AngularVelocity, basis.X x offsetToConstrainedPointOnB)
+        //The (transpose) jacobian is the transform that pulls the body velocity into constraint space- 
+        //and here, we can see that we have an axis being dotted with each component of the velocity. That's gives us the jacobian for that degree of freedom.
+        //The same form applies to all three axes of the basis, since they're all doing the same thing (just on different directions and with different force bounds).
+        //Note that we don't explicitly output linear jacobians- they are just the axes of the basis, and the linear jacobians of B are just the negated linear jacobians of A.
+        Matrix3x3Wide.CreateFromQuaternion(basisQuaternion, out basis);
+        Vector3Wide.CrossWithoutOverlap(offsetA, basis.X, out horizontalAngularJacobianA.X);
+        Vector3Wide.CrossWithoutOverlap(offsetA, basis.Y, out verticalAngularJacobianA);
+        Vector3Wide.CrossWithoutOverlap(offsetA, basis.Z, out horizontalAngularJacobianA.Y);
+
     }
 
-    //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
+    [MethodImpl(MethodImplOptions.AggressiveInlining)]
+    private static void ApplyHorizontalImpulse(in Matrix3x3Wide basis,
+        in Matrix2x3Wide angularJacobianA, in Vector2Wide constraintSpaceImpulse,
+        in BodyInertiaWide inertiaA, 
+
+        ref BodyVelocityWide velocityA)
     {
-        public QuaternionWide SurfaceBasis;
-        public Vector3Wide OffsetFromCharacter;
-        public Vector2Wide TargetVelocity;
-        public Symmetric2x2Wide HorizontalEffectiveMass;
-        public Vector<float> MaximumHorizontalImpulse;
-        public BodyInertias InertiaA;
-        public Vector<float> VerticalBiasVelocity;
-        public Vector<float> VerticalEffectiveMass;
-        public Vector<float> MaximumVerticalForce;
+        //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);
+        Vector3Wide.Scale(basis.Z, constraintSpaceImpulse.Y, out var linearImpulseAY);
+        Vector3Wide.Add(linearImpulseAX, linearImpulseAY, out var linearImpulseA);
+        Vector3Wide.Scale(linearImpulseA, inertiaA.InverseMass, out var linearChangeA);
+        Vector3Wide.Add(velocityA.Linear, linearChangeA, out velocityA.Linear);
+
+
+        Matrix2x3Wide.Transform(constraintSpaceImpulse, angularJacobianA, out var angularImpulseA);
+        Symmetric3x3Wide.TransformWithoutOverlap(angularImpulseA, inertiaA.InverseInertiaTensor, out var angularChangeA);
+        Vector3Wide.Add(velocityA.Angular, angularChangeA, out velocityA.Angular);
 
     }
 
-    public struct StaticCharacterMotionFunctions : IOneBodyConstraintFunctions<StaticCharacterMotionPrestep, StaticCharacterMotionProjection, CharacterMotionAccumulatedImpulse>
+    [MethodImpl(MethodImplOptions.AggressiveInlining)]
+    private static void ApplyVerticalImpulse(in Matrix3x3Wide basis,
+        in Vector3Wide angularJacobianA, in Vector<float> constraintSpaceImpulse,
+        in BodyInertiaWide inertiaA, 
+
+        ref BodyVelocityWide velocityA)
     {
-        [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        static void ComputeJacobians(in Vector3Wide offsetA, in QuaternionWide basisQuaternion,
-            out Matrix3x3Wide basis,
-            out Matrix2x3Wide horizontalAngularJacobianA, 
-            out Vector3Wide verticalAngularJacobianA)
-        {
-            //Both of the motion constraints are velocity motors, like tangent friction. They don't actually have a position level goal.
-            //But if we did want to make such a position level goal, it could be expressed as:
-            //dot(basis.X, constrainedPointOnA - constrainedPointOnB) = 0
-            //dot(basis.Y, constrainedPointOnA - constrainedPointOnB) <= 0 
-            //dot(basis.Z, constrainedPointOnA - constrainedPointOnB) = 0
-            //Note that the Y axis, corresponding to the vertical motion constraint, is an inequality. It pulls toward the surface, but never pushes away.
-            //It also has a separate maximum force and acts on an independent axis; that's why we solve it as a separate constraint.
-            //To get a velocity constraint out of these position goals, differentiate with respect to time:
-            //d/dt(dot(basis.X, constrainedPointOnA - constrainedPointOnB)) = dot(basis.X, d/dt(constrainedPointOnA - constrainedPointOnB))
-            //                                                              = dot(basis.X, a.LinearVelocity + a.AngularVelocity x offsetToConstrainedPointOnA - b.linearVelocity - b.AngularVelocity x offsetToConstrainedPointOnB)
-            //Throwing some algebra and identities at it:
-            //dot(basis.X, a.LinearVelocity) + dot(basis.X, a.AngularVelocity x offsetToConstrainedPointOnA) + dot(-basis.X, b.LinearVelocity) + dot(basis.X, offsetToConstrainedPointOnB x b.AngularVelocity)
-            //dot(basis.X, a.LinearVelocity) + dot(a.AngularVelocity, offsetToConstrainedPointOnA x basis.X) + dot(-basis.X, b.LinearVelocity) + dot(b.AngularVelocity, basis.X x offsetToConstrainedPointOnB)
-            //The (transpose) jacobian is the transform that pulls the body velocity into constraint space- 
-            //and here, we can see that we have an axis being dotted with each component of the velocity. That's gives us the jacobian for that degree of freedom.
-            //The same form applies to all three axes of the basis, since they're all doing the same thing (just on different directions and with different force bounds).
-            //Note that we don't explicitly output linear jacobians- they are just the axes of the basis, and the linear jacobians of B are just the negated linear jacobians of A.
-            Matrix3x3Wide.CreateFromQuaternion(basisQuaternion, out basis);
-            Vector3Wide.CrossWithoutOverlap(offsetA, basis.X, out horizontalAngularJacobianA.X);
-            Vector3Wide.CrossWithoutOverlap(offsetA, basis.Y, out verticalAngularJacobianA);
-            Vector3Wide.CrossWithoutOverlap(offsetA, basis.Z, out horizontalAngularJacobianA.Y);
-        }
-
-        [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public void Prestep(Bodies bodies, ref Vector<int> bodyReferences, int count, float dt, float inverseDt, ref BodyInertias inertiaA, 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<float>.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<float>.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,
-            in BodyInertias inertiaA, 
-            ref BodyVelocities 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);
-            Vector3Wide.Scale(basis.Z, constraintSpaceImpulse.Y, out var linearImpulseAY);
-            Vector3Wide.Add(linearImpulseAX, linearImpulseAY, out var linearImpulseA);
-            Vector3Wide.Scale(linearImpulseA, inertiaA.InverseMass, out var linearChangeA);
-            Vector3Wide.Add(velocityA.Linear, linearChangeA, out velocityA.Linear);
-
-            Matrix2x3Wide.Transform(constraintSpaceImpulse, angularJacobianA, out var angularImpulseA);
-            Symmetric3x3Wide.TransformWithoutOverlap(angularImpulseA, inertiaA.InverseInertiaTensor, out var angularChangeA);
-            Vector3Wide.Add(velocityA.Angular, angularChangeA, out velocityA.Angular);
-        }
-
-        [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        private static void ApplyVerticalImpulse(in Matrix3x3Wide basis,
-            in Vector3Wide angularJacobianA, in Vector<float> constraintSpaceImpulse,
-            in BodyInertias inertiaA, 
-            ref BodyVelocities velocityA)
-        {
-            Vector3Wide.Scale(basis.Y, constraintSpaceImpulse, out var linearImpulseA);
-            Vector3Wide.Scale(linearImpulseA, inertiaA.InverseMass, out var linearChangeA);
-            Vector3Wide.Add(velocityA.Linear, linearChangeA, out velocityA.Linear);
-
-            Vector3Wide.Scale(angularJacobianA, constraintSpaceImpulse, out var angularImpulseA);
-            Symmetric3x3Wide.TransformWithoutOverlap(angularImpulseA, inertiaA.InverseInertiaTensor, out var angularChangeA);
-            Vector3Wide.Add(velocityA.Angular, angularChangeA, out velocityA.Angular);
-        }
-
-        [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public void WarmStart(ref BodyVelocities 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 BodyVelocities 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<float>.One, projection.MaximumHorizontalImpulse / Vector.Max(new Vector<float>(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<float>.Zero, Vector.Max(accumulatedImpulse.Vertical + verticalCorrectiveImpulse, -projection.MaximumVerticalForce));
-            verticalCorrectiveImpulse = accumulatedImpulse.Vertical - previousVerticalAccumulatedImpulse;
-
-            ApplyVerticalImpulse(basis, verticalAngularJacobianA, verticalCorrectiveImpulse, projection.InertiaA, ref velocityA);
-        }
+        Vector3Wide.Scale(basis.Y, constraintSpaceImpulse, out var linearImpulseA);
+        Vector3Wide.Scale(linearImpulseA, inertiaA.InverseMass, out var linearChangeA);
+        Vector3Wide.Add(velocityA.Linear, linearChangeA, out velocityA.Linear);
+
+
+        Vector3Wide.Scale(angularJacobianA, constraintSpaceImpulse, out var angularImpulseA);
+        Symmetric3x3Wide.TransformWithoutOverlap(angularImpulseA, inertiaA.InverseInertiaTensor, out var angularChangeA);
+        Vector3Wide.Add(velocityA.Angular, angularChangeA, out velocityA.Angular);
 
     }
+               
+
+    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);
+        ApplyHorizontalImpulse(basis, horizontalAngularJacobianA, accumulatedImpulses.Horizontal, inertiaA, ref velocityA);
+        ApplyVerticalImpulse(basis, verticalAngularJacobianA, accumulatedImpulses.Vertical, inertiaA, ref 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.
+        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<float>(dt);
+        var maximumHorizontalImpulse = prestep.MaximumHorizontalForce * dtWide;
+        var scale = Vector.Min(Vector<float>.One, maximumHorizontalImpulse / Vector.Max(new Vector<float>(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<float>.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.
+        //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<float>.Zero, Vector.Max(accumulatedImpulses.Vertical + verticalCorrectiveImpulse, -maximumVerticalImpulse));
+        verticalCorrectiveImpulse = accumulatedImpulses.Vertical - previousVerticalAccumulatedImpulse;
+
+        ApplyVerticalImpulse(basis, verticalAngularJacobianA, verticalCorrectiveImpulse, inertiaA, ref velocityA);
+    }
 
-    //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<StaticCharacterMotionPrestep, StaticCharacterMotionProjection, CharacterMotionAccumulatedImpulse, StaticCharacterMotionFunctions>
+        
+    public static bool RequiresIncrementalSubstepUpdates => true;
+    public static void IncrementallyUpdateForSubstep(in Vector<float> dt, in BodyVelocityWide velocityA, ref StaticCharacterMotionPrestep prestep)
     {
-        /// <summary>
-        /// 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
-        /// registry of all types that can exist (custom ones, like this one, can always be created), but having it be constant helps simplify and optimize its internal usage.
-        /// </summary>
-        public const int BatchTypeId = 50;
+        //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;
     }
-    //Constraint descriptions provide an explicit mapping from the array-of-structures format to the internal array-of-structures-of-arrays format used by the solver.
-    //Note that there is a separate description for the one and two body case- constraint implementations take advantage of the lack of a second body to reduce data gathering requirements.
+}
+
+//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<StaticCharacterMotionPrestep, CharacterMotionAccumulatedImpulse, StaticCharacterMotionFunctions, AccessAll, AccessAll>
+{
     /// <summary>
-    /// Description of a character motion constraint where the support is dynamic.
+    /// 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
+    /// registry of all types that can exist (custom ones, like this one, can always be created), but having it be constant helps simplify and optimize its internal usage.
     /// </summary>
-    public struct DynamicCharacterMotionConstraint : ITwoBodyConstraintDescription<DynamicCharacterMotionConstraint>
-    {
-        /// <summary>
-        /// Maximum force that the horizontal motion constraint can apply to reach the current velocity goal.
-        /// </summary>
-        public float MaximumHorizontalForce;
-        /// <summary>
-        /// Maximum force that the vertical motion constraint can apply to fight separation.
-        /// </summary>
-        public float MaximumVerticalForce;
-        /// <summary>
-        /// Target horizontal velocity in terms of the basis X and -Z axes.
-        /// </summary>
-        public Vector2 TargetVelocity;
-        /// <summary>
-        /// Depth of the supporting contact. The vertical motion constraint permits separating velocity if, after a frame, the objects will still be touching.
-        /// </summary>
-		public float Depth;
-        /// <summary>
-        /// Stores the quaternion-packed orthonormal basis for the motion constraint. When expanded into a matrix, X and Z will represent the Right and Backward directions respectively. Y will represent Up.
-        /// In other words, a target tangential velocity of (4, 2) will result in a goal velocity of 4 along the (1, 0, 0) * Basis direction and a goal velocity of 2 along the (0, 0, -1) * Basis direction.
-        /// All motion moving along the (0, 1, 0) * Basis axis will be fought against by the vertical motion constraint.
-        /// </summary>
-        public Quaternion SurfaceBasis;
-        /// <summary>
-        /// World space offset from the character's center to apply impulses at.
-        /// </summary>
-        public Vector3 OffsetFromCharacterToSupportPoint;
-        /// <summary>
-        /// World space offset from the support's center to apply impulses at.
-        /// </summary>
-        public Vector3 OffsetFromSupportToSupportPoint;
-
-        //It's possible to create multiple descriptions for the same underlying constraint type id which can update different parts of the constraint data.
-        //This functionality isn't used very often, though- you'll notice that the engine has a 1:1 mapping (at least at the time of this writing).
-        //But in principle, it doesn't have to be that way. So, the description must provide information about the type and type id.
-        /// <summary>
-        /// Gets the constraint type id that this description is associated with. 
-        /// </summary>
-        public readonly int ConstraintTypeId => DynamicCharacterMotionTypeProcessor.BatchTypeId;
-
-        /// <summary>
-        /// Gets the TypeProcessor type that is associated with this description.
-        /// </summary>
-        public readonly Type TypeProcessorType => typeof(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,
-        //we refer to that slot using the same struct and then write only to the first slot.
-        //(Note that accessing slots after the first may result in access violations; the 'offset instance' is not guaranteed to refer to valid data beyond the first slot!)
-        public readonly void ApplyDescription(ref TypeBatch batch, int bundleIndex, int innerIndex)
-        {
-            ref var target = ref GetOffsetInstance(ref Buffer<DynamicCharacterMotionPrestep>.Get(ref batch.PrestepData, bundleIndex), innerIndex);
-            QuaternionWide.WriteFirst(SurfaceBasis, ref target.SurfaceBasis);
-            GetFirst(ref target.MaximumHorizontalForce) = MaximumHorizontalForce;
-            GetFirst(ref target.MaximumVerticalForce) = MaximumVerticalForce;
-            Vector2Wide.WriteFirst(TargetVelocity, ref target.TargetVelocity);
-			GetFirst(ref target.Depth) = Depth;
-            Vector3Wide.WriteFirst(OffsetFromCharacterToSupportPoint, ref target.OffsetFromCharacter);
-            Vector3Wide.WriteFirst(OffsetFromSupportToSupportPoint, ref target.OffsetFromSupport);
-        }
-
-        public readonly void BuildDescription(ref TypeBatch batch, int bundleIndex, int innerIndex, out DynamicCharacterMotionConstraint description)
-        {
-            ref var source = ref GetOffsetInstance(ref Buffer<DynamicCharacterMotionPrestep>.Get(ref batch.PrestepData, bundleIndex), innerIndex);
-            QuaternionWide.ReadFirst(source.SurfaceBasis, out description.SurfaceBasis);
-            description.MaximumHorizontalForce = GetFirst(ref source.MaximumHorizontalForce);
-            description.MaximumVerticalForce = GetFirst(ref source.MaximumVerticalForce);
-            Vector2Wide.ReadFirst(source.TargetVelocity, out description.TargetVelocity);
-			description.Depth = GetFirst(ref source.Depth);
-            Vector3Wide.ReadFirst(source.OffsetFromCharacter, out description.OffsetFromCharacterToSupportPoint);
-            Vector3Wide.ReadFirst(source.OffsetFromSupport, out description.OffsetFromSupportToSupportPoint);
-        }
-    }
+    public const int BatchTypeId = 50;
+}
+
+//Constraint descriptions provide an explicit mapping from the array-of-structures format to the internal array-of-structures-of-arrays format used by the solver.
+//Note that there is a separate description for the one and two body case- constraint implementations take advantage of the lack of a second body to reduce data gathering requirements.
+/// <summary>
+/// Description of a character motion constraint where the support is dynamic.
+/// </summary>
+public struct DynamicCharacterMotionConstraint : ITwoBodyConstraintDescription<DynamicCharacterMotionConstraint>
+{
+    /// <summary>
+    /// Maximum force that the horizontal motion constraint can apply to reach the current velocity goal.
+    /// </summary>
+    public float MaximumHorizontalForce;
+    /// <summary>
+    /// Maximum force that the vertical motion constraint can apply to fight separation.
+    /// </summary>
+    public float MaximumVerticalForce;
+    /// <summary>
+    /// Target horizontal velocity in terms of the basis X and -Z axes.
+    /// </summary>
+    public Vector2 TargetVelocity;
+    /// <summary>
+    /// Depth of the supporting contact. The vertical motion constraint permits separating velocity if, after a frame, the objects will still be touching.
+    /// </summary>
+	public float Depth;
+    /// <summary>
+    /// Stores the quaternion-packed orthonormal basis for the motion constraint. When expanded into a matrix, X and Z will represent the Right and Backward directions respectively. Y will represent Up.
+    /// In other words, a target tangential velocity of (4, 2) will result in a goal velocity of 4 along the (1, 0, 0) * Basis direction and a goal velocity of 2 along the (0, 0, -1) * Basis direction.
+    /// All motion moving along the (0, 1, 0) * Basis axis will be fought against by the vertical motion constraint.
+    /// </summary>
+    public Quaternion SurfaceBasis;
+    /// <summary>
+    /// World space offset from the character's center to apply impulses at.
+    /// </summary>
+    public Vector3 OffsetFromCharacterToSupportPoint;
+
+    /// <summary>
+    /// World space offset from the support's center to apply impulses at.
+    /// </summary>
+    public Vector3 OffsetFromSupportToSupportPoint;
 
-    //Note that all the solver-side data is in terms of 'Wide' data types- the solver never works on just one constraint at a time. Instead,
-    //it executes them in bundles of width equal to the runtime/hardware exposed SIMD unit width. This lets the solver scale with wider compute units.
-    //(This is important for machines that can perform 8 or more operations per instruction- there's no good way to map a single constraint instance's 
-    //computation onto such a wide instruction, so if the solver tried to do such a thing, it would leave a huge amount of performance on the table.)
 
-    //"Prestep" data can be thought of as the input to the solver. It describes everything the solver needs to know about.
+    //It's possible to create multiple descriptions for the same underlying constraint type id which can update different parts of the constraint data.
+    //This functionality isn't used very often, though- you'll notice that the engine has a 1:1 mapping (at least at the time of this writing).
+    //But in principle, it doesn't have to be that way. So, the description must provide information about the type and type id.
     /// <summary>
-    /// AOSOA formatted bundle of prestep data for multiple dynamic-supported character motion constraints.
+    /// Gets the constraint type id that this description is associated with. 
     /// </summary>
-    public struct DynamicCharacterMotionPrestep
+    public static int ConstraintTypeId => DynamicCharacterMotionTypeProcessor.BatchTypeId;
+
+    /// <summary>
+    /// Gets the TypeProcessor type that is associated with this description.
+    /// </summary>
+    public static Type TypeProcessorType => typeof(DynamicCharacterMotionTypeProcessor);    
+    /// <summary>
+    /// Creates a type processor for this constraint type.
+    /// </summary>
+    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,
+    //we refer to that slot using the same struct and then write only to the first slot.
+    //(Note that accessing slots after the first may result in access violations; the 'offset instance' is not guaranteed to refer to valid data beyond the first slot!)
+    public readonly void ApplyDescription(ref TypeBatch batch, int bundleIndex, int innerIndex)
+    {
+        ref var target = ref GetOffsetInstance(ref Buffer<DynamicCharacterMotionPrestep>.Get(ref batch.PrestepData, bundleIndex), innerIndex);
+        QuaternionWide.WriteFirst(SurfaceBasis, ref target.SurfaceBasis);
+        GetFirst(ref target.MaximumHorizontalForce) = MaximumHorizontalForce;
+        GetFirst(ref target.MaximumVerticalForce) = MaximumVerticalForce;
+        Vector2Wide.WriteFirst(TargetVelocity, ref target.TargetVelocity);
+		GetFirst(ref target.Depth) = Depth;
+        Vector3Wide.WriteFirst(OffsetFromCharacterToSupportPoint, ref target.OffsetFromCharacter);
+
+        Vector3Wide.WriteFirst(OffsetFromSupportToSupportPoint, ref target.OffsetFromSupport);
+
+    }
+
+    public static void BuildDescription(ref TypeBatch batch, int bundleIndex, int innerIndex, out DynamicCharacterMotionConstraint description)
+    {
+        ref var source = ref GetOffsetInstance(ref Buffer<DynamicCharacterMotionPrestep>.Get(ref batch.PrestepData, bundleIndex), innerIndex);
+        QuaternionWide.ReadFirst(source.SurfaceBasis, out description.SurfaceBasis);
+        description.MaximumHorizontalForce = GetFirst(ref source.MaximumHorizontalForce);
+        description.MaximumVerticalForce = GetFirst(ref source.MaximumVerticalForce);
+        Vector2Wide.ReadFirst(source.TargetVelocity, out description.TargetVelocity);
+		description.Depth = GetFirst(ref source.Depth);
+        Vector3Wide.ReadFirst(source.OffsetFromCharacter, out description.OffsetFromCharacterToSupportPoint);
+
+        Vector3Wide.ReadFirst(source.OffsetFromSupport, out description.OffsetFromSupportToSupportPoint);
+
+    }
+}
+
+//Note that all the solver-side data is in terms of 'Wide' data types- the solver never works on just one constraint at a time. Instead,
+//it executes them in bundles of width equal to the runtime/hardware exposed SIMD unit width. This lets the solver scale with wider compute units.
+//(This is important for machines that can perform 8 or more operations per instruction- there's no good way to map a single constraint instance's 
+//computation onto such a wide instruction, so if the solver tried to do such a thing, it would leave a huge amount of performance on the table.)
+
+//"Prestep" data can be thought of as the input to the solver. It describes everything the solver needs to know about.
+/// <summary>
+/// AOSOA formatted bundle of prestep data for multiple dynamic-supported character motion constraints.
+/// </summary>
+public struct DynamicCharacterMotionPrestep
+{
+    //Note that the prestep data layout is important. The solver tends to be severely memory bandwidth bound, so using a minimal representation is valuable.
+    //That's why the Basis is stored as a quaternion and not a full Matrix- the cost of the arithmetic operations to expand it back into the original matrix form is far less
+    //than the cost of loading all the extra lanes of data when scaled up to many cores.
+    public QuaternionWide SurfaceBasis;
+    public Vector<float> MaximumHorizontalForce;
+    public Vector<float> MaximumVerticalForce;
+	public Vector<float> Depth;
+    public Vector2Wide TargetVelocity;
+    public Vector3Wide OffsetFromCharacter;
+
+    public Vector3Wide OffsetFromSupport;
+
+}
+
+public struct DynamicCharacterMotionFunctions : ITwoBodyConstraintFunctions<DynamicCharacterMotionPrestep, CharacterMotionAccumulatedImpulse>
+{
+    [MethodImpl(MethodImplOptions.AggressiveInlining)]
+    static void ComputeJacobians(in Vector3Wide offsetA, in Vector3Wide offsetB, in QuaternionWide basisQuaternion,
+        out Matrix3x3Wide basis,
+        out Matrix2x3Wide horizontalAngularJacobianA, out Matrix2x3Wide horizontalAngularJacobianB,
+
+        out Vector3Wide verticalAngularJacobianA, out Vector3Wide verticalAngularJacobianB)
     {
-        //Note that the prestep data layout is important. The solver tends to be severely memory bandwidth bound, so using a minimal representation is valuable.
-        //That's why the Basis is stored as a quaternion and not a full Matrix- the cost of the arithmetic operations to expand it back into the original matrix form is far less
-        //than the cost of loading all the extra lanes of data when scaled up to many cores.
-        public QuaternionWide SurfaceBasis;
-        public Vector<float> MaximumHorizontalForce;
-        public Vector<float> MaximumVerticalForce;
-		public Vector<float> Depth;
-        public Vector2Wide TargetVelocity;
-        public Vector3Wide OffsetFromCharacter;
-        public Vector3Wide OffsetFromSupport;
+        //Both of the motion constraints are velocity motors, like tangent friction. They don't actually have a position level goal.
+        //But if we did want to make such a position level goal, it could be expressed as:
+        //dot(basis.X, constrainedPointOnA - constrainedPointOnB) = 0
+        //dot(basis.Y, constrainedPointOnA - constrainedPointOnB) <= 0 
+        //dot(basis.Z, constrainedPointOnA - constrainedPointOnB) = 0
+        //Note that the Y axis, corresponding to the vertical motion constraint, is an inequality. It pulls toward the surface, but never pushes away.
+        //It also has a separate maximum force and acts on an independent axis; that's why we solve it as a separate constraint.
+        //To get a velocity constraint out of these position goals, differentiate with respect to time:
+        //d/dt(dot(basis.X, constrainedPointOnA - constrainedPointOnB)) = dot(basis.X, d/dt(constrainedPointOnA - constrainedPointOnB))
+        //                                                              = dot(basis.X, a.LinearVelocity + a.AngularVelocity x offsetToConstrainedPointOnA - b.linearVelocity - b.AngularVelocity x offsetToConstrainedPointOnB)
+        //Throwing some algebra and identities at it:
+        //dot(basis.X, a.LinearVelocity) + dot(basis.X, a.AngularVelocity x offsetToConstrainedPointOnA) + dot(-basis.X, b.LinearVelocity) + dot(basis.X, offsetToConstrainedPointOnB x b.AngularVelocity)
+        //dot(basis.X, a.LinearVelocity) + dot(a.AngularVelocity, offsetToConstrainedPointOnA x basis.X) + dot(-basis.X, b.LinearVelocity) + dot(b.AngularVelocity, basis.X x offsetToConstrainedPointOnB)
+        //The (transpose) jacobian is the transform that pulls the body velocity into constraint space- 
+        //and here, we can see that we have an axis being dotted with each component of the velocity. That's gives us the jacobian for that degree of freedom.
+        //The same form applies to all three axes of the basis, since they're all doing the same thing (just on different directions and with different force bounds).
+        //Note that we don't explicitly output linear jacobians- they are just the axes of the basis, and the linear jacobians of B are just the negated linear jacobians of A.
+        Matrix3x3Wide.CreateFromQuaternion(basisQuaternion, out basis);
+        Vector3Wide.CrossWithoutOverlap(offsetA, basis.X, out horizontalAngularJacobianA.X);
+        Vector3Wide.CrossWithoutOverlap(offsetA, basis.Y, out verticalAngularJacobianA);
+        Vector3Wide.CrossWithoutOverlap(offsetA, basis.Z, out horizontalAngularJacobianA.Y);
+
+        Vector3Wide.CrossWithoutOverlap(basis.X, offsetB, out horizontalAngularJacobianB.X);
+        Vector3Wide.CrossWithoutOverlap(basis.Y, offsetB, out verticalAngularJacobianB);
+        Vector3Wide.CrossWithoutOverlap(basis.Z, offsetB, out horizontalAngularJacobianB.Y);
+
     }
 
-    //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
+    [MethodImpl(MethodImplOptions.AggressiveInlining)]
+    private static void ApplyHorizontalImpulse(in Matrix3x3Wide basis,
+        in Matrix2x3Wide angularJacobianA, in Matrix2x3Wide angularJacobianB, in Vector2Wide constraintSpaceImpulse,
+        in BodyInertiaWide inertiaA, in BodyInertiaWide inertiaB,
+
+        ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB)
     {
-        public QuaternionWide SurfaceBasis;
-        public Vector3Wide OffsetFromCharacter;
-        public Vector3Wide OffsetFromSupport;
-        public Vector2Wide TargetVelocity;
-        public Symmetric2x2Wide HorizontalEffectiveMass;
-        public Vector<float> MaximumHorizontalImpulse;
-        public BodyInertias InertiaA;
-        public BodyInertias InertiaB;
-        public Vector<float> VerticalBiasVelocity;
-        public Vector<float> VerticalEffectiveMass;
-        public Vector<float> MaximumVerticalForce;
+        //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);
+        Vector3Wide.Scale(basis.Z, constraintSpaceImpulse.Y, out var linearImpulseAY);
+        Vector3Wide.Add(linearImpulseAX, linearImpulseAY, out var linearImpulseA);
+        Vector3Wide.Scale(linearImpulseA, inertiaA.InverseMass, out var linearChangeA);
+        Vector3Wide.Add(velocityA.Linear, linearChangeA, out velocityA.Linear);
+
+        Vector3Wide.Scale(linearImpulseA, inertiaB.InverseMass, out var negatedLinearChangeB); //Linear jacobians for B are just A's negated linear jacobians.
+        Vector3Wide.Subtract(velocityB.Linear, negatedLinearChangeB, out velocityB.Linear);
+
+
+        Matrix2x3Wide.Transform(constraintSpaceImpulse, angularJacobianA, out var angularImpulseA);
+        Symmetric3x3Wide.TransformWithoutOverlap(angularImpulseA, inertiaA.InverseInertiaTensor, out var angularChangeA);
+        Vector3Wide.Add(velocityA.Angular, angularChangeA, out velocityA.Angular);
+
+        Matrix2x3Wide.Transform(constraintSpaceImpulse, angularJacobianB, out var angularImpulseB);
+        Symmetric3x3Wide.TransformWithoutOverlap(angularImpulseB, inertiaB.InverseInertiaTensor, out var angularChangeB);
+        Vector3Wide.Add(velocityB.Angular, angularChangeB, out velocityB.Angular);
 
     }
 
-    public struct DynamicCharacterMotionFunctions : IConstraintFunctions<DynamicCharacterMotionPrestep, DynamicCharacterMotionProjection, CharacterMotionAccumulatedImpulse>
+    [MethodImpl(MethodImplOptions.AggressiveInlining)]
+    private static void ApplyVerticalImpulse(in Matrix3x3Wide basis,
+        in Vector3Wide angularJacobianA, in Vector3Wide angularJacobianB, in Vector<float> constraintSpaceImpulse,
+        in BodyInertiaWide inertiaA, in BodyInertiaWide inertiaB,
+
+        ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB)
     {
-        [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        static void ComputeJacobians(in Vector3Wide offsetA, in Vector3Wide offsetB, in QuaternionWide basisQuaternion,
-            out Matrix3x3Wide basis,
-            out Matrix2x3Wide horizontalAngularJacobianA, out Matrix2x3Wide horizontalAngularJacobianB,
-            out Vector3Wide verticalAngularJacobianA, out Vector3Wide verticalAngularJacobianB)
-        {
-            //Both of the motion constraints are velocity motors, like tangent friction. They don't actually have a position level goal.
-            //But if we did want to make such a position level goal, it could be expressed as:
-            //dot(basis.X, constrainedPointOnA - constrainedPointOnB) = 0
-            //dot(basis.Y, constrainedPointOnA - constrainedPointOnB) <= 0 
-            //dot(basis.Z, constrainedPointOnA - constrainedPointOnB) = 0
-            //Note that the Y axis, corresponding to the vertical motion constraint, is an inequality. It pulls toward the surface, but never pushes away.
-            //It also has a separate maximum force and acts on an independent axis; that's why we solve it as a separate constraint.
-            //To get a velocity constraint out of these position goals, differentiate with respect to time:
-            //d/dt(dot(basis.X, constrainedPointOnA - constrainedPointOnB)) = dot(basis.X, d/dt(constrainedPointOnA - constrainedPointOnB))
-            //                                                              = dot(basis.X, a.LinearVelocity + a.AngularVelocity x offsetToConstrainedPointOnA - b.linearVelocity - b.AngularVelocity x offsetToConstrainedPointOnB)
-            //Throwing some algebra and identities at it:
-            //dot(basis.X, a.LinearVelocity) + dot(basis.X, a.AngularVelocity x offsetToConstrainedPointOnA) + dot(-basis.X, b.LinearVelocity) + dot(basis.X, offsetToConstrainedPointOnB x b.AngularVelocity)
-            //dot(basis.X, a.LinearVelocity) + dot(a.AngularVelocity, offsetToConstrainedPointOnA x basis.X) + dot(-basis.X, b.LinearVelocity) + dot(b.AngularVelocity, basis.X x offsetToConstrainedPointOnB)
-            //The (transpose) jacobian is the transform that pulls the body velocity into constraint space- 
-            //and here, we can see that we have an axis being dotted with each component of the velocity. That's gives us the jacobian for that degree of freedom.
-            //The same form applies to all three axes of the basis, since they're all doing the same thing (just on different directions and with different force bounds).
-            //Note that we don't explicitly output linear jacobians- they are just the axes of the basis, and the linear jacobians of B are just the negated linear jacobians of A.
-            Matrix3x3Wide.CreateFromQuaternion(basisQuaternion, out basis);
-            Vector3Wide.CrossWithoutOverlap(offsetA, basis.X, out horizontalAngularJacobianA.X);
-            Vector3Wide.CrossWithoutOverlap(offsetA, basis.Y, out verticalAngularJacobianA);
-            Vector3Wide.CrossWithoutOverlap(offsetA, basis.Z, out horizontalAngularJacobianA.Y);
-            Vector3Wide.CrossWithoutOverlap(basis.X, offsetB, out horizontalAngularJacobianB.X);
-            Vector3Wide.CrossWithoutOverlap(basis.Y, offsetB, out verticalAngularJacobianB);
-            Vector3Wide.CrossWithoutOverlap(basis.Z, offsetB, out horizontalAngularJacobianB.Y);
-        }
-
-        [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)
-        {
-            //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<float>.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<float>.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,
-            in BodyInertias inertiaA, in BodyInertias inertiaB,
-            ref BodyVelocities velocityA, ref BodyVelocities 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);
-            Vector3Wide.Scale(basis.Z, constraintSpaceImpulse.Y, out var linearImpulseAY);
-            Vector3Wide.Add(linearImpulseAX, linearImpulseAY, out var linearImpulseA);
-            Vector3Wide.Scale(linearImpulseA, inertiaA.InverseMass, out var linearChangeA);
-            Vector3Wide.Add(velocityA.Linear, linearChangeA, out velocityA.Linear);
-            Vector3Wide.Scale(linearImpulseA, inertiaB.InverseMass, out var negatedLinearChangeB); //Linear jacobians for B are just A's negated linear jacobians.
-            Vector3Wide.Subtract(velocityB.Linear, negatedLinearChangeB, out velocityB.Linear);
-
-            Matrix2x3Wide.Transform(constraintSpaceImpulse, angularJacobianA, out var angularImpulseA);
-            Symmetric3x3Wide.TransformWithoutOverlap(angularImpulseA, inertiaA.InverseInertiaTensor, out var angularChangeA);
-            Vector3Wide.Add(velocityA.Angular, angularChangeA, out velocityA.Angular);
-            Matrix2x3Wide.Transform(constraintSpaceImpulse, angularJacobianB, 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 ApplyVerticalImpulse(in Matrix3x3Wide basis,
-            in Vector3Wide angularJacobianA, in Vector3Wide angularJacobianB, in Vector<float> constraintSpaceImpulse,
-            in BodyInertias inertiaA, in BodyInertias inertiaB,
-            ref BodyVelocities velocityA, ref BodyVelocities velocityB)
-        {
-            Vector3Wide.Scale(basis.Y, constraintSpaceImpulse, out var linearImpulseA);
-            Vector3Wide.Scale(linearImpulseA, inertiaA.InverseMass, out var linearChangeA);
-            Vector3Wide.Add(velocityA.Linear, linearChangeA, out velocityA.Linear);
-            Vector3Wide.Scale(linearImpulseA, inertiaB.InverseMass, out var negatedLinearChangeB); //Linear jacobians for B are just A's negated linear jacobians.
-            Vector3Wide.Subtract(velocityB.Linear, negatedLinearChangeB, out velocityB.Linear);
-
-            Vector3Wide.Scale(angularJacobianA, constraintSpaceImpulse, out var angularImpulseA);
-            Symmetric3x3Wide.TransformWithoutOverlap(angularImpulseA, inertiaA.InverseInertiaTensor, out var angularChangeA);
-            Vector3Wide.Add(velocityA.Angular, angularChangeA, out velocityA.Angular);
-            Vector3Wide.Scale(angularJacobianB, constraintSpaceImpulse, out var angularImpulseB);
-            Symmetric3x3Wide.TransformWithoutOverlap(angularImpulseB, inertiaB.InverseInertiaTensor, out var angularChangeB);
-            Vector3Wide.Add(velocityB.Angular, angularChangeB, out velocityB.Angular);
-        }
-
-        [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public void WarmStart(ref BodyVelocities velocityA, ref BodyVelocities 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 BodyVelocities velocityA, ref BodyVelocities 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<float>.One, projection.MaximumHorizontalImpulse / Vector.Max(new Vector<float>(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<float>.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);
-        }
+        Vector3Wide.Scale(basis.Y, constraintSpaceImpulse, out var linearImpulseA);
+        Vector3Wide.Scale(linearImpulseA, inertiaA.InverseMass, out var linearChangeA);
+        Vector3Wide.Add(velocityA.Linear, linearChangeA, out velocityA.Linear);
+
+        Vector3Wide.Scale(linearImpulseA, inertiaB.InverseMass, out var negatedLinearChangeB); //Linear jacobians for B are just A's negated linear jacobians.
+        Vector3Wide.Subtract(velocityB.Linear, negatedLinearChangeB, out velocityB.Linear);
+
 
+        Vector3Wide.Scale(angularJacobianA, constraintSpaceImpulse, out var angularImpulseA);
+        Symmetric3x3Wide.TransformWithoutOverlap(angularImpulseA, inertiaA.InverseInertiaTensor, out var angularChangeA);
+        Vector3Wide.Add(velocityA.Angular, angularChangeA, out velocityA.Angular);
+
+        Vector3Wide.Scale(angularJacobianB, constraintSpaceImpulse, out var angularImpulseB);
+        Symmetric3x3Wide.TransformWithoutOverlap(angularImpulseB, inertiaB.InverseInertiaTensor, out var angularChangeB);
+        Vector3Wide.Add(velocityB.Angular, angularChangeB, out velocityB.Angular);
+
+    }
+               
+
+    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);
+        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 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.
+        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<float>(dt);
+        var maximumHorizontalImpulse = prestep.MaximumHorizontalForce * dtWide;
+        var scale = Vector.Min(Vector<float>.One, maximumHorizontalImpulse / Vector.Max(new Vector<float>(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<float>.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.
+        //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<float>.Zero, Vector.Max(accumulatedImpulses.Vertical + verticalCorrectiveImpulse, -maximumVerticalImpulse));
+        verticalCorrectiveImpulse = accumulatedImpulses.Vertical - previousVerticalAccumulatedImpulse;
+
+        ApplyVerticalImpulse(basis, verticalAngularJacobianA, verticalAngularJacobianB, verticalCorrectiveImpulse, inertiaA, inertiaB, ref velocityA, ref 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<DynamicCharacterMotionPrestep, DynamicCharacterMotionProjection, CharacterMotionAccumulatedImpulse, DynamicCharacterMotionFunctions>
+        
+    public static bool RequiresIncrementalSubstepUpdates => true;
+    public static void IncrementallyUpdateForSubstep(in Vector<float> dt, in BodyVelocityWide velocityA, in BodyVelocityWide velocityB, ref DynamicCharacterMotionPrestep prestep)
     {
-        /// <summary>
-        /// 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
-        /// registry of all types that can exist (custom ones, like this one, can always be created), but having it be constant helps simplify and optimize its internal usage.
-        /// </summary>
-        public const int BatchTypeId = 51;
+        //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;
     }
-}
\ No newline at end of file
+}
+
+//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<DynamicCharacterMotionPrestep, CharacterMotionAccumulatedImpulse, DynamicCharacterMotionFunctions, AccessAll, AccessAll, AccessAll, AccessAll>
+{
+    /// <summary>
+    /// 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
+    /// registry of all types that can exist (custom ones, like this one, can always be created), but having it be constant helps simplify and optimize its internal usage.
+    /// </summary>
+    public const int BatchTypeId = 51;
+}
+
diff --git a/Demos/Demos/Characters/CharacterMotionConstraint.tt b/Demos/Demos/Characters/CharacterMotionConstraint.tt
index c627c2259..457f6f4fb 100644
--- a/Demos/Demos/Characters/CharacterMotionConstraint.tt
+++ b/Demos/Demos/Characters/CharacterMotionConstraint.tt
@@ -13,396 +13,365 @@ using BepuPhysics.Constraints;
 using BepuUtilities.Memory;
 using static BepuUtilities.GatherScatter;
 
-namespace Demos.Demos.Characters
-{ 
-    public struct CharacterMotionAccumulatedImpulse
-    {
-        public Vector2Wide Horizontal;
-        public Vector<float> Vertical;
-    }
+namespace Demos.Demos.Characters;
+
+public struct CharacterMotionAccumulatedImpulse
+{
+    public Vector2Wide Horizontal;
+    public Vector<float> Vertical;
+}
 
-    //IMPORTANT NOTE: The static and dynamic motion constraint variants were autogenerated from a text template. To modify the constraint, modify the text template instead.
-    //Using code generators to generate these sorts of constraint variants can work around some of the limitations in compile time expressiveness-
-    //here, we avoid creating a bunch of duplicate math for the one and two body constraint cases which would have to be separately maintained.
-    //There are other cases where this is pushed even further. Within the engine, the dozens of contact constraint variants are generated by text templates.
-    //This isn't the most wonderful solution in terms of development experience or tooling, 
-    //but it maximizes performance and avoids the need for in-language turing complete metaprogramming, so I'm okay with it.
+//IMPORTANT NOTE: The static and dynamic motion constraint variants were autogenerated from a text template. To modify the constraint, modify the text template instead.
+//Using code generators to generate these sorts of constraint variants can work around some of the limitations in compile time expressiveness-
+//here, we avoid creating a bunch of duplicate math for the one and two body constraint cases which would have to be separately maintained.
+//There are other cases where this is pushed even further. Within the engine, the dozens of contact constraint variants are generated by text templates.
+//This isn't the most wonderful solution in terms of development experience or tooling, 
+//but it maximizes performance and avoids the need for in-language turing complete metaprogramming, so I'm okay with it.
 
 <#for (int i = 0; i < 2; ++i) 
 {
-    var dynamic = i == 1;
-    var prefix = dynamic ? "Dynamic" : "Static";
+var dynamic = i == 1;
+var prefix = dynamic ? "Dynamic" : "Static";
 #>
-    //Constraint descriptions provide an explicit mapping from the array-of-structures format to the internal array-of-structures-of-arrays format used by the solver.
-    //Note that there is a separate description for the one and two body case- constraint implementations take advantage of the lack of a second body to reduce data gathering requirements.
+//Constraint descriptions provide an explicit mapping from the array-of-structures format to the internal array-of-structures-of-arrays format used by the solver.
+//Note that there is a separate description for the one and two body case- constraint implementations take advantage of the lack of a second body to reduce data gathering requirements.
+/// <summary>
+/// Description of a character motion constraint where the support is <#=prefix.ToLower()#>.
+/// </summary>
+public struct <#=prefix#>CharacterMotionConstraint : I<#=dynamic ? "TwoBody" : "OneBody"#>ConstraintDescription<<#=prefix#>CharacterMotionConstraint>
+{
     /// <summary>
-    /// Description of a character motion constraint where the support is <#=prefix.ToLower()#>.
+    /// Maximum force that the horizontal motion constraint can apply to reach the current velocity goal.
     /// </summary>
-    public struct <#=prefix#>CharacterMotionConstraint : I<#=dynamic ? "TwoBody" : "OneBody"#>ConstraintDescription<<#=prefix#>CharacterMotionConstraint>
-    {
-        /// <summary>
-        /// Maximum force that the horizontal motion constraint can apply to reach the current velocity goal.
-        /// </summary>
-        public float MaximumHorizontalForce;
-        /// <summary>
-        /// Maximum force that the vertical motion constraint can apply to fight separation.
-        /// </summary>
-        public float MaximumVerticalForce;
-        /// <summary>
-        /// Target horizontal velocity in terms of the basis X and -Z axes.
-        /// </summary>
-        public Vector2 TargetVelocity;
-        /// <summary>
-        /// Depth of the supporting contact. The vertical motion constraint permits separating velocity if, after a frame, the objects will still be touching.
-        /// </summary>
-		public float Depth;
-        /// <summary>
-        /// Stores the quaternion-packed orthonormal basis for the motion constraint. When expanded into a matrix, X and Z will represent the Right and Backward directions respectively. Y will represent Up.
-        /// In other words, a target tangential velocity of (4, 2) will result in a goal velocity of 4 along the (1, 0, 0) * Basis direction and a goal velocity of 2 along the (0, 0, -1) * Basis direction.
-        /// All motion moving along the (0, 1, 0) * Basis axis will be fought against by the vertical motion constraint.
-        /// </summary>
-        public Quaternion SurfaceBasis;
-        /// <summary>
-        /// World space offset from the character's center to apply impulses at.
-        /// </summary>
-        public Vector3 OffsetFromCharacterToSupportPoint;
-<#if(dynamic) {#>
-        /// <summary>
-        /// World space offset from the support's center to apply impulses at.
-        /// </summary>
-        public Vector3 OffsetFromSupportToSupportPoint;
-<#}#>
-
-        //It's possible to create multiple descriptions for the same underlying constraint type id which can update different parts of the constraint data.
-        //This functionality isn't used very often, though- you'll notice that the engine has a 1:1 mapping (at least at the time of this writing).
-        //But in principle, it doesn't have to be that way. So, the description must provide information about the type and type id.
-        /// <summary>
-        /// Gets the constraint type id that this description is associated with. 
-        /// </summary>
-        public readonly int ConstraintTypeId => <#=prefix#>CharacterMotionTypeProcessor.BatchTypeId;
-
-        /// <summary>
-        /// Gets the TypeProcessor type that is associated with this description.
-        /// </summary>
-        public readonly Type TypeProcessorType => typeof(<#=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,
-        //we refer to that slot using the same struct and then write only to the first slot.
-        //(Note that accessing slots after the first may result in access violations; the 'offset instance' is not guaranteed to refer to valid data beyond the first slot!)
-        public readonly void ApplyDescription(ref TypeBatch batch, int bundleIndex, int innerIndex)
-        {
-            ref var target = ref GetOffsetInstance(ref Buffer<<#=prefix#>CharacterMotionPrestep>.Get(ref batch.PrestepData, bundleIndex), innerIndex);
-            QuaternionWide.WriteFirst(SurfaceBasis, ref target.SurfaceBasis);
-            GetFirst(ref target.MaximumHorizontalForce) = MaximumHorizontalForce;
-            GetFirst(ref target.MaximumVerticalForce) = MaximumVerticalForce;
-            Vector2Wide.WriteFirst(TargetVelocity, ref target.TargetVelocity);
-			GetFirst(ref target.Depth) = Depth;
-            Vector3Wide.WriteFirst(OffsetFromCharacterToSupportPoint, ref target.OffsetFromCharacter);
-<#if(dynamic) {#>
-            Vector3Wide.WriteFirst(OffsetFromSupportToSupportPoint, ref target.OffsetFromSupport);
-<#}#>
-        }
-
-        public readonly 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);
-            description.MaximumHorizontalForce = GetFirst(ref source.MaximumHorizontalForce);
-            description.MaximumVerticalForce = GetFirst(ref source.MaximumVerticalForce);
-            Vector2Wide.ReadFirst(source.TargetVelocity, out description.TargetVelocity);
-			description.Depth = GetFirst(ref source.Depth);
-            Vector3Wide.ReadFirst(source.OffsetFromCharacter, out description.OffsetFromCharacterToSupportPoint);
+    public float MaximumHorizontalForce;
+    /// <summary>
+    /// Maximum force that the vertical motion constraint can apply to fight separation.
+    /// </summary>
+    public float MaximumVerticalForce;
+    /// <summary>
+    /// Target horizontal velocity in terms of the basis X and -Z axes.
+    /// </summary>
+    public Vector2 TargetVelocity;
+    /// <summary>
+    /// Depth of the supporting contact. The vertical motion constraint permits separating velocity if, after a frame, the objects will still be touching.
+    /// </summary>
+	public float Depth;
+    /// <summary>
+    /// Stores the quaternion-packed orthonormal basis for the motion constraint. When expanded into a matrix, X and Z will represent the Right and Backward directions respectively. Y will represent Up.
+    /// In other words, a target tangential velocity of (4, 2) will result in a goal velocity of 4 along the (1, 0, 0) * Basis direction and a goal velocity of 2 along the (0, 0, -1) * Basis direction.
+    /// All motion moving along the (0, 1, 0) * Basis axis will be fought against by the vertical motion constraint.
+    /// </summary>
+    public Quaternion SurfaceBasis;
+    /// <summary>
+    /// World space offset from the character's center to apply impulses at.
+    /// </summary>
+    public Vector3 OffsetFromCharacterToSupportPoint;
 <#if(dynamic) {#>
-            Vector3Wide.ReadFirst(source.OffsetFromSupport, out description.OffsetFromSupportToSupportPoint);
+    /// <summary>
+    /// World space offset from the support's center to apply impulses at.
+    /// </summary>
+    public Vector3 OffsetFromSupportToSupportPoint;
 <#}#>
-        }
-    }
 
-    //Note that all the solver-side data is in terms of 'Wide' data types- the solver never works on just one constraint at a time. Instead,
-    //it executes them in bundles of width equal to the runtime/hardware exposed SIMD unit width. This lets the solver scale with wider compute units.
-    //(This is important for machines that can perform 8 or more operations per instruction- there's no good way to map a single constraint instance's 
-    //computation onto such a wide instruction, so if the solver tried to do such a thing, it would leave a huge amount of performance on the table.)
+    //It's possible to create multiple descriptions for the same underlying constraint type id which can update different parts of the constraint data.
+    //This functionality isn't used very often, though- you'll notice that the engine has a 1:1 mapping (at least at the time of this writing).
+    //But in principle, it doesn't have to be that way. So, the description must provide information about the type and type id.
+    /// <summary>
+    /// Gets the constraint type id that this description is associated with. 
+    /// </summary>
+    public static int ConstraintTypeId => <#=prefix#>CharacterMotionTypeProcessor.BatchTypeId;
 
-    //"Prestep" data can be thought of as the input to the solver. It describes everything the solver needs to know about.
     /// <summary>
-    /// AOSOA formatted bundle of prestep data for multiple <#=prefix.ToLower()#>-supported character motion constraints.
+    /// Gets the TypeProcessor type that is associated with this description.
+    /// </summary>
+    public static Type TypeProcessorType => typeof(<#=prefix#>CharacterMotionTypeProcessor);    
+    /// <summary>
+    /// Creates a type processor for this constraint type.
     /// </summary>
-    public struct <#=prefix#>CharacterMotionPrestep
+    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,
+    //we refer to that slot using the same struct and then write only to the first slot.
+    //(Note that accessing slots after the first may result in access violations; the 'offset instance' is not guaranteed to refer to valid data beyond the first slot!)
+    public readonly void ApplyDescription(ref TypeBatch batch, int bundleIndex, int innerIndex)
     {
-        //Note that the prestep data layout is important. The solver tends to be severely memory bandwidth bound, so using a minimal representation is valuable.
-        //That's why the Basis is stored as a quaternion and not a full Matrix- the cost of the arithmetic operations to expand it back into the original matrix form is far less
-        //than the cost of loading all the extra lanes of data when scaled up to many cores.
-        public QuaternionWide SurfaceBasis;
-        public Vector<float> MaximumHorizontalForce;
-        public Vector<float> MaximumVerticalForce;
-		public Vector<float> Depth;
-        public Vector2Wide TargetVelocity;
-        public Vector3Wide OffsetFromCharacter;
+        ref var target = ref GetOffsetInstance(ref Buffer<<#=prefix#>CharacterMotionPrestep>.Get(ref batch.PrestepData, bundleIndex), innerIndex);
+        QuaternionWide.WriteFirst(SurfaceBasis, ref target.SurfaceBasis);
+        GetFirst(ref target.MaximumHorizontalForce) = MaximumHorizontalForce;
+        GetFirst(ref target.MaximumVerticalForce) = MaximumVerticalForce;
+        Vector2Wide.WriteFirst(TargetVelocity, ref target.TargetVelocity);
+		GetFirst(ref target.Depth) = Depth;
+        Vector3Wide.WriteFirst(OffsetFromCharacterToSupportPoint, ref target.OffsetFromCharacter);
 <#if(dynamic) {#>
-        public Vector3Wide OffsetFromSupport;
+        Vector3Wide.WriteFirst(OffsetFromSupportToSupportPoint, ref target.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 <#=prefix#>CharacterMotionProjection
+    public static void BuildDescription(ref TypeBatch batch, int bundleIndex, int innerIndex, out <#=prefix#>CharacterMotionConstraint description)
     {
-        public QuaternionWide SurfaceBasis;
-        public Vector3Wide OffsetFromCharacter;
+        ref var source = ref GetOffsetInstance(ref Buffer<<#=prefix#>CharacterMotionPrestep>.Get(ref batch.PrestepData, bundleIndex), innerIndex);
+        QuaternionWide.ReadFirst(source.SurfaceBasis, out description.SurfaceBasis);
+        description.MaximumHorizontalForce = GetFirst(ref source.MaximumHorizontalForce);
+        description.MaximumVerticalForce = GetFirst(ref source.MaximumVerticalForce);
+        Vector2Wide.ReadFirst(source.TargetVelocity, out description.TargetVelocity);
+		description.Depth = GetFirst(ref source.Depth);
+        Vector3Wide.ReadFirst(source.OffsetFromCharacter, out description.OffsetFromCharacterToSupportPoint);
 <#if(dynamic) {#>
-        public Vector3Wide OffsetFromSupport;
+        Vector3Wide.ReadFirst(source.OffsetFromSupport, out description.OffsetFromSupportToSupportPoint);
 <#}#>
-        public Vector2Wide TargetVelocity;
-        public Symmetric2x2Wide HorizontalEffectiveMass;
-        public Vector<float> MaximumHorizontalImpulse;
-        public BodyInertias InertiaA;
+    }
+}
+
+//Note that all the solver-side data is in terms of 'Wide' data types- the solver never works on just one constraint at a time. Instead,
+//it executes them in bundles of width equal to the runtime/hardware exposed SIMD unit width. This lets the solver scale with wider compute units.
+//(This is important for machines that can perform 8 or more operations per instruction- there's no good way to map a single constraint instance's 
+//computation onto such a wide instruction, so if the solver tried to do such a thing, it would leave a huge amount of performance on the table.)
+
+//"Prestep" data can be thought of as the input to the solver. It describes everything the solver needs to know about.
+/// <summary>
+/// AOSOA formatted bundle of prestep data for multiple <#=prefix.ToLower()#>-supported character motion constraints.
+/// </summary>
+public struct <#=prefix#>CharacterMotionPrestep
+{
+    //Note that the prestep data layout is important. The solver tends to be severely memory bandwidth bound, so using a minimal representation is valuable.
+    //That's why the Basis is stored as a quaternion and not a full Matrix- the cost of the arithmetic operations to expand it back into the original matrix form is far less
+    //than the cost of loading all the extra lanes of data when scaled up to many cores.
+    public QuaternionWide SurfaceBasis;
+    public Vector<float> MaximumHorizontalForce;
+    public Vector<float> MaximumVerticalForce;
+	public Vector<float> Depth;
+    public Vector2Wide TargetVelocity;
+    public Vector3Wide OffsetFromCharacter;
 <#if(dynamic) {#>
-        public BodyInertias InertiaB;
+    public Vector3Wide OffsetFromSupport;
 <#}#>
-        public Vector<float> VerticalBiasVelocity;
-        public Vector<float> VerticalEffectiveMass;
-        public Vector<float> MaximumVerticalForce;
+}
 
-    }
+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,
+        out Matrix3x3Wide basis,
+        out Matrix2x3Wide horizontalAngularJacobianA, <#if (dynamic) {#>out Matrix2x3Wide horizontalAngularJacobianB,<#}#>
 
-    public struct <#=prefix#>CharacterMotionFunctions : I<#if(!dynamic) { Write("OneBody"); }#>ConstraintFunctions<<#=prefix#>CharacterMotionPrestep, <#=prefix#>CharacterMotionProjection, CharacterMotionAccumulatedImpulse>
+        out Vector3Wide verticalAngularJacobianA<#if (dynamic) {#>, out Vector3Wide verticalAngularJacobianB<#}#>)
     {
-        [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        static void ComputeJacobians(in Vector3Wide offsetA, <#if (dynamic) {#>in Vector3Wide offsetB, <#}#>in QuaternionWide basisQuaternion,
-            out Matrix3x3Wide basis,
-            out Matrix2x3Wide horizontalAngularJacobianA, <#if (dynamic) {#>out Matrix2x3Wide horizontalAngularJacobianB,<#}#>
-
-            out Vector3Wide verticalAngularJacobianA<#if (dynamic) {#>, out Vector3Wide verticalAngularJacobianB<#}#>)
-        {
-            //Both of the motion constraints are velocity motors, like tangent friction. They don't actually have a position level goal.
-            //But if we did want to make such a position level goal, it could be expressed as:
-            //dot(basis.X, constrainedPointOnA - constrainedPointOnB) = 0
-            //dot(basis.Y, constrainedPointOnA - constrainedPointOnB) <= 0 
-            //dot(basis.Z, constrainedPointOnA - constrainedPointOnB) = 0
-            //Note that the Y axis, corresponding to the vertical motion constraint, is an inequality. It pulls toward the surface, but never pushes away.
-            //It also has a separate maximum force and acts on an independent axis; that's why we solve it as a separate constraint.
-            //To get a velocity constraint out of these position goals, differentiate with respect to time:
-            //d/dt(dot(basis.X, constrainedPointOnA - constrainedPointOnB)) = dot(basis.X, d/dt(constrainedPointOnA - constrainedPointOnB))
-            //                                                              = dot(basis.X, a.LinearVelocity + a.AngularVelocity x offsetToConstrainedPointOnA - b.linearVelocity - b.AngularVelocity x offsetToConstrainedPointOnB)
-            //Throwing some algebra and identities at it:
-            //dot(basis.X, a.LinearVelocity) + dot(basis.X, a.AngularVelocity x offsetToConstrainedPointOnA) + dot(-basis.X, b.LinearVelocity) + dot(basis.X, offsetToConstrainedPointOnB x b.AngularVelocity)
-            //dot(basis.X, a.LinearVelocity) + dot(a.AngularVelocity, offsetToConstrainedPointOnA x basis.X) + dot(-basis.X, b.LinearVelocity) + dot(b.AngularVelocity, basis.X x offsetToConstrainedPointOnB)
-            //The (transpose) jacobian is the transform that pulls the body velocity into constraint space- 
-            //and here, we can see that we have an axis being dotted with each component of the velocity. That's gives us the jacobian for that degree of freedom.
-            //The same form applies to all three axes of the basis, since they're all doing the same thing (just on different directions and with different force bounds).
-            //Note that we don't explicitly output linear jacobians- they are just the axes of the basis, and the linear jacobians of B are just the negated linear jacobians of A.
-            Matrix3x3Wide.CreateFromQuaternion(basisQuaternion, out basis);
-            Vector3Wide.CrossWithoutOverlap(offsetA, basis.X, out horizontalAngularJacobianA.X);
-            Vector3Wide.CrossWithoutOverlap(offsetA, basis.Y, out verticalAngularJacobianA);
-            Vector3Wide.CrossWithoutOverlap(offsetA, basis.Z, out horizontalAngularJacobianA.Y);
+        //Both of the motion constraints are velocity motors, like tangent friction. They don't actually have a position level goal.
+        //But if we did want to make such a position level goal, it could be expressed as:
+        //dot(basis.X, constrainedPointOnA - constrainedPointOnB) = 0
+        //dot(basis.Y, constrainedPointOnA - constrainedPointOnB) <= 0 
+        //dot(basis.Z, constrainedPointOnA - constrainedPointOnB) = 0
+        //Note that the Y axis, corresponding to the vertical motion constraint, is an inequality. It pulls toward the surface, but never pushes away.
+        //It also has a separate maximum force and acts on an independent axis; that's why we solve it as a separate constraint.
+        //To get a velocity constraint out of these position goals, differentiate with respect to time:
+        //d/dt(dot(basis.X, constrainedPointOnA - constrainedPointOnB)) = dot(basis.X, d/dt(constrainedPointOnA - constrainedPointOnB))
+        //                                                              = dot(basis.X, a.LinearVelocity + a.AngularVelocity x offsetToConstrainedPointOnA - b.linearVelocity - b.AngularVelocity x offsetToConstrainedPointOnB)
+        //Throwing some algebra and identities at it:
+        //dot(basis.X, a.LinearVelocity) + dot(basis.X, a.AngularVelocity x offsetToConstrainedPointOnA) + dot(-basis.X, b.LinearVelocity) + dot(basis.X, offsetToConstrainedPointOnB x b.AngularVelocity)
+        //dot(basis.X, a.LinearVelocity) + dot(a.AngularVelocity, offsetToConstrainedPointOnA x basis.X) + dot(-basis.X, b.LinearVelocity) + dot(b.AngularVelocity, basis.X x offsetToConstrainedPointOnB)
+        //The (transpose) jacobian is the transform that pulls the body velocity into constraint space- 
+        //and here, we can see that we have an axis being dotted with each component of the velocity. That's gives us the jacobian for that degree of freedom.
+        //The same form applies to all three axes of the basis, since they're all doing the same thing (just on different directions and with different force bounds).
+        //Note that we don't explicitly output linear jacobians- they are just the axes of the basis, and the linear jacobians of B are just the negated linear jacobians of A.
+        Matrix3x3Wide.CreateFromQuaternion(basisQuaternion, out basis);
+        Vector3Wide.CrossWithoutOverlap(offsetA, basis.X, out horizontalAngularJacobianA.X);
+        Vector3Wide.CrossWithoutOverlap(offsetA, basis.Y, out verticalAngularJacobianA);
+        Vector3Wide.CrossWithoutOverlap(offsetA, basis.Z, out horizontalAngularJacobianA.Y);
 <#if(dynamic) {#>
-            Vector3Wide.CrossWithoutOverlap(basis.X, offsetB, out horizontalAngularJacobianB.X);
-            Vector3Wide.CrossWithoutOverlap(basis.Y, offsetB, out verticalAngularJacobianB);
-            Vector3Wide.CrossWithoutOverlap(basis.Z, offsetB, out horizontalAngularJacobianB.Y);
+        Vector3Wide.CrossWithoutOverlap(basis.X, offsetB, out horizontalAngularJacobianB.X);
+        Vector3Wide.CrossWithoutOverlap(basis.Y, offsetB, out verticalAngularJacobianB);
+        Vector3Wide.CrossWithoutOverlap(basis.Z, offsetB, out horizontalAngularJacobianB.Y);
 <#}#>
-        }
-
-        [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public void Prestep(Bodies bodies, ref <#=dynamic ? "TwoBodyReferences" : "Vector<int>"#> bodyReferences, int count, float dt, float inverseDt, ref BodyInertias inertiaA, <#if (dynamic) {#>ref BodyInertias inertiaB, <#}#>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"#>);
+    }
+
+    [MethodImpl(MethodImplOptions.AggressiveInlining)]
+    private static void ApplyHorizontalImpulse(in Matrix3x3Wide basis,
+        in Matrix2x3Wide angularJacobianA, <#if (dynamic) {#>in Matrix2x3Wide angularJacobianB, <#}#>in Vector2Wide constraintSpaceImpulse,
+        in BodyInertiaWide inertiaA, <#if (dynamic) {#>in BodyInertiaWide inertiaB,<#}#>
+
+        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);
+        Vector3Wide.Scale(basis.Z, constraintSpaceImpulse.Y, out var linearImpulseAY);
+        Vector3Wide.Add(linearImpulseAX, linearImpulseAY, out var linearImpulseA);
+        Vector3Wide.Scale(linearImpulseA, inertiaA.InverseMass, out var linearChangeA);
+        Vector3Wide.Add(velocityA.Linear, linearChangeA, out velocityA.Linear);
 <#if (dynamic) {#>
-            Symmetric3x3Wide.MatrixSandwich(horizontalAngularJacobianB, inertiaB.InverseInertiaTensor, out var horizontalAngularContributionB);
-            Symmetric2x2Wide.Add(horizontalAngularContributionA, horizontalAngularContributionB, out var inverseHorizontalEffectiveMass);
+        Vector3Wide.Scale(linearImpulseA, inertiaB.InverseMass, out var negatedLinearChangeB); //Linear jacobians for B are just A's negated linear jacobians.
+        Vector3Wide.Subtract(velocityB.Linear, negatedLinearChangeB, out velocityB.Linear);
 <#}#>
-            //The linear jacobians are unit length vectors, so J * M^-1 * JT is just M^-1.
+
+        Matrix2x3Wide.Transform(constraintSpaceImpulse, angularJacobianA, out var angularImpulseA);
+        Symmetric3x3Wide.TransformWithoutOverlap(angularImpulseA, inertiaA.InverseInertiaTensor, out var angularChangeA);
+        Vector3Wide.Add(velocityA.Angular, angularChangeA, out velocityA.Angular);
 <#if (dynamic) {#>
-            var linearContribution = inertiaA.InverseMass + inertiaB.InverseMass;
+        Matrix2x3Wide.Transform(constraintSpaceImpulse, angularJacobianB, out var angularImpulseB);
+        Symmetric3x3Wide.TransformWithoutOverlap(angularImpulseB, inertiaB.InverseInertiaTensor, out var angularChangeB);
+        Vector3Wide.Add(velocityB.Angular, angularChangeB, out velocityB.Angular);
 <#}#>
-            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);
+    }
+
+    [MethodImpl(MethodImplOptions.AggressiveInlining)]
+    private static void ApplyVerticalImpulse(in Matrix3x3Wide basis,
+        in Vector3Wide angularJacobianA, <#if (dynamic) {#>in Vector3Wide angularJacobianB, <#}#>in Vector<float> constraintSpaceImpulse,
+        in BodyInertiaWide inertiaA, <#if (dynamic) {#>in BodyInertiaWide inertiaB,<#}#>
+
+        ref BodyVelocityWide velocityA<#if (dynamic) {#>, ref BodyVelocityWide velocityB<#}#>)
+    {
+        Vector3Wide.Scale(basis.Y, constraintSpaceImpulse, out var linearImpulseA);
+        Vector3Wide.Scale(linearImpulseA, inertiaA.InverseMass, out var linearChangeA);
+        Vector3Wide.Add(velocityA.Linear, linearChangeA, out velocityA.Linear);
 <#if (dynamic) {#>
-            Symmetric3x3Wide.VectorSandwich(verticalAngularJacobianB, inertiaB.InverseInertiaTensor, out var verticalAngularContributionB);
+        Vector3Wide.Scale(linearImpulseA, inertiaB.InverseMass, out var negatedLinearChangeB); //Linear jacobians for B are just A's negated linear jacobians.
+        Vector3Wide.Subtract(velocityB.Linear, negatedLinearChangeB, out velocityB.Linear);
 <#}#>
-            var inverseVerticalEffectiveMass = verticalAngularContributionA + <#=dynamic ? "verticalAngularContributionB + linearContribution" : "inertiaA.InverseMass"#>;
-            projection.VerticalEffectiveMass = Vector<float>.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;
+
+        Vector3Wide.Scale(angularJacobianA, constraintSpaceImpulse, out var angularImpulseA);
+        Symmetric3x3Wide.TransformWithoutOverlap(angularImpulseA, inertiaA.InverseInertiaTensor, out var angularChangeA);
+        Vector3Wide.Add(velocityA.Angular, angularChangeA, out velocityA.Angular);
 <#if (dynamic) {#>
-            projection.InertiaB = inertiaB;
+        Vector3Wide.Scale(angularJacobianB, constraintSpaceImpulse, out var angularImpulseB);
+        Symmetric3x3Wide.TransformWithoutOverlap(angularImpulseB, inertiaB.InverseInertiaTensor, out var angularChangeB);
+        Vector3Wide.Add(velocityB.Angular, angularChangeB, out velocityB.Angular);
 <#}#>
-            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<float>.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,
-            in BodyInertias inertiaA, <#if (dynamic) {#>in BodyInertias inertiaB,<#}#>
-
-            ref BodyVelocities velocityA<#if (dynamic) {#>, ref BodyVelocities 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);
-            Vector3Wide.Scale(basis.Z, constraintSpaceImpulse.Y, out var linearImpulseAY);
-            Vector3Wide.Add(linearImpulseAX, linearImpulseAY, out var linearImpulseA);
-            Vector3Wide.Scale(linearImpulseA, inertiaA.InverseMass, out var linearChangeA);
-            Vector3Wide.Add(velocityA.Linear, linearChangeA, out velocityA.Linear);
+    }
+               
+
+    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<#}#>);
+        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 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.
+        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) {#>
-            Vector3Wide.Scale(linearImpulseA, inertiaB.InverseMass, out var negatedLinearChangeB); //Linear jacobians for B are just A's negated linear jacobians.
-            Vector3Wide.Subtract(velocityB.Linear, negatedLinearChangeB, out velocityB.Linear);
+        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);
 <#}#>
 
-            Matrix2x3Wide.Transform(constraintSpaceImpulse, angularJacobianA, out var angularImpulseA);
-            Symmetric3x3Wide.TransformWithoutOverlap(angularImpulseA, inertiaA.InverseInertiaTensor, out var angularChangeA);
-            Vector3Wide.Add(velocityA.Angular, angularChangeA, out velocityA.Angular);
+        //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) {#>
-            Matrix2x3Wide.Transform(constraintSpaceImpulse, angularJacobianB, out var angularImpulseB);
-            Symmetric3x3Wide.TransformWithoutOverlap(angularImpulseB, inertiaB.InverseInertiaTensor, out var angularChangeB);
-            Vector3Wide.Add(velocityB.Angular, angularChangeB, out velocityB.Angular);
+        Symmetric3x3Wide.MatrixSandwich(horizontalAngularJacobianB, inertiaB.InverseInertiaTensor, out var horizontalAngularContributionB);
+        Symmetric2x2Wide.Add(horizontalAngularContributionA, horizontalAngularContributionB, out var inverseHorizontalEffectiveMass);
 <#}#>
-        }
-
-        [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        private static void ApplyVerticalImpulse(in Matrix3x3Wide basis,
-            in Vector3Wide angularJacobianA, <#if (dynamic) {#>in Vector3Wide angularJacobianB, <#}#>in Vector<float> constraintSpaceImpulse,
-            in BodyInertias inertiaA, <#if (dynamic) {#>in BodyInertias inertiaB,<#}#>
-
-            ref BodyVelocities velocityA<#if (dynamic) {#>, ref BodyVelocities velocityB<#}#>)
-        {
-            Vector3Wide.Scale(basis.Y, constraintSpaceImpulse, out var linearImpulseA);
-            Vector3Wide.Scale(linearImpulseA, inertiaA.InverseMass, out var linearChangeA);
-            Vector3Wide.Add(velocityA.Linear, linearChangeA, out velocityA.Linear);
+        //The linear jacobians are unit length vectors, so J * M^-1 * JT is just M^-1.
 <#if (dynamic) {#>
-            Vector3Wide.Scale(linearImpulseA, inertiaB.InverseMass, out var negatedLinearChangeB); //Linear jacobians for B are just A's negated linear jacobians.
-            Vector3Wide.Subtract(velocityB.Linear, negatedLinearChangeB, out velocityB.Linear);
+        var linearContribution = inertiaA.InverseMass + inertiaB.InverseMass;
 <#}#>
-
-            Vector3Wide.Scale(angularJacobianA, constraintSpaceImpulse, out var angularImpulseA);
-            Symmetric3x3Wide.TransformWithoutOverlap(angularImpulseA, inertiaA.InverseInertiaTensor, out var angularChangeA);
-            Vector3Wide.Add(velocityA.Angular, angularChangeA, out velocityA.Angular);
+        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<float>(dt);
+        var maximumHorizontalImpulse = prestep.MaximumHorizontalForce * dtWide;
+        var scale = Vector.Min(Vector<float>.One, maximumHorizontalImpulse / Vector.Max(new Vector<float>(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.Scale(angularJacobianB, constraintSpaceImpulse, out var angularImpulseB);
-            Symmetric3x3Wide.TransformWithoutOverlap(angularImpulseB, inertiaB.InverseInertiaTensor, out var angularChangeB);
-            Vector3Wide.Add(velocityB.Angular, angularChangeB, out velocityB.Angular);
+        Vector3Wide.Dot(basis.Y, velocityB.Linear, out var negatedVerticalLinearB);
+        Vector3Wide.Dot(velocityB.Angular, verticalAngularJacobianB, out var verticalAngularB);
 <#}#>
-        }
-
-        [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public void WarmStart(ref BodyVelocities velocityA, <#if (dynamic) {#>ref BodyVelocities 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 BodyVelocities velocityA, <#if (dynamic) {#>ref BodyVelocities 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 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<float>.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.
+        //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) {#>
-            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);
+        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<float>.Zero, Vector.Max(accumulatedImpulses.Vertical + verticalCorrectiveImpulse, -maximumVerticalImpulse));
+        verticalCorrectiveImpulse = accumulatedImpulses.Vertical - previousVerticalAccumulatedImpulse;
 
-            Vector2Wide.Subtract(projection.TargetVelocity, horizontalVelocity, out var horizontalConstraintSpaceVelocityChange);
-            Symmetric2x2Wide.TransformWithoutOverlap(horizontalConstraintSpaceVelocityChange, projection.HorizontalEffectiveMass, out var horizontalCorrectiveImpulse);
+        ApplyVerticalImpulse(basis, verticalAngularJacobianA, <#if (dynamic) {#>verticalAngularJacobianB, <#}#>verticalCorrectiveImpulse, inertiaA, <#if (dynamic) {#>inertiaB, <#}#>ref velocityA<#if (dynamic) {#>, ref velocityB<#}#>);
+    }
 
-            //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<float>.One, projection.MaximumHorizontalImpulse / Vector.Max(new Vector<float>(1e-16f), horizontalImpulseMagnitude));
-            Vector2Wide.Scale(accumulatedImpulse.Horizontal, scale, out accumulatedImpulse.Horizontal);
-            Vector2Wide.Subtract(accumulatedImpulse.Horizontal, previousHorizontalAccumulatedImpulse, out horizontalCorrectiveImpulse);
+        
+    public static bool RequiresIncrementalSubstepUpdates => true;
+    public static void IncrementallyUpdateForSubstep(in Vector<float> 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.
 
-            ApplyHorizontalImpulse(basis, horizontalAngularJacobianA, <#if (dynamic) {#>horizontalAngularJacobianB, <#}#>horizontalCorrectiveImpulse, projection.InertiaA, <#if (dynamic) {#>projection.InertiaB, <#}#>ref velocityA<#if (dynamic) {#>, ref velocityB<#}#>);
+        //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);
 
-            //Same thing for the vertical constraint.
-            Vector3Wide.Dot(basis.Y, velocityA.Linear, out var verticalLinearA);
-            Vector3Wide.Dot(velocityA.Angular, verticalAngularJacobianA, out var verticalAngularA);
+        var normal = QuaternionWide.TransformUnitY(prestep.SurfaceBasis);
 <#if (dynamic) {#>
-            Vector3Wide.Dot(basis.Y, velocityB.Linear, out var negatedVerticalLinearB);
-            Vector3Wide.Dot(velocityB.Angular, verticalAngularJacobianB, out var verticalAngularB);
+        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);
 <#}#>
-            //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<float>.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<#}#>);
-        }
-
+        prestep.Depth -= estimatedDepthChangeVelocity * dt;
     }
+}
 
-    //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>
-    {
-        /// <summary>
-        /// 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
-        /// registry of all types that can exist (custom ones, like this one, can always be created), but having it be constant helps simplify and optimize its internal usage.
-        /// </summary>
-        public const int BatchTypeId = <#=dynamic ? "51" : "50"#>;
-    }
+//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, CharacterMotionAccumulatedImpulse, <#=prefix#>CharacterMotionFunctions, AccessAll, AccessAll<#=dynamic ? (", AccessAll, AccessAll") : ""#>>
+{
+    /// <summary>
+    /// 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
+    /// registry of all types that can exist (custom ones, like this one, can always be created), but having it be constant helps simplify and optimize its internal usage.
+    /// </summary>
+    public const int BatchTypeId = <#=dynamic ? "51" : "50"#>;
+}
 <#}#>
-}
\ No newline at end of file
diff --git a/Demos/Demos/Characters/CharacterNarrowphaseCallbacks.cs b/Demos/Demos/Characters/CharacterNarrowphaseCallbacks.cs
index 792c94609..bbe0438ec 100644
--- a/Demos/Demos/Characters/CharacterNarrowphaseCallbacks.cs
+++ b/Demos/Demos/Characters/CharacterNarrowphaseCallbacks.cs
@@ -4,55 +4,54 @@
 using System.Runtime.CompilerServices;
 using BepuPhysics.Constraints;
 
-namespace Demos.Demos.Characters
+namespace Demos.Demos.Characters;
+
+/// <summary>
+/// Implements simple callbacks to inform the CharacterControllers system of created contacts.
+/// </summary>
+struct CharacterNarrowphaseCallbacks : INarrowPhaseCallbacks
 {
-    /// <summary>
-    /// Implements simple callbacks to inform the CharacterControllers system of created contacts.
-    /// </summary>
-    struct CharacterNarrowphaseCallbacks : INarrowPhaseCallbacks
+    public CharacterControllers Characters;
+
+    public CharacterNarrowphaseCallbacks(CharacterControllers characters)
+    {
+        Characters = characters;
+    }
+
+    [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 bool ConfigureContactManifold<TManifold>(int workerIndex, CollidablePair pair, ref TManifold manifold, out PairMaterialProperties pairMaterial) where TManifold : unmanaged, IContactManifold<TManifold>
+    {
+        pairMaterial = new PairMaterialProperties { FrictionCoefficient = 1, MaximumRecoveryVelocity = 2, SpringSettings = new SpringSettings(30, 1) };
+        Characters.TryReportContacts(pair, ref manifold, workerIndex, ref pairMaterial);
+        return true;
+    }
+
+    [MethodImpl(MethodImplOptions.AggressiveInlining)]
+    public bool ConfigureContactManifold(int workerIndex, CollidablePair pair, int childIndexA, int childIndexB, ref ConvexContactManifold manifold)
+    {
+        return true;
+    }
+
+    public void Dispose()
+    {
+        Characters.Dispose();
+    }
+
+    public void Initialize(Simulation simulation)
     {
-        public CharacterControllers Characters;
-
-        public CharacterNarrowphaseCallbacks(CharacterControllers characters)
-        {
-            Characters = characters;
-        }
-
-        [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public bool AllowContactGeneration(int workerIndex, CollidableReference a, CollidableReference b)
-        {
-            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<TManifold>(int workerIndex, CollidablePair pair, ref TManifold manifold, out PairMaterialProperties pairMaterial) where TManifold : unmanaged, IContactManifold<TManifold>
-        {
-            pairMaterial = new PairMaterialProperties { FrictionCoefficient = 1, MaximumRecoveryVelocity = 2, SpringSettings = new SpringSettings(30, 1) };
-            Characters.TryReportContacts(pair, ref manifold, workerIndex, ref pairMaterial);
-            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()
-        {
-            Characters.Dispose();
-        }
-
-        public void Initialize(Simulation simulation)
-        {
-            Characters.Initialize(simulation);
-        }
+        Characters.Initialize(simulation);
     }
 }
 
diff --git a/Demos/Demos/ClothDemo.cs b/Demos/Demos/ClothDemo.cs
index 1fc4ed052..845007d5a 100644
--- a/Demos/Demos/ClothDemo.cs
+++ b/Demos/Demos/ClothDemo.cs
@@ -12,257 +12,271 @@
 using System.Numerics;
 using System.Runtime.CompilerServices;
 
-namespace Demos.Demos
+namespace Demos.Demos;
+
+struct ClothCollisionFilter
 {
-    struct ClothCollisionFilter
-    {
-        ushort x;
-        ushort y;
-        int instanceId;
+    ushort x;
+    ushort y;
+    int instanceId;
 
-        [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public ClothCollisionFilter(int x, int y, int instanceId)
-        {
-            const int max = 1 << 16;
-            Debug.Assert(x >= 0 && x < max && y >= 0 && y < max, "This filter packs local indices, so their range is limited.");
-            this.x = (ushort)x;
-            this.y = (ushort)y;
-            this.instanceId = instanceId;
-        }
+    [MethodImpl(MethodImplOptions.AggressiveInlining)]
+    public ClothCollisionFilter(int x, int y, int instanceId)
+    {
+        const int max = 1 << 16;
+        Debug.Assert(x >= 0 && x < max && y >= 0 && y < max, "This filter packs local indices, so their range is limited.");
+        this.x = (ushort)x;
+        this.y = (ushort)y;
+        this.instanceId = instanceId;
+    }
 
-        [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        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;
-            var differenceX = a.x - b.x;
-            if (differenceX < -minimumDistance || differenceX > minimumDistance)
-                return true;
-            var differenceY = a.y - b.y;
-            if (differenceY < -minimumDistance || differenceY > minimumDistance)
-                return true;
-            return false;
-        }
+    [MethodImpl(MethodImplOptions.AggressiveInlining)]
+    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)).
+        var differenceX = a.x - b.x;
+        if (differenceX < -minimumDistance || differenceX > minimumDistance)
+            return true;
+        var differenceY = a.y - b.y;
+        if (differenceY < -minimumDistance || differenceY > minimumDistance)
+            return true;
+        return false;
     }
+}
 
 
-    struct ClothCallbacks : INarrowPhaseCallbacks
+struct ClothCallbacks : INarrowPhaseCallbacks, Dancers.IDancerNarrowPhaseCallbacks<ClothCallbacks, ClothCollisionFilter> //"IDancerNarrowPhaseCallbacks" just means this is a INarrowPhaseCallbacks usable with the DemoDancers.
+{
+    public CollidableProperty<ClothCollisionFilter> Filters;
+    public PairMaterialProperties Material;
+    /// <summary>
+    /// 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.
+    /// </summary>
+    public int MinimumDistanceForSelfCollisions;
+
+    public ClothCallbacks(CollidableProperty<ClothCollisionFilter> filters, PairMaterialProperties material, int minimumDistanceForSelfCollisions = 3)
     {
-        public CollidableProperty<ClothCollisionFilter> Filters;
-        public void Initialize(Simulation simulation)
-        {
-            Filters.Initialize(simulation);
-        }
+        Filters = filters;
+        Material = material;
+        MinimumDistanceForSelfCollisions = minimumDistanceForSelfCollisions;
+    }
+    static ClothCallbacks Dancers.IDancerNarrowPhaseCallbacks<ClothCallbacks, ClothCollisionFilter>.Create(CollidableProperty<ClothCollisionFilter> filters, PairMaterialProperties pairMaterialProperties, int minimumDistanceForSelfCollisions)
+    {
+        return new ClothCallbacks(filters, pairMaterialProperties, minimumDistanceForSelfCollisions);
+    }
+    public ClothCallbacks(CollidableProperty<ClothCollisionFilter> filters, int minimumDistanceForSelfCollisions = 3)
+        : this(filters, new PairMaterialProperties { SpringSettings = new SpringSettings(30, 1), FrictionCoefficient = 0.25f, MaximumRecoveryVelocity = 2f }, minimumDistanceForSelfCollisions)
+    {
+    }
 
-        [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public bool AllowContactGeneration(int workerIndex, CollidableReference a, CollidableReference b)
-        {
-            if (a.Mobility != CollidableMobility.Static && b.Mobility != CollidableMobility.Static)
-            {
-                return ClothCollisionFilter.Test(Filters[a.BodyHandle], Filters[b.BodyHandle]);
-            }
-            return a.Mobility == CollidableMobility.Dynamic || b.Mobility == CollidableMobility.Dynamic;
-        }
+    public void Initialize(Simulation simulation)
+    {
+        Filters.Initialize(simulation);
+    }
 
-        [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public bool AllowContactGeneration(int workerIndex, CollidablePair pair, int childIndexA, int childIndexB)
-        {
-            return true;
-        }
-         
-        [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public unsafe bool ConfigureContactManifold<TManifold>(int workerIndex, CollidablePair pair, ref TManifold manifold, out PairMaterialProperties pairMaterial) where TManifold : unmanaged, IContactManifold<TManifold>
+    [MethodImpl(MethodImplOptions.AggressiveInlining)]
+    public bool AllowContactGeneration(int workerIndex, CollidableReference a, CollidableReference b, ref float speculativeMargin)
+    {
+        if (a.Mobility != CollidableMobility.Static && b.Mobility != CollidableMobility.Static)
         {
-            pairMaterial.FrictionCoefficient = 0.25f;
-            pairMaterial.MaximumRecoveryVelocity = 2f;
-            pairMaterial.SpringSettings = new SpringSettings(30, 1);
-            return true;
+            return ClothCollisionFilter.Test(Filters[a.BodyHandle], Filters[b.BodyHandle], MinimumDistanceForSelfCollisions);
         }
+        return a.Mobility == CollidableMobility.Dynamic || b.Mobility == CollidableMobility.Dynamic;
+    }
 
-        [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public unsafe 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()
-        {
-            Filters.Dispose();
-        }
+    [MethodImpl(MethodImplOptions.AggressiveInlining)]
+    public bool ConfigureContactManifold<TManifold>(int workerIndex, CollidablePair pair, ref TManifold manifold, out PairMaterialProperties pairMaterial) where TManifold : unmanaged, IContactManifold<TManifold>
+    {
+        pairMaterial = Material;
+        return true;
     }
 
-    /// <summary>
-    /// Shows a few different examples of cloth-ish constraint lattices.
-    /// </summary>
-    public class ClothDemo : Demo
+    [MethodImpl(MethodImplOptions.AggressiveInlining)]
+    public bool ConfigureContactManifold(int workerIndex, CollidablePair pair, int childIndexA, int childIndexB, ref ConvexContactManifold manifold)
     {
-        delegate bool KinematicDecider(int rowIndex, int columnIndex, int width, int height);
+        return true;
+    }
+
+    public void Dispose()
+    {
+        Filters.Dispose();
+    }
+}
 
-        BodyHandle[,] CreateBodyGrid(in Vector3 position, in Quaternion orientation, int width, int height, float spacing, float bodyRadius, float massPerBody,
-            int instanceId, CollidableProperty<ClothCollisionFilter> filters, KinematicDecider isKinematic)
+/// <summary>
+/// Shows a few different examples of cloth-ish constraint lattices.
+/// </summary>
+public class ClothDemo : Demo
+{
+    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<ClothCollisionFilter> filters, KinematicDecider isKinematic)
+    {
+        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)
         {
-            var description = new BodyDescription
+            for (int columnIndex = 0; columnIndex < width; ++columnIndex)
             {
-                Activity = new BodyActivityDescription(0.01f),
-                Collidable = new CollidableDescription(Simulation.Shapes.Add(new Sphere(bodyRadius)), 0.1f),
-                LocalInertia = default,
-                Pose = new RigidPose(default, orientation)
-            };
-            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);
-                }
+                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)
+    void CreateAreaConstraints(BodyHandle[,] bodyHandles, SpringSettings springSettings)
+    {
+        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)
-                {
-                    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));
-                }
+                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 = 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));
+                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 CreateDistanceConstraints(BodyHandle[,] bodyHandles, SpringSettings springSettings)
+    {
+        void CreateConstraintBetweenBodies(BodyHandle aHandle, BodyHandle bHandle)
         {
-            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)
-                {
-                    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)
+            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)
             {
-                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]);
-                }
+                //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));
             }
         }
-
-        RolloverInfo rolloverInfo;
-
-        public unsafe override void Initialize(ContentArchive content, Camera camera)
+        for (int rowIndex = 0; rowIndex < bodyHandles.GetLength(0); ++rowIndex)
         {
-            camera.Position = new Vector3(0, 25, 100);
-            camera.Yaw = 0;
-            camera.Pitch = 0;
-
-            var filters = new CollidableProperty<ClothCollisionFilter>();
-            //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());
-            rolloverInfo = new RolloverInfo();
-
-            bool KinematicTopCorners(int rowIndex, int columnIndex, int width, int height)
+            for (int columnIndex = 0; columnIndex < bodyHandles.GetLength(1) - 1; ++columnIndex)
             {
-                return rowIndex == 0 && (columnIndex == width - 1 || columnIndex == 0);
-            }
-            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);
-            {
-                var position = new Vector3(-90, 40, 0);
-                var handles = CreateBodyGrid(position, initialRotation, 10, 30, 1f, 0.65f, 1, clothInstanceId++, filters, KinematicTopCorners);
-                CreateDistanceConstraints(handles, new SpringSettings(20, 1));
-                rolloverInfo.Add(position + new Vector3(5, 2, 0), "Stiff distance constraints only, no area constraints");
-            }
-            {
-                var position = new Vector3(-70, 40, 0);
-                var handles = CreateBodyGrid(position, initialRotation, 10, 30, 1f, 0.65f, 1, clothInstanceId++, filters, KinematicTopCorners);
-                CreateDistanceConstraints(handles, new SpringSettings(20, 1));
-                CreateAreaConstraints(handles, new SpringSettings(30, 1));
-                rolloverInfo.Add(position + new Vector3(5, 2, 0), "Stiff distance constraints with area constraints");
+                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)
             {
-                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));
-                rolloverInfo.Add(position + new Vector3(5, 2, 0), "Soft distance constraints only, no area constraints");
+                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)
             {
-                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));
-                CreateAreaConstraints(handles, new SpringSettings(30, 1));
-                rolloverInfo.Add(position + new Vector3(5, 2, 0), "Soft distance constraints with area constraints (note narrowing)");
+                CreateConstraintBetweenBodies(bodyHandles[rowIndex, columnIndex], bodyHandles[rowIndex + 1, columnIndex + 1]);
+                CreateConstraintBetweenBodies(bodyHandles[rowIndex, columnIndex + 1], bodyHandles[rowIndex + 1, columnIndex]);
             }
+        }
+    }
 
+    RolloverInfo rolloverInfo;
 
-            Simulation.Statics.Add(new StaticDescription(
-                new Vector3(60, 20, 0), QuaternionEx.CreateFromAxisAngle(new Vector3(0, 0, 1), MathF.PI * 0.5f),
-                new CollidableDescription(Simulation.Shapes.Add(new Capsule(8, 120)), 0.1f)));
-            Simulation.Statics.Add(new StaticDescription(
-                new Vector3(30, 5, 0), QuaternionEx.CreateFromAxisAngle(new Vector3(1, 0, 0), MathF.PI * 0.5f),
-                new CollidableDescription(Simulation.Shapes.Add(new Capsule(8, 60)), 0.1f)));
+    public override void Initialize(ContentArchive content, Camera camera)
+    {
+        camera.Position = new Vector3(0, 25, 100);
+        camera.Yaw = 0;
+        camera.Pitch = 0;
 
-            {
-                var position = new Vector3(10, 40, -32);
-                var handles = CreateBodyGrid(position, initialRotation, 96, 96, 0.666f, 0.5f, 1, clothInstanceId++, filters, FullyDynamic);
-                CreateDistanceConstraints(handles, new SpringSettings(10, 1));
-                CreateAreaConstraints(handles, new SpringSettings(30, 1));
-                rolloverInfo.Add(position + new Vector3(32, 2, 0), "Medium stiffness with area constraints");
-            }
+        var filters = new CollidableProperty<ClothCollisionFilter>();
+        Simulation = Simulation.Create(BufferPool, new ClothCallbacks(filters), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(8, 1));
+        rolloverInfo = new RolloverInfo();
 
-            Simulation.Statics.Add(new StaticDescription(new Vector3(-40, 0, 0), new CollidableDescription(Simulation.Shapes.Add(new Box(200, 1, 200)), 0.1f)));
+        bool KinematicTopCorners(int rowIndex, int columnIndex, int width, int height)
+        {
+            return rowIndex == 0 && (columnIndex == width - 1 || columnIndex == 0);
+        }
+        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);
+        {
+            var position = new Vector3(-90, 40, 0);
+            var handles = CreateBodyGrid(position, initialRotation, 10, 30, 1f, 0.65f, 1, clothInstanceId++, filters, KinematicTopCorners);
+            CreateDistanceConstraints(handles, new SpringSettings(20, 1));
+            rolloverInfo.Add(position + new Vector3(5, 2, 0), "Stiff distance constraints only, no area constraints");
         }
+        {
+            var position = new Vector3(-70, 40, 0);
+            var handles = CreateBodyGrid(position, initialRotation, 10, 30, 1f, 0.65f, 1, clothInstanceId++, filters, KinematicTopCorners);
+            CreateDistanceConstraints(handles, new SpringSettings(20, 1));
+            CreateAreaConstraints(handles, new SpringSettings(30, 1));
+            rolloverInfo.Add(position + new Vector3(5, 2, 0), "Stiff distance constraints with area constraints");
+        }
+        {
+            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(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(5, 1));
+            CreateAreaConstraints(handles, new SpringSettings(30, 1));
+            rolloverInfo.Add(position + new Vector3(5, 2, 0), "Soft distance constraints with area constraints");
+        }
+
+
+        Simulation.Statics.Add(new StaticDescription(
+            new Vector3(60, 20, 0), QuaternionEx.CreateFromAxisAngle(new Vector3(0, 0, 1), MathF.PI * 0.5f),
+            Simulation.Shapes.Add(new Capsule(8, 120))));
+        Simulation.Statics.Add(new StaticDescription(
+            new Vector3(30, 5, 0), QuaternionEx.CreateFromAxisAngle(new Vector3(1, 0, 0), MathF.PI * 0.5f),
+            Simulation.Shapes.Add(new Capsule(8, 60))));
 
-        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(10, 40, -32);
+            var handles = CreateBodyGrid(position, initialRotation, 96, 96, 0.666f, 0.5f, 1, clothInstanceId++, filters, FullyDynamic);
+            CreateDistanceConstraints(handles, new SpringSettings(10, 1));
+            CreateAreaConstraints(handles, new SpringSettings(30, 1));
+            rolloverInfo.Add(position + new Vector3(32, 2, 0), "Medium stiffness with area constraints");
         }
 
+        Simulation.Statics.Add(new StaticDescription(new Vector3(-40, 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 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/CollisionQueryDemo.cs b/Demos/Demos/CollisionQueryDemo.cs
index 0e562e743..8ed1903a4 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;
@@ -9,257 +10,258 @@
 using DemoRenderer.UI;
 using DemoUtilities;
 using System;
-using System.Collections.Generic;
 using System.Numerics;
 using System.Runtime.CompilerServices;
-using System.Text;
 
-namespace Demos.Demos
+namespace Demos.Demos;
+
+/// <summary>
+/// Shows one way of handling collision queries that require contact-level test accuracy.
+/// </summary>
+public class CollisionQueryDemo : Demo
 {
-    /// <summary>
-    /// Shows one way of handling collision queries that require contact-level test accuracy.
-    /// </summary>
-    public class CollisionQueryDemo : Demo
+    public override void Initialize(ContentArchive content, Camera camera)
     {
-        public override void Initialize(ContentArchive content, Camera camera)
-        {
-            camera.Position = new Vector3(0, 7, 20);
-            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());
+        camera.Position = new Vector3(0, 7, 20);
+        camera.Yaw = 0;
+        camera.Pitch = 0;
 
-            Simulation.Statics.Add(new StaticDescription(new Vector3(), new CollidableDescription(Simulation.Shapes.Add(new Box(100, 1, 100)), 0.1f)));
+        Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(new SpringSettings(30, 1)), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(8, 1));
 
-            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.3f), 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());
-                Simulation.Bodies.Add(descriptionToDrop);
-            }
+        Simulation.Statics.Add(new StaticDescription(new Vector3(), Simulation.Shapes.Add(new Box(100, 1, 100))));
 
-            //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);
-            Simulation.Statics.Add(new StaticDescription(new Vector3(0, 0, 0), new CollidableDescription(Simulation.Shapes.Add(mesh), 0.1f)));
+        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 < 128; ++i)
+        {
+            descriptionToDrop.Pose.Position = new Vector3(-5 + 10 * random.NextSingle(), 45 + 150 * random.NextSingle(), -5 + 10 * random.NextSingle());
+            Simulation.Bodies.Add(descriptionToDrop);
         }
 
+        //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.
+        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)));
+    }
+
+    /// <summary>
+    /// Provides callbacks for filtering and data collection to the CollisionBatcher we'll be using to test query shapes against the detected environment.
+    /// </summary>
+    public struct BatcherCallbacks : ICollisionCallbacks
+    {
         /// <summary>
-        /// Provides callbacks for filtering and data collection to the CollisionBatcher we'll be using to test query shapes against the detected environment.
+        /// Set to true for a pair if a nonnegative depth is detected by collision testing.
         /// </summary>
-        public struct BatcherCallbacks : ICollisionCallbacks
-        {
-            /// <summary>
-            /// Set to true for a pair if a nonnegative depth is detected by collision testing.
-            /// </summary>
-            public Buffer<bool> QueryWasTouched;
+        public Buffer<bool> QueryWasTouched;
 
-            //These callbacks provide filtering and reporting for pairs being processed by the collision batcher.
-            //"Pair id" refers to the identifier given to the pair when it was added to the batcher.
-            [MethodImpl(MethodImplOptions.AggressiveInlining)]
-            public bool AllowCollisionTesting(int pairId, int childA, int childB)
-            {
-                //If you wanted to filter based on the children of an encountered nonconvex object, here would be the place to do it.
-                //The pairId could be used to look up the involved objects and any metadata necessary for filtering.
-                return true;
-            }
+        //These callbacks provide filtering and reporting for pairs being processed by the collision batcher.
+        //"Pair id" refers to the identifier given to the pair when it was added to the batcher.
+        [MethodImpl(MethodImplOptions.AggressiveInlining)]
+        public bool AllowCollisionTesting(int pairId, int childA, int childB)
+        {
+            //If you wanted to filter based on the children of an encountered nonconvex object, here would be the place to do it.
+            //The pairId could be used to look up the involved objects and any metadata necessary for filtering.
+            return true;
+        }
 
-            [MethodImpl(MethodImplOptions.AggressiveInlining)]
-            public void OnChildPairCompleted(int pairId, int childA, int childB, ref ConvexContactManifold manifold)
-            {
-                //If you need to do any processing on a child manifold before it goes back to a nonconvex processing pass, this is the place to do it.
-                //Convex-convex pairs won't invoke this function at all.
-            }
+        [MethodImpl(MethodImplOptions.AggressiveInlining)]
+        public void OnChildPairCompleted(int pairId, int childA, int childB, ref ConvexContactManifold manifold)
+        {
+            //If you need to do any processing on a child manifold before it goes back to a nonconvex processing pass, this is the place to do it.
+            //Convex-convex pairs won't invoke this function at all.
+        }
 
-            [MethodImpl(MethodImplOptions.AggressiveInlining)]
-            public void OnPairCompleted<TManifold>(int pairId, ref TManifold manifold) where TManifold : unmanaged, IContactManifold<TManifold>
+        [MethodImpl(MethodImplOptions.AggressiveInlining)]
+        public void OnPairCompleted<TManifold>(int pairId, ref TManifold manifold) where TManifold : unmanaged, IContactManifold<TManifold>
+        {
+            //This function hands off the completed manifold with all postprocessing (NonconvexReduction, MeshReduction, etc.) complete.
+            //For the purposes of this demo, we're interested in boolean collision testing.
+            //(This process was a little overkill for a pure boolean test, but there is no pure boolean path right now because the contact manifold generators turned out fast enough.
+            //And if you find yourself wanting contact data, well, you've got it handy!)
+            for (int i = 0; i < manifold.Count; ++i)
             {
-                //This function hands off the completed manifold with all postprocessing (NonconvexReduction, MeshReduction, etc.) complete.
-                //For the purposes of this demo, we're interested in boolean collision testing.
-                //(This process was a little overkill for a pure boolean test, but there is no pure boolean path right now because the contact manifold generators turned out fast enough.
-                //And if you find yourself wanting contact data, well, you've got it handy!)
-                for (int i = 0; i < manifold.Count; ++i)
+                if (manifold.GetDepth(i) >= 0)
                 {
-                    //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)
-                    {
-                        QueryWasTouched[pairId] = true;
-                        break;
-                    }
+                    QueryWasTouched[pairId] = true;
+                    break;
                 }
             }
         }
+    }
 
-        /// <summary>
-        /// Called by the BroadPhase.GetOverlaps to collect all encountered collidables.
-        /// </summary>
-        struct BroadPhaseOverlapEnumerator : IBreakableForEach<CollidableReference>
+    /// <summary>
+    /// Called by the BroadPhase.GetOverlaps to collect all encountered collidables.
+    /// </summary>
+    struct BroadPhaseOverlapEnumerator : IBreakableForEach<CollidableReference>
+    {
+        public QuickList<CollidableReference> References;
+        //The enumerator never gets stored into unmanaged memory, so it's safe to include a reference type instance.
+        public BufferPool Pool;
+        public bool LoopBody(CollidableReference reference)
         {
-            public QuickList<CollidableReference> References;
-            //The enumerator never gets stored into unmanaged memory, so it's safe to include a reference type instance.
-            public BufferPool Pool;
-            public bool LoopBody(CollidableReference reference)
-            {
-                References.Allocate(Pool) = reference;
-                //If you wanted to do any top-level filtering, this would be a good spot for it.
-                //The CollidableReference tells you whether it's a body or a static object and the associated handle. You can look up metadata with that.
-                return true;
-            }
+            References.Allocate(Pool) = reference;
+            //If you wanted to do any top-level filtering, this would be a good spot for it.
+            //The CollidableReference tells you whether it's a body or a static object and the associated handle. You can look up metadata with that.
+            return true;
         }
+    }
 
 
-        void GetPoseAndShape(CollidableReference reference, out RigidPose pose, out TypedIndex shapeIndex)
+    void GetPoseAndShape(CollidableReference reference, out RigidPose pose, out TypedIndex shapeIndex)
+    {
+        //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)
         {
-            //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;
-            }
-            else
-            {
-                var bodyReference = Simulation.Bodies.GetBodyReference(reference.BodyHandle);
-                pose = bodyReference.Pose;
-                shapeIndex = bodyReference.Collidable.Shape;
-            }
+            var collidable = Simulation.Statics[reference.StaticHandle];
+            pose = collidable.Pose;
+            shapeIndex = collidable.Shape;
         }
+        else
+        {
+            var bodyReference = Simulation.Bodies[reference.BodyHandle];
+            pose = bodyReference.Pose;
+            shapeIndex = bodyReference.Collidable.Shape;
+        }
+    }
 
-        /// <summary>
-        /// Adds a shape query to the collision batcher.
-        /// </summary>
-        /// <param name="queryShapeType">Type of the shape to test.</param>
-        /// <param name="queryShapeData">Shape data to test.</param>
-        /// <param name="queryShapeSize">Size of the shape data in bytes.</param>
-        /// <param name="queryBoundsMin">Minimum of the query shape's bounding box.</param>
-        /// <param name="queryBoundsMax">Maximum of the query shape's bounding box.</param>
-        /// <param name="queryPose">Pose of the query shape.</param>
-        /// <param name="queryId">Id to use to refer to this query when the collision batcher finishes processing it.</param>
-        /// <param name="batcher">Batcher to add the query's tests to.</param>
-        public unsafe void AddQueryToBatch(int queryShapeType, void* queryShapeData, int queryShapeSize, in Vector3 queryBoundsMin, in Vector3 queryBoundsMax, in RigidPose queryPose, int queryId, ref CollisionBatcher<BatcherCallbacks> batcher)
+    /// <summary>
+    /// Adds a shape query to the collision batcher.
+    /// </summary>
+    /// <param name="queryShapeType">Type of the shape to test.</param>
+    /// <param name="queryShapeData">Shape data to test.</param>
+    /// <param name="queryShapeSize">Size of the shape data in bytes.</param>
+    /// <param name="queryBoundsMin">Minimum of the query shape's bounding box.</param>
+    /// <param name="queryBoundsMax">Maximum of the query shape's bounding box.</param>
+    /// <param name="queryPose">Pose of the query shape.</param>
+    /// <param name="queryId">Id to use to refer to this query when the collision batcher finishes processing it.</param>
+    /// <param name="batcher">Batcher to add the query's tests to.</param>
+    public unsafe void AddQueryToBatch(int queryShapeType, void* queryShapeData, int queryShapeSize, Vector3 queryBoundsMin, Vector3 queryBoundsMax, in RigidPose queryPose, int queryId, ref CollisionBatcher<BatcherCallbacks> batcher)
+    {
+        var broadPhaseEnumerator = new BroadPhaseOverlapEnumerator { Pool = BufferPool, References = new QuickList<CollidableReference>(16, BufferPool) };
+        Simulation.BroadPhase.GetOverlaps(queryBoundsMin, queryBoundsMax, BufferPool, ref broadPhaseEnumerator);
+        for (int overlapIndex = 0; overlapIndex < broadPhaseEnumerator.References.Count; ++overlapIndex)
         {
-            var broadPhaseEnumerator = new BroadPhaseOverlapEnumerator { Pool = BufferPool, References = new QuickList<CollidableReference>(16, BufferPool) };
-            Simulation.BroadPhase.GetOverlaps(queryBoundsMin, queryBoundsMax, ref broadPhaseEnumerator);
-            for (int overlapIndex = 0; overlapIndex < broadPhaseEnumerator.References.Count; ++overlapIndex)
-            {
-                GetPoseAndShape(broadPhaseEnumerator.References[overlapIndex], out var pose, out var shapeIndex);
-                Simulation.Shapes[shapeIndex.Type].GetShapeData(shapeIndex.Index, out var shapeData, out _);
-                //In this path, we assume that the incoming shape data is ephemeral. The collision batcher may last longer than the data pointer.
-                //To avoid undefined access, we cache the query data into the collision batcher and use a pointer to the cache instead.
-                batcher.CacheShapeB(shapeIndex.Type, queryShapeType, queryShapeData, queryShapeSize, out var cachedQueryShapeData);
-                batcher.AddDirectly(
-                    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));
-            }
-            broadPhaseEnumerator.References.Dispose(BufferPool);
+            GetPoseAndShape(broadPhaseEnumerator.References[overlapIndex], out var pose, out var shapeIndex);
+            Simulation.Shapes[shapeIndex.Type].GetShapeData(shapeIndex.Index, out var shapeData, out _);
+            //In this path, we assume that the incoming shape data is ephemeral. The collision batcher may last longer than the data pointer.
+            //To avoid undefined access, we cache the query data into the collision batcher and use a pointer to the cache instead.
+            batcher.CacheShapeB(shapeIndex.Type, queryShapeType, queryShapeData, queryShapeSize, out var cachedQueryShapeData);
+            batcher.AddDirectly(
+                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, pose.Orientation, queryPose.Orientation, 0, new PairContinuation(queryId));
         }
+        broadPhaseEnumerator.References.Dispose(BufferPool);
+    }
 
-        /// <summary>
-        /// Adds a shape query to the collision batcher.
-        /// </summary>
-        /// <typeparam name="TShape">Type of the query shape.</typeparam>
-        /// <param name="shape">Shape to use in the query.</param>
-        /// <param name="pose">Pose of the query shape.</param>
-        /// <param name="queryId">Id to use to refer to this query when the collision batcher finishes processing it.</param>
-        /// <param name="batcher">Batcher to add the query's tests to.</param>
-        public unsafe void AddQueryToBatch<TShape>(TShape shape, in RigidPose pose, int queryId, ref CollisionBatcher<BatcherCallbacks> batcher) where TShape : IConvexShape
+    /// <summary>
+    /// Adds a shape query to the collision batcher.
+    /// </summary>
+    /// <typeparam name="TShape">Type of the query shape.</typeparam>
+    /// <param name="shape">Shape to use in the query.</param>
+    /// <param name="pose">Pose of the query shape.</param>
+    /// <param name="queryId">Id to use to refer to this query when the collision batcher finishes processing it.</param>
+    /// <param name="batcher">Batcher to add the query's tests to.</param>
+    public unsafe void AddQueryToBatch<TShape>(TShape shape, in RigidPose pose, int queryId, ref CollisionBatcher<BatcherCallbacks> batcher) where TShape : IConvexShape
+    {
+        var queryShapeData = Unsafe.AsPointer(ref shape);
+        var queryShapeSize = Unsafe.SizeOf<TShape>();
+        shape.ComputeBounds(pose.Orientation, out var boundingBoxMin, out var boundingBoxMax);
+        boundingBoxMin += pose.Position;
+        boundingBoxMax += pose.Position;
+        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.
+    /// <summary>
+    /// Adds a shape query to the collision batcher.
+    /// </summary>
+    /// <typeparam name="TShape">Type of the query shape.</typeparam>
+    /// <param name="shape">Shape to use in the query.</param>
+    /// <param name="pose">Pose of the query shape.</param>
+    /// <param name="queryId">Id to use to refer to this query when the collision batcher finishes processing it.</param>
+    /// <param name="batcher">Batcher to add the query's tests to.</param>
+    public unsafe void AddQueryToBatch(Shapes shapes, TypedIndex queryShapeIndex, in RigidPose queryPose, int queryId, ref CollisionBatcher<BatcherCallbacks> batcher)
+    {
+        var shapeBatch = shapes[queryShapeIndex.Type];
+        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<CollidableReference>(16, BufferPool) };
+        Simulation.BroadPhase.GetOverlaps(queryBoundsMin, queryBoundsMax, BufferPool, ref broadPhaseEnumerator);
+        for (int overlapIndex = 0; overlapIndex < broadPhaseEnumerator.References.Count; ++overlapIndex)
         {
-            var queryShapeData = Unsafe.AsPointer(ref shape);
-            var queryShapeSize = Unsafe.SizeOf<TShape>();
-            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);
+            GetPoseAndShape(broadPhaseEnumerator.References[overlapIndex], out var pose, out var shapeIndex);
+            //Since both involved shapes are from the simulation cache, we don't need to cache them ourselves.
+            Simulation.Shapes[shapeIndex.Type].GetShapeData(shapeIndex.Index, out var shapeData, out _);
+            batcher.AddDirectly(
+                shapeIndex.Type, queryShapeIndex.Type,
+                shapeData, queryShapeData,
+                //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));
         }
+        broadPhaseEnumerator.References.Dispose(BufferPool);
+    }
 
-        //This version of the query isn't used in the demo, but shows how you could use a simulation-cached shape in a query.
-        /// <summary>
-        /// Adds a shape query to the collision batcher.
-        /// </summary>
-        /// <typeparam name="TShape">Type of the query shape.</typeparam>
-        /// <param name="shape">Shape to use in the query.</param>
-        /// <param name="pose">Pose of the query shape.</param>
-        /// <param name="queryId">Id to use to refer to this query when the collision batcher finishes processing it.</param>
-        /// <param name="batcher">Batcher to add the query's tests to.</param>
-        public unsafe void AddQueryToBatch(Shapes shapes, TypedIndex queryShapeIndex, in RigidPose queryPose, int queryId, ref CollisionBatcher<BatcherCallbacks> batcher)
+    //For the demo, we'll use a bunch of boxes as queries.
+    struct Query
+    {
+        public Box Box;
+        public RigidPose Pose;
+    }
+
+    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<BatcherCallbacks>(BufferPool, Simulation.Shapes, Simulation.NarrowPhase.CollisionTaskRegistry, 0, new BatcherCallbacks());
+
+        //Create a set of queries up front. We store them so that we can render them after the fact with colors according to their touched states.
+        const int widthInQueries = 5;
+        var queries = new QuickList<Query>(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 < widthInQueries; ++i)
         {
-            var shapeBatch = shapes[queryShapeIndex.Type];
-            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<CollidableReference>(16, BufferPool) };
-            Simulation.BroadPhase.GetOverlaps(queryBoundsMin, queryBoundsMax, ref broadPhaseEnumerator);
-            for (int overlapIndex = 0; overlapIndex < broadPhaseEnumerator.References.Count; ++overlapIndex)
+            for (int j = 0; j < widthInQueries; ++j)
             {
-                GetPoseAndShape(broadPhaseEnumerator.References[overlapIndex], out var pose, out var shapeIndex);
-                //Since both involved shapes are from the simulation cache, we don't need to cache them ourselves.
-                Simulation.Shapes[shapeIndex.Type].GetShapeData(shapeIndex.Index, out var shapeData, out _);
-                batcher.AddDirectly(
-                    shapeIndex.Type, queryShapeIndex.Type,
-                    shapeData, queryShapeData,
-                    //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));
+                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))
+                };
             }
-            broadPhaseEnumerator.References.Dispose(BufferPool);
         }
+        //Note that the callbacks and set are value types, so you have to be a little careful about copying.
+        ref var queryWasTouched = ref collisionBatcher.Callbacks.QueryWasTouched;
+        BufferPool.Take(queries.Count, out queryWasTouched);
+        queryWasTouched.Clear(0, queryWasTouched.Length);
 
-        //For the demo, we'll use a bunch of boxes as queries.
-        struct Query
+        for (int queryIndex = 0; queryIndex < queries.Count; ++queryIndex)
         {
-            public Box Box;
-            public RigidPose Pose;
+            ref var query = ref queries[queryIndex];
+            AddQueryToBatch(query.Box, query.Pose, queryIndex, ref collisionBatcher);
         }
+        //While the collision batcher may flush batches here and there when a new test is added if it fills a batch, 
+        //it's likely that there remain leftover pairs that didn't fill up a batch completely. Force a complete flush.
+        //Note that this also returns all resources used by the batcher to the BufferPool.
+        collisionBatcher.Flush();
 
-        public unsafe override void Render(Renderer renderer, Camera camera, Input input, TextBuilder text, Font font)
+        //Render the query boxes with the proper color.
+        for (int i = 0; i < queries.Count; ++i)
         {
-            //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<BatcherCallbacks>(BufferPool, Simulation.Shapes, Simulation.NarrowPhase.CollisionTaskRegistry, 0, new BatcherCallbacks());
-
-            //Create a set of queries up front. We store them so that we can render them after the fact with colors according to their touched states.
-            const int widthInQueries = 5;
-            var queries = new QuickList<Query>(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 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)) };
-                }
-            }
-            //Note that the callbacks and set are value types, so you have to be a little careful about copying.
-            ref var queryWasTouched = ref collisionBatcher.Callbacks.QueryWasTouched;
-            BufferPool.Take(queries.Count, out queryWasTouched);
-            queryWasTouched.Clear(0, queryWasTouched.Length);
-
-            for (int queryIndex = 0; queryIndex < queries.Count; ++queryIndex)
-            {
-                ref var query = ref queries[queryIndex];
-                AddQueryToBatch(query.Box, query.Pose, queryIndex, ref collisionBatcher);
-            }
-            //While the collision batcher may flush batches here and there when a new test is added if it fills a batch, 
-            //it's likely that there remain leftover pairs that didn't fill up a batch completely. Force a complete flush.
-            //Note that this also returns all resources used by the batcher to the BufferPool.
-            collisionBatcher.Flush();
+            ref var query = ref queries[i];
+            renderer.Shapes.AddShape(query.Box, Simulation.Shapes, query.Pose, queryWasTouched[i] ? new Vector3(0, 1, 0) : new Vector3(1, 0, 0));
+        }
 
-            //Render the query boxes with the proper color.
-            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));
-            }
+        BufferPool.Return(ref queryWasTouched);
+        queries.Dispose(BufferPool);
 
-            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);
     }
 }
 
diff --git a/Demos/Demos/CollisionTrackingDemo.cs b/Demos/Demos/CollisionTrackingDemo.cs
new file mode 100644
index 000000000..b99b4a5fb
--- /dev/null
+++ b/Demos/Demos/CollisionTrackingDemo.cs
@@ -0,0 +1,558 @@
+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;
+
+namespace Demos.Demos;
+
+/// <summary>
+/// Shows how to store out collision information provided in <see cref="INarrowPhaseCallbacks"/> for later analysis, 
+/// and how to use it to implement something like collision events without imposing the weird control flow that events imply.
+/// </summary>
+/// <remarks>
+/// This implementation focuses more on simplicity than performance.
+/// 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!
+/// </remarks>
+public class CollisionTrackingDemo : Demo
+{
+    /// <summary>
+    /// Refers to a collidable, or a child within a collidable.
+    /// </summary>
+    /// <remarks>This contact tracker allows the children of compounds to be tracked, so it's not enough to only store a <see cref="CollidableReference"/>.
+    /// That would only permit top-level pair analysis.</remarks>
+    public struct ContactSource : IEqualityComparerRef<ContactSource>, IEquatable<ContactSource>
+    {
+        /// <summary>
+        /// Collidable associated with this listener.
+        /// </summary>
+        public CollidableReference Collidable;
+        /// <summary>
+        /// Child index within the collidable associated with this listener, if any. -1 if the listener is associated with the entire collidable.
+        /// </summary>
+        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 };
+    }
+
+    /// <summary>
+    /// Stores contacts generated by the narrow phase for a pair.
+    /// </summary>
+    public struct PairCollision
+    {
+        /// <summary>
+        /// 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.
+        /// </summary>
+        public bool OtherIsAInPair;
+
+        /// <summary>
+        /// Gets the contacts generated by the narrow phase in the latest update affecting this pair.
+        /// </summary>
+        public NonconvexContactManifold Contacts;
+    }
+
+
+
+    /// <summary>
+    /// Tracks collisions for a set of collidables (or their children).
+    /// </summary>
+    public class CollisionTracker : IDisposable
+    {
+        IThreadDispatcher dispatcher;
+        BufferPool pool;
+        Simulation simulation;
+
+        public CollisionTracker(BufferPool pool)
+        {
+            this.pool = pool;
+            Tracked = new QuickDictionary<ContactSource, TrackedPairs, ContactSource>(16, pool);
+        }
+
+        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;
+        }
+
+        /// <summary>
+        /// Disposes the collision tracker's internal allocations and detaches its callbacks from the simulation.
+        /// </summary>
+        public void Dispose()
+        {
+            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
+        {
+            /// <summary>
+            /// 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.
+            /// </summary>
+            public QuickDictionary<ContactSource, PairCollision, ContactSource> Pairs;
+            /// <summary>
+            /// 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.
+            /// </summary>
+            public QuickDictionary<ContactSource, PairCollision, ContactSource> PreviousPairs;
+
+            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.
+                //(Or just part of them, incrementally! Probably better to avoid excess resizes!)
+                Pairs = new QuickDictionary<ContactSource, PairCollision, ContactSource>(16, pool);
+                PreviousPairs = new QuickDictionary<ContactSource, PairCollision, ContactSource>(16, pool);
+            }
+
+
+            public void Dispose(BufferPool pool)
+            {
+                Pairs.Dispose(pool);
+                PreviousPairs.Dispose(pool);
+            }
+        }
+
+        /// <summary>
+        /// Maps tracked objects to the contacts associated with them.
+        /// </summary>
+        public QuickDictionary<ContactSource, TrackedPairs, ContactSource> Tracked;
+
+        /// <summary>
+        /// Adds a collidable (or its child) to the collision tracker. Collisions associated with it will be included for queries.
+        /// </summary>
+        /// <param name="target">Collidable (or child) to track.</param>
+        /// <exception cref="ArgumentException">Triggered if the collidable or child is already present.</exception>
+        public void Track(ContactSource target)
+        {
+            if (Tracked.ContainsKey(target))
+                throw new ArgumentException("Object already tracked.");
+            Tracked.Add(target, new TrackedPairs(pool), pool);
+        }
+
+        /// <summary>
+        /// Removes a listener from the tracker.
+        /// </summary>
+        /// <param name="target">Collidable (or child) to untrack.</param>
+        /// <exception cref="ArgumentException">Triggered if the collidable or child is not present.</exception>
+        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);
+            Tracked.Values[elementIndex].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.
+
+        /// <summary>
+        /// Stores a single tracked result in a worker cache to later be flushed.
+        /// </summary>
+        struct WorkerPairContacts
+        {
+            public ContactSource Self;
+            public ContactSource Other;
+            public PairCollision Collision;
+        }
+
+        /// <summary>
+        /// Stores contacts for listeners reported by callbacks to a particular thread.
+        /// </summary>
+        Buffer<QuickList<WorkerPairContacts>> workerCaches;
+
+        /// <summary>
+        /// 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.
+        /// </summary>
+        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 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 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 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 = tracked.Pairs.Keys[j];
+                        if (other.Collidable.Mobility != CollidableMobility.Static && simulation.Bodies[other.Collidable.BodyHandle].Awake)
+                        {
+                            //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.
+                    }
+                }
+            }
+
+            const int initialWorkerCapacity = 512;
+            if (dispatcher != null)
+            {
+                //Cache the dispatcher so ReportContacts can use the buffers in the narrowphase callbacks.
+                workerCaches = new Buffer<QuickList<WorkerPairContacts>>(dispatcher.ThreadCount, pool);
+                for (int i = 0; i < workerCaches.Length; ++i)
+                {
+                    workerCaches[i] = new QuickList<WorkerPairContacts>(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<QuickList<WorkerPairContacts>>(1, pool);
+                workerCaches[0] = new QuickList<WorkerPairContacts>(initialWorkerCapacity, pool);
+            }
+        }
+
+        void FlushWorkerCache(ref QuickList<WorkerPairContacts> 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);
+        }
+        /// <summary>
+        /// Flushes all collisions found in the previous timestep into efficient storage for queries.
+        /// </summary>
+        void Flush(float dt, IThreadDispatcher threadDispatcher)
+        {
+            if (dispatcher != null)
+            {
+                //Flush the worker caches into the main storage and dispose the caches.
+                for (int i = 0; i < workerCaches.Length; ++i)
+                {
+                    FlushWorkerCache(ref workerCaches[i], 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<TContacts>(int workerIndex, ContactSource self, ContactSource other, bool otherIsA, ref TContacts contacts) where TContacts : unmanaged, IContactManifold<TContacts>
+        {
+            if (Tracked.ContainsKey(self))
+            {
+                //A is a listener, add it.            
+                ref var pairContacts = ref workerCaches[workerIndex].Allocate(dispatcher != null ? dispatcher.WorkerPools[workerIndex] : pool);
+                pairContacts.Self = self;
+                pairContacts.Other = other;
+                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.
+                    //(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<TContacts, ConvexContactManifold>(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.Collision.Contacts = Unsafe.As<TContacts, NonconvexContactManifold>(ref contacts);
+                }
+            }
+        }
+
+        /// <summary>
+        /// Notifies the tracker of a collision between collidable children.
+        /// </summary>
+        /// <param name="collidableA">First collidable in the pair.</param>
+        /// <param name="childIndexA">Child index associated with the first collidable in the pair.</param>
+        /// <param name="collidableB">Second collidable in the pair.</param>
+        /// <param name="childIndexB">Child index associated with the second collidable in the pair.</param>
+        /// <param name="workerIndex">Index of the worker invoking this callback.</param>
+        /// <param name="contacts">Contacts reported in the callback.</param>
+        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);
+        }
+
+
+        /// <summary>
+        /// Notifies the tracker of a collision between collidables.
+        /// </summary>
+        /// <typeparam name="TManifold">Type of the manifold being reported.</typeparam>
+        /// <param name="collidableA">First collidable in the pair.</param>
+        /// <param name="collidableB">Second collidable in the pair.</param>
+        /// <param name="workerIndex">Index of the worker invoking this callback.</param>
+        /// <param name="contacts">Contacts reported in the callback.</param>
+        public void ReportContacts<TManifold>(CollidableReference collidableA, CollidableReference collidableB, int workerIndex, ref TManifold contacts) where TManifold : unmanaged, IContactManifold<TManifold>
+        {
+            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);
+        }
+
+    }
+
+    /// <summary>
+    /// Callbacks invoked by the simulation's narrow phase.
+    /// In this demo, we'll collect all contact data associated with tracked objects for later processing.
+    /// </summary>
+    public 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 bool ConfigureContactManifold<TManifold>(int workerIndex, CollidablePair pair, ref TManifold manifold, out PairMaterialProperties pairMaterial) where TManifold : unmanaged, IContactManifold<TManifold>
+        {
+            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)
+        {
+            collisionTracker.Initialize(simulation);
+        }
+
+        public void Dispose()
+        {
+        }
+
+    }
+
+    /// <summary>
+    /// Tracks a particle created by a collision.
+    /// </summary>
+    struct ContactResponseParticle
+    {
+        public Vector3 Position;
+        public float Age;
+        public Vector3 Normal;
+    }
+    QuickList<ContactResponseParticle> 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<ContactResponseParticle>(8, BufferPool);
+
+        collisionTracker = new CollisionTracker(BufferPool);
+        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))));
+    }
+
+    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 PreviousContainsTouchingFeatureId(ref PairCollision pair, int featureId)
+    {
+        for (int i = 0; i < pair.Contacts.Count; ++i)
+        {
+            if (pair.Contacts.GetFeatureId(i) == featureId && pair.Contacts.GetDepth(i) >= 0)
+                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.
+        base.Update(window, camera, input, dt);
+
+        //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)
+        {
+            ref var collisions = ref collisionTracker.Tracked.Values[trackedIndex];
+            var self = collisionTracker.Tracked.Keys[trackedIndex];
+            for (int pairIndex = 0; pairIndex < collisions.Pairs.Count; ++pairIndex)
+            {
+                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 collisions.PreviousPairs.Values[otherIndexInPrevious];
+                    for (int i = 0; i < pair.Contacts.Count; ++i)
+                    {
+                        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(i), pair.Contacts.GetNormal(i), pair.OtherIsAInPair ? other.Collidable : self.Collidable);
+                        }
+                    }
+                }
+                else
+                {
+                    //No previous collision, so all contacts are new.
+                    for (int i = 0; i < pair.Contacts.Count; ++i)
+                    {
+                        AddParticle(pair.Contacts.GetOffset(i), pair.Contacts.GetNormal(i), pair.OtherIsAInPair ? 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("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);
+    }
+}
diff --git a/Demos/Demos/ColosseumDemo.cs b/Demos/Demos/ColosseumDemo.cs
index f0d207d78..ca336052c 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;
@@ -8,131 +9,118 @@
 using System;
 using System.Numerics;
 
-namespace Demos.Demos
+namespace Demos.Demos;
+
+/// <summary>
+/// A colosseum made out of boxes that is sometimes hit by large purple hail.
+/// </summary>
+public class ColosseumDemo : Demo
 {
-    /// <summary>
-    /// A colosseum made out of boxes that is sometimes hit by large purple hail.
-    /// </summary>
-    public class ColosseumDemo : 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 = new RigidPose(
-                        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 = new RigidPose(
-                    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 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;
+    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;
 
-            //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 SpringSettings(30, 1)), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(8, 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));
+        var ringBoxShape = new Box(0.5f, 1, 3);
+        var boxDescription = BodyDescription.CreateDynamic(new Vector3(), ringBoxShape.ComputeInertia(1), Simulation.Shapes.Add(ringBoxShape), 0.01f);
 
-            var layerPosition = new Vector3();
-            const int layerCount = 6;
-            var innerRadius = 15f;
-            var heightPerPlatform = 3;
-            var platformsPerLayer = 1;
-            var ringSpacing = 0.5f;
-            for (int layerIndex = 0; layerIndex < layerCount; ++layerIndex)
+        var layerPosition = new Vector3();
+        const int layerCount = 6;
+        var innerRadius = 15f;
+        var heightPerPlatform = 3;
+        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(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), new CollidableDescription(Simulation.Shapes.Add(new Box(500, 1, 500)), 0.1f)));
-
-            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));
+        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);
-            shootiePatootieShape.ComputeInertia(100, out var shootiePatootieInertia);
-            shootiePatootieDescription = BodyDescription.CreateDynamic(new Vector3(), shootiePatootieInertia, new CollidableDescription(Simulation.Shapes.Add(shootiePatootieShape), 10), new BodyActivityDescription(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))
             {
-                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);
-                }
+                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);
         }
+        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 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);
-        }
+    public override void Render(Renderer renderer, Camera camera, Input input, TextBuilder text, Font font)
+    {
+        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/Demos/CompoundDemo.cs b/Demos/Demos/CompoundDemo.cs
new file mode 100644
index 000000000..a1ad668f2
--- /dev/null
+++ b/Demos/Demos/CompoundDemo.cs
@@ -0,0 +1,223 @@
+using BepuUtilities;
+using DemoRenderer;
+using BepuPhysics;
+using BepuPhysics.Collidables;
+using System;
+using System.Numerics;
+using DemoContentLoader;
+using BepuPhysics.Constraints;
+using DemoRenderer.UI;
+using DemoUtilities;
+
+namespace Demos.Demos;
+
+public class CompoundDemo : Demo
+{
+    public override void Initialize(ContentArchive content, Camera camera)
+    {
+        camera.Position = new Vector3(-13f, 6, -13f);
+        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, -10f, 0)), new SolveDescription(8, 1));
+
+        using (var compoundBuilder = new CompoundBuilder(BufferPool, Simulation.Shapes, 8))
+        {
+            {
+                //Note that, in bepuphysics v2, there is no 'recentering' when constructing a shape. The pose you pass in for a child is exactly the pose that the compound will use, 
+                //even if the 'true' center of mass isn't at the local origin.
+                //Instead, if recentering is desired, it should performed ahead of time. The CompoundBuilder can help with this.
+                //We'll construct this compound using shapes far from the origin, and then use the CompoundBuilder overload that recenters the children and outputs the computed center.
+                var capsuleChildShape = new Capsule(0.5f, 0.5f);
+                var capsuleLocalPose = new RigidPose { Position = new Vector3(-0.5f, 4, 4), Orientation = Quaternion.Identity };
+                var boxChildShape = new Box(0.5f, 1f, 1.5f);
+                var boxLocalPose = new RigidPose { Position = new Vector3(0.5f, 4, 4), Orientation = Quaternion.Identity };
+
+                //All allocations from the buffer pool used for the final compound shape will be disposed when the demo is disposed. Don't have to worry about leaks in these demos.
+                compoundBuilder.Add(capsuleChildShape, capsuleLocalPose, 1);
+                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, Simulation.Shapes.Add(new Compound(compoundChildren)), 0.01f));
+            }
+
+            //Build a stack of sphere grids to stress manifold reduction heuristics in a convex-ish situation.
+            {
+                var gridShape = new Sphere(0.5f);
+                const float gridSpacing = 1.5f;
+                const int gridWidth = 3;
+                var gridShapeIndex = Simulation.Shapes.Add(gridShape);
+                var gridBoxInertia = gridShape.ComputeInertia(1);
+                float localPoseOffset = -0.5f * gridSpacing * (gridWidth - 1);
+                for (int i = 0; i < gridWidth; ++i)
+                {
+                    for (int j = 0; j < gridWidth; ++j)
+                    {
+                        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);
+                compoundBuilder.Reset();
+                var gridCompound = new Compound(gridChildren);
+                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);
+                    Simulation.Bodies.Add(bodyDescription);
+                }
+            }
+
+            //Build a table and use it for a couple of different tests. 
+            {
+                var legShape = new Box(0.2f, 1, 0.2f);
+                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 };
+                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 table = new Compound(tableChildren);
+                var tableDescription = BodyDescription.CreateDynamic(RigidPose.Identity, tableInertia, Simulation.Shapes.Add(table), 0.01f);
+
+                //Stack some tables.
+                {
+                    for (int i = 0; i < 10; ++i)
+                    {
+                        tableDescription.Pose.Position = new Vector3(10, 3 + i * 1.4f, 10);
+                        Simulation.Bodies.Add(tableDescription);
+                    }
+                }
+                {
+                    for (int k = 0; k < 5; ++k)
+                    {
+                        tableDescription.Pose.Position = new Vector3(64 + k * 3, 6 + k * 1.4f, 32);
+                        Simulation.Bodies.Add(tableDescription);
+                    }
+                    //for (int i = 0; i < 10; ++i)
+                    //{
+                    //    for (int j = 0; j < 20; ++j)
+                    //    {
+                    //        for (int k = 0; k < 10; ++k)
+                    //        {
+                    //            tableDescription.Pose.Position = new Vector3(32 + i * 6, 6 + j * 1.4f, 16 + k * 6);
+                    //            Simulation.Bodies.Add(tableDescription);
+                    //        }
+                    //    }
+                    //}
+                }
+
+                //Put a table on top of a sphere to stress out nonconvex reduction for divergent normals.
+                {
+                    tableDescription.Pose.Position = new Vector3(10, 6, 0);
+                    Simulation.Bodies.Add(tableDescription);
+
+                    var sphereShape = new Sphere(3);
+                    var sphereIndex = Simulation.Shapes.Add(sphereShape);
+                    var sphereDescription = new StaticDescription(new Vector3(10, 2, 0), sphereIndex);
+                    Simulation.Statics.Add(sphereDescription);
+                }
+
+                //Put another table on the ground, but with a clamp-ish thing on it that generates opposing normals.
+                {
+                    tableDescription.Pose.Position = new Vector3(10, 3, -10);
+                    Simulation.Bodies.Add(tableDescription);
+
+                    var clampPieceShape = new Box(2f, 0.1f, 0.3f);
+                    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 };
+                    var clamp2 = new RigidPose { Position = new Vector3(0, -0.2f, 0), Orientation = Quaternion.Identity };
+                    var clamp3 = new RigidPose { Position = new Vector3(0, 0.2f, 0), Orientation = Quaternion.Identity };
+                    var clamp4 = new RigidPose { Position = new Vector3(0, -0.2f, 1.1f), Orientation = Quaternion.Identity };
+                    var clamp5 = new RigidPose { Position = new Vector3(0, 0.2f, 1.1f), Orientation = Quaternion.Identity };
+                    compoundBuilder.Add(clampPieceShapeIndex, clamp0, clampPieceInverseInertia.InverseInertiaTensor, 1);
+                    compoundBuilder.Add(clampPieceShapeIndex, clamp1, clampPieceInverseInertia.InverseInertiaTensor, 1);
+                    compoundBuilder.Add(clampPieceShapeIndex, clamp2, clampPieceInverseInertia.InverseInertiaTensor, 1);
+                    compoundBuilder.Add(clampPieceShapeIndex, clamp3, clampPieceInverseInertia.InverseInertiaTensor, 1);
+                    compoundBuilder.Add(clampPieceShapeIndex, clamp4, clampPieceInverseInertia.InverseInertiaTensor, 1);
+                    compoundBuilder.Add(clampPieceShapeIndex, clamp5, clampPieceInverseInertia.InverseInertiaTensor, 1);
+
+                    compoundBuilder.BuildDynamicCompound(out var clampChildren, out var clampInertia, out var clampCenter);
+                    compoundBuilder.Reset();
+                    var clamp = new Compound(clampChildren);
+                    var clampDescription = BodyDescription.CreateDynamic(
+                        tableDescription.Pose.Position + new Vector3(2f, 0.3f, 0), clampInertia, Simulation.Shapes.Add(clamp), 0.01f);
+                    Simulation.Bodies.Add(clampDescription);
+                }
+
+            }
+
+            //Create a tree-accelerated big compound.
+            {
+                var random = new Random(5);
+                var treeCompoundBoxShape = new Box(0.5f, 1.5f, 1f);
+                var treeCompoundBoxShapeIndex = Simulation.Shapes.Add(treeCompoundBoxShape);
+                var childInertia = treeCompoundBoxShape.ComputeInertia(1);
+                for (int i = 0; i < 128; ++i)
+                {
+                    RigidPose localPose;
+                    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(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 compound = new BigCompound(children, Simulation.Shapes, BufferPool);
+                //var compound = new Compound(children);
+                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, compoundIndex, 0.01f));
+                }
+            }
+        }
+
+        //Prevent stuff from falling into the infinite void.
+        {
+            var boxShape = new Box(256, 1, 256);
+            var groundShapeIndex = Simulation.Shapes.Add(boxShape);
+            var groundDescription = new StaticDescription(RigidPose.Identity, groundShapeIndex);
+            Simulation.Statics.Add(groundDescription);
+        }
+        const int planeWidth = 48;
+        const int planeHeight = 48;
+        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);
+        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);
+    }
+}
diff --git a/Demos/Demos/CompoundTestDemo.cs b/Demos/Demos/CompoundTestDemo.cs
deleted file mode 100644
index 8a22ab1a1..000000000
--- a/Demos/Demos/CompoundTestDemo.cs
+++ /dev/null
@@ -1,250 +0,0 @@
-using BepuUtilities;
-using DemoRenderer;
-using BepuPhysics;
-using BepuPhysics.Collidables;
-using System;
-using System.Numerics;
-using DemoContentLoader;
-
-namespace Demos.Demos
-{
-    public class CompoundTestDemo : Demo
-    {
-        public unsafe override void Initialize(ContentArchive content, Camera camera)
-        {
-            camera.Position = new Vector3(-13f, 6, -13f);
-            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 PositionFirstTimestepper());
-
-            using (var compoundBuilder = new CompoundBuilder(BufferPool, Simulation.Shapes, 8))
-            {
-                {
-                    //Note that, in bepuphysics v2, there is no 'recentering' when constructing a shape. The pose you pass in for a child is exactly the pose that the compound will use, 
-                    //even if the 'true' center of mass isn't at the local origin.
-                    //Instead, if recentering is desired, it should performed ahead of time. The CompoundBuilder can help with this.
-                    //We'll construct this compound using shapes far from the origin, and then use the CompoundBuilder overload that recenters the children and outputs the computed center.
-                    var capsuleChildShape = new Capsule(0.5f, 0.5f);
-                    var capsuleLocalPose = new RigidPose { Position = new Vector3(-0.5f, 4, 4), Orientation = Quaternion.Identity };
-                    var boxChildShape = new Box(0.5f, 1f, 1.5f);
-                    var boxLocalPose = new RigidPose { Position = new Vector3(0.5f, 4, 4), Orientation = Quaternion.Identity };
-
-                    //All allocations from the buffer pool used for the final compound shape will be disposed when the demo is disposed. Don't have to worry about leaks in these demos.
-                    compoundBuilder.Add(capsuleChildShape, capsuleLocalPose, 1);
-                    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)));
-                }
-
-                //Build a stack of sphere grids to stress manifold reduction heuristics in a convex-ish situation.
-                {
-                    var gridShape = new Sphere(0.5f);
-                    const float gridSpacing = 1.5f;
-                    const int gridWidth = 3;
-                    var gridShapeIndex = Simulation.Shapes.Add(gridShape);
-                    gridShape.ComputeInertia(1, out var gridBoxInertia);
-                    float localPoseOffset = -0.5f * gridSpacing * (gridWidth - 1);
-                    for (int i = 0; i < gridWidth; ++i)
-                    {
-                        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.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));
-                    for (int i = 0; i < 4; ++i)
-                    {
-                        bodyDescription.Pose.Position = new Vector3(0, 2 + i * 3, 0);
-                        Simulation.Bodies.Add(bodyDescription);
-                    }
-                }
-
-                //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 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 };
-                    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 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 = new RigidPose { Orientation = Quaternion.Identity }
-                    };
-
-                    //Stack some tables.
-                    {
-                        for (int i = 0; i < 10; ++i)
-                        {
-                            tableDescription.Pose.Position = new Vector3(10, 3 + i * 1.4f, 10);
-                            Simulation.Bodies.Add(tableDescription);
-                        }
-                    }
-                    {
-                        for (int k = 0; k < 5; ++k)
-                        {
-                            tableDescription.Pose.Position = new Vector3(64 + k * 3, 6 + k * 1.4f, 32);
-                            Simulation.Bodies.Add(tableDescription);
-                        }
-                        //for (int i = 0; i < 10; ++i)
-                        //{
-                        //    for (int j = 0; j < 20; ++j)
-                        //    {
-                        //        for (int k = 0; k < 10; ++k)
-                        //        {
-                        //            tableDescription.Pose.Position = new Vector3(32 + i * 6, 6 + j * 1.4f, 16 + k * 6);
-                        //            Simulation.Bodies.Add(tableDescription);
-                        //        }
-                        //    }
-                        //}
-                    }
-
-                    //Put a table on top of a sphere to stress out nonconvex reduction for divergent normals.
-                    {
-                        tableDescription.Pose.Position = new Vector3(10, 6, 0);
-                        Simulation.Bodies.Add(tableDescription);
-
-                        var sphereShape = new Sphere(3);
-                        var sphereIndex = Simulation.Shapes.Add(sphereShape);
-                        var sphereDescription = new StaticDescription
-                        {
-                            Collidable = new CollidableDescription
-                            {
-                                Shape = sphereIndex,
-                                SpeculativeMargin = 0.1f,
-                            },
-                            Pose = new RigidPose { Position = new Vector3(10, 2, 0), Orientation = Quaternion.Identity }
-                        };
-                        Simulation.Statics.Add(sphereDescription);
-                    }
-
-                    //Put another table on the ground, but with a clamp-ish thing on it that generates opposing normals.
-                    {
-                        tableDescription.Pose.Position = new Vector3(10, 3, -10);
-                        Simulation.Bodies.Add(tableDescription);
-
-                        var clampPieceShape = new Box(2f, 0.1f, 0.3f);
-                        clampPieceShape.ComputeInertia(1f, out var clampPieceInverseInertia);
-                        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 };
-                        var clamp2 = new RigidPose { Position = new Vector3(0, -0.2f, 0), Orientation = Quaternion.Identity };
-                        var clamp3 = new RigidPose { Position = new Vector3(0, 0.2f, 0), Orientation = Quaternion.Identity };
-                        var clamp4 = new RigidPose { Position = new Vector3(0, -0.2f, 1.1f), Orientation = Quaternion.Identity };
-                        var clamp5 = new RigidPose { Position = new Vector3(0, 0.2f, 1.1f), Orientation = Quaternion.Identity };
-                        compoundBuilder.Add(clampPieceShapeIndex, clamp0, clampPieceInverseInertia.InverseInertiaTensor, 1);
-                        compoundBuilder.Add(clampPieceShapeIndex, clamp1, clampPieceInverseInertia.InverseInertiaTensor, 1);
-                        compoundBuilder.Add(clampPieceShapeIndex, clamp2, clampPieceInverseInertia.InverseInertiaTensor, 1);
-                        compoundBuilder.Add(clampPieceShapeIndex, clamp3, clampPieceInverseInertia.InverseInertiaTensor, 1);
-                        compoundBuilder.Add(clampPieceShapeIndex, clamp4, clampPieceInverseInertia.InverseInertiaTensor, 1);
-                        compoundBuilder.Add(clampPieceShapeIndex, clamp5, clampPieceInverseInertia.InverseInertiaTensor, 1);
-
-                        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 }
-                        };
-                        Simulation.Bodies.Add(clampDescription);
-                    }
-
-                }
-
-                //Create a tree-accelerated big compound.
-                {
-                    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);
-                    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);
-                        float orientationLengthSquared;
-                        do
-                        {
-                            localPose.Orientation = new Quaternion((float)random.NextDouble(), (float)random.NextDouble(), (float)random.NextDouble(), (float)random.NextDouble());
-                            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 compound = new BigCompound(children, Simulation.Shapes, BufferPool);
-                    //var compound = new Compound(children);
-                    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)));
-                    }
-                }
-            }
-
-            //Prevent stuff from falling into the infinite void.
-            {
-                var boxShape = new Box(256, 1, 256);
-                var groundShapeIndex = Simulation.Shapes.Add(boxShape);
-                var groundDescription = new StaticDescription
-                {
-                    Collidable = new CollidableDescription
-                    {
-                        Shape = groundShapeIndex,
-                        SpeculativeMargin = 0.1f,
-                    },
-                    Pose = new RigidPose { Position = new Vector3(0, 0, 0), Orientation = Quaternion.Identity }
-                };
-                Simulation.Statics.Add(groundDescription);
-            }
-            const int planeWidth = 48;
-            const int planeHeight = 48;
-            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);
-            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)));
-        }
-    }
-}
diff --git a/Demos/Demos/ContactEventsDemo.cs b/Demos/Demos/ContactEventsDemo.cs
index c1fbdc5f9..a0b5761c7 100644
--- a/Demos/Demos/ContactEventsDemo.cs
+++ b/Demos/Demos/ContactEventsDemo.cs
@@ -16,725 +16,771 @@
 using System.Threading;
 using System.Runtime.InteropServices;
 
-namespace Demos.Demos
+namespace Demos.Demos;
+
+//Bepuphysics v2 doesn't have any concept of 'events'. It has callbacks that report the current status of contact manifolds.
+//Events can be built around those callbacks. This demo shows one way of doing that.
+
+//It's worth noting a few things about this event handler approach:
+//1) It's a little nonidiomatic, since there are no actual C# 'events' being used. An interface is invoked instead. This preserves the generic manifold type.
+//You could shuffle things around a bit and invoke delegates for a more event-ish workflow, or even function pointers if you wanted to jump through some hoops.
+//2) All event handlers (except OnPairEnded) potentially execute from the multithreaded context of the simulation execution, so you have to be careful about what the event handlers do.
+//A "deferred" event model could be built on top of this.
+//3) Event handling isn't free; try to avoid attaching events to everything if you can avoid it, and try to make the event handlers as cheap as possible.
+//4) This provides no insight into the constraints associated with these contacts. For example, if you want to spawn particles in response to heavy collisions or play sounds
+//with volume dependent on the impact force, you will need to go pull impulse data from the solver. Further, those sorts of use cases often only apply to nearby objects,
+//so a listener based model isn't ideal. Instead, querying nearby active objects and examining their contact constraints would be much more direct.
+//5) Contacts can be created speculatively. A contact existing does not guarantee a nonnegative penetration depth!
+//Note that only manifolds with contacts with nonnegative depths are reported as 'touching' below.
+//6) On the flipside of #6, an impact is not guaranteed to always have a nonnegative penetration depth at some point!
+//Solving the constraints associated with speculative contacts could end up bouncing an object off a surface between two collision detection runs that both show negative depth.
+//7) There are other ways of pulling contact data. For example, check out the SolverContactEnumerationDemo which pulls data from the solver constraints created from contacts.
+//Pulling accumulated impulses from the solver is one way to never miss an actual collision; if the contact constraints applied any force, then there must have been an impact.
+//8) Pairs involving two sleeping bodies will not generate events. That means OnTouching and OnPairUpdated will not fire for sleeping pairs.
+
+/// <summary>
+/// Implements handlers for various collision events.
+/// </summary>
+public interface IContactEventHandler
 {
-    //Bepuphysics v2 doesn't have any concept of 'events'. It has callbacks that report the current status of contact manifolds.
-    //Events can be built around those callbacks. This demo shows one way of doing that.
-
-    //It's worth noting a few things about this event handler approach:
-    //1) It's a little nonidiomatic, since there are no actual C# 'events' being used. An interface is invoked instead. This preserves the generic manifold type.
-    //You could shuffle things around a bit and invoke delegates for a more event-ish workflow, or even function pointers if you wanted to jump through some hoops.
-    //2) All event handlers (except OnPairEnded) potentially execute from the multithreaded context of the simulation execution, so you have to be careful about what the event handlers do.
-    //A "deferred" event model could be built on top of this.
-    //3) Event handling isn't free; try to avoid attaching events to everything if you can avoid it, and try to make the event handlers as cheap as possible.
-    //4) This provides no insight into the constraints associated with these contacts. For example, if you want to spawn particles in response to heavy collisions or play sounds
-    //with volume dependent on the impact force, you will need to go pull impulse data from the solver. Further, those sorts of use cases often only apply to nearby objects,
-    //so a listener based model isn't ideal. Instead, querying nearby active objects and examining their contact constraints would be much more direct.
-    //5) Contacts can be created speculatively. A contact existing does not guarantee a nonnegative penetration depth!
-    //Note that only manifolds with contacts with nonnegative depths are reported as 'touching' below.
-    //6) On the flipside of #6, an impact is not guaranteed to always have a nonnegative penetration depth at some point!
-    //Solving the constraints associated with speculative contacts could end up bouncing an object off a surface between two collision detection runs that both show negative depth.
-    //7) There are other ways of pulling contact data. For example, check out the SolverContactEnumerationDemo which pulls data from the solver constraints created from contacts.
-    //Pulling accumulated impulses from the solver is one way to never miss an actual collision; if the contact constraints applied any force, then there must have been an impact.
-    //8) Pairs involving two sleeping bodies will not generate events. That means OnTouching and OnPairUpdated will not fire for sleeping pairs.
+    /// <summary>
+    /// Fires when a contact is added.
+    /// </summary>
+    /// <typeparam name="TManifold">Type of the contact manifold detected.</typeparam>
+    /// <param name="eventSource">Collidable that the event was attached to.</param>
+    /// <param name="pair">Collidable pair triggering the event.</param>
+    /// <param name="contactManifold">Set of remaining contacts in the collision.</param>
+    /// <param name="contactOffset">Offset from the pair's local origin to the new contact.</param>
+    /// <param name="contactNormal">Normal of the new contact.</param>
+    /// <param name="depth">Depth of the new contact.</param>
+    /// <param name="featureId">Feature id of the new contact.</param>
+    /// <param name="contactIndex">Index of the new contact in the contact manifold.</param>
+    /// <param name="workerIndex">Index of the worker thread that fired this event.</param>
+    void OnContactAdded<TManifold>(CollidableReference eventSource, CollidablePair pair, ref TManifold contactManifold,
+        Vector3 contactOffset, Vector3 contactNormal, float depth, int featureId, int contactIndex, int workerIndex) where TManifold : unmanaged, IContactManifold<TManifold>
+    {
+    }
 
     /// <summary>
-    /// Implements handlers for various collision events.
+    /// Fires when a contact is removed.
     /// </summary>
-    public interface IContactEventHandler
-    {
-        /// <summary>
-        /// Fires when a contact is added.
-        /// </summary>
-        /// <typeparam name="TManifold">Type of the contact manifold detected.</typeparam>
-        /// <param name="eventSource">Collidable that the event was attached to.</param>
-        /// <param name="pair">Collidable pair triggering the event.</param>
-        /// <param name="contactManifold">Set of remaining contacts in the collision.</param>
-        /// <param name="contactOffset">Offset from the pair's local origin to the new contact.</param>
-        /// <param name="contactNormal">Normal of the new contact.</param>
-        /// <param name="depth">Depth of the new contact.</param>
-        /// <param name="featureId">Feature id of the new contact.</param>
-        /// <param name="contactIndex">Index of the new contact in the contact manifold.</param>
-        /// <param name="workerIndex">Index of the worker thread that fired this event.</param>
-        void OnContactAdded<TManifold>(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<TManifold>
-        {
-        }
+    /// <typeparam name="TManifold">Type of the contact manifold detected.</typeparam>
+    /// <param name="eventSource">Collidable that the event was attached to.</param>
+    /// <param name="pair">Collidable pair triggering the event.</param>
+    /// <param name="contactManifold">Set of remaining contacts in the collision.</param>
+    /// <param name="removedFeatureId">Feature id of the contact that was removed and is no longer present in the contact manifold.</param>
+    /// <param name="workerIndex">Index of the worker thread that fired this event.</param>
+    void OnContactRemoved<TManifold>(CollidableReference eventSource, CollidablePair pair, ref TManifold contactManifold, int removedFeatureId, int workerIndex) where TManifold : unmanaged, IContactManifold<TManifold>
+    {
+    }
 
-        /// <summary>
-        /// Fires when a contact is removed.
-        /// </summary>
-        /// <typeparam name="TManifold">Type of the contact manifold detected.</typeparam>
-        /// <param name="eventSource">Collidable that the event was attached to.</param>
-        /// <param name="pair">Collidable pair triggering the event.</param>
-        /// <param name="contactManifold">Set of remaining contacts in the collision.</param>
-        /// <param name="removedFeatureId">Feature id of the contact that was removed and is no longer present in the contact manifold.</param>
-        /// <param name="workerIndex">Index of the worker thread that fired this event.</param>
-        void OnContactRemoved<TManifold>(CollidableReference eventSource, CollidablePair pair, ref TManifold contactManifold, int removedFeatureId, int workerIndex) where TManifold : unmanaged, IContactManifold<TManifold>
-        {
-        }
+    /// <summary>
+    /// Fires the first time a pair is observed to be touching. Touching means that there are contacts with nonnegative depths in the manifold.
+    /// </summary>
+    /// <typeparam name="TManifold">Type of the contact manifold detected.</typeparam>
+    /// <param name="eventSource">Collidable that the event was attached to.</param>
+    /// <param name="pair">Collidable pair triggering the event.</param>
+    /// <param name="contactManifold">Set of remaining contacts in the collision.</param>
+    /// <param name="workerIndex">Index of the worker thread that fired this event.</param>
+    void OnStartedTouching<TManifold>(CollidableReference eventSource, CollidablePair pair, ref TManifold contactManifold, int workerIndex) where TManifold : unmanaged, IContactManifold<TManifold>
+    {
+    }
 
-        /// <summary>
-        /// Fires the first time a pair is observed to be touching. Touching means that there are contacts with nonnegative depths in the manifold.
-        /// </summary>
-        /// <typeparam name="TManifold">Type of the contact manifold detected.</typeparam>
-        /// <param name="eventSource">Collidable that the event was attached to.</param>
-        /// <param name="pair">Collidable pair triggering the event.</param>
-        /// <param name="contactManifold">Set of remaining contacts in the collision.</param>
-        /// <param name="workerIndex">Index of the worker thread that fired this event.</param>
-        void OnStartedTouching<TManifold>(CollidableReference eventSource, CollidablePair pair, ref TManifold contactManifold, int workerIndex) where TManifold : unmanaged, IContactManifold<TManifold>
-        {
-        }
+    /// <summary>
+    /// Fires whenever a pair is observed to be touching. Touching means that there are contacts with nonnegative depths in the manifold. Will not fire for sleeping pairs.
+    /// </summary>
+    /// <typeparam name="TManifold">Type of the contact manifold detected.</typeparam>
+    /// <param name="eventSource">Collidable that the event was attached to.</param>
+    /// <param name="pair">Collidable pair triggering the event.</param>
+    /// <param name="contactManifold">Set of remaining contacts in the collision.</param>
+    /// <param name="workerIndex">Index of the worker thread that fired this event.</param>
+    void OnTouching<TManifold>(CollidableReference eventSource, CollidablePair pair, ref TManifold contactManifold, int workerIndex) where TManifold : unmanaged, IContactManifold<TManifold>
+    {
+    }
 
-        /// <summary>
-        /// Fires whenever a pair is observed to be touching. Touching means that there are contacts with nonnegative depths in the manifold. Will not fire for sleeping pairs.
-        /// </summary>
-        /// <typeparam name="TManifold">Type of the contact manifold detected.</typeparam>
-        /// <param name="eventSource">Collidable that the event was attached to.</param>
-        /// <param name="pair">Collidable pair triggering the event.</param>
-        /// <param name="contactManifold">Set of remaining contacts in the collision.</param>
-        /// <param name="workerIndex">Index of the worker thread that fired this event.</param>
-        void OnTouching<TManifold>(CollidableReference eventSource, CollidablePair pair, ref TManifold contactManifold, int workerIndex) where TManifold : unmanaged, IContactManifold<TManifold>
-        {
-        }
 
+    /// <summary>
+    /// Fires when a pair stops touching. Touching means that there are contacts with nonnegative depths in the manifold.
+    /// </summary>
+    /// <typeparam name="TManifold">Type of the contact manifold detected.</typeparam>
+    /// <param name="eventSource">Collidable that the event was attached to.</param>
+    /// <param name="pair">Collidable pair triggering the event.</param>
+    /// <param name="contactManifold">Set of remaining contacts in the collision.</param>
+    /// <param name="workerIndex">Index of the worker thread that fired this event.</param>
+    void OnStoppedTouching<TManifold>(CollidableReference eventSource, CollidablePair pair, ref TManifold contactManifold, int workerIndex) where TManifold : unmanaged, IContactManifold<TManifold>
+    {
+    }
 
-        /// <summary>
-        /// Fires when a pair stops touching. Touching means that there are contacts with nonnegative depths in the manifold.
-        /// </summary>
-        /// <typeparam name="TManifold">Type of the contact manifold detected.</typeparam>
-        /// <param name="eventSource">Collidable that the event was attached to.</param>
-        /// <param name="pair">Collidable pair triggering the event.</param>
-        /// <param name="contactManifold">Set of remaining contacts in the collision.</param>
-        /// <param name="workerIndex">Index of the worker thread that fired this event.</param>
-        void OnStoppedTouching<TManifold>(CollidableReference eventSource, CollidablePair pair, ref TManifold contactManifold, int workerIndex) where TManifold : unmanaged, IContactManifold<TManifold>
-        {
-        }
 
+    /// <summary>
+    /// Fires when a pair is observed for the first time.
+    /// </summary>
+    /// <typeparam name="TManifold">Type of the contact manifold detected.</typeparam>
+    /// <param name="eventSource">Collidable that the event was attached to.</param>
+    /// <param name="pair">Collidable pair triggering the event.</param>
+    /// <param name="contactManifold">Set of remaining contacts in the collision.</param>
+    /// <param name="workerIndex">Index of the worker thread that fired this event.</param>
+    void OnPairCreated<TManifold>(CollidableReference eventSource, CollidablePair pair, ref TManifold contactManifold, int workerIndex) where TManifold : unmanaged, IContactManifold<TManifold>
+    {
+    }
 
-        /// <summary>
-        /// Fires when a pair is observed for the first time.
-        /// </summary>
-        /// <typeparam name="TManifold">Type of the contact manifold detected.</typeparam>
-        /// <param name="eventSource">Collidable that the event was attached to.</param>
-        /// <param name="pair">Collidable pair triggering the event.</param>
-        /// <param name="contactManifold">Set of remaining contacts in the collision.</param>
-        /// <param name="workerIndex">Index of the worker thread that fired this event.</param>
-        void OnPairCreated<TManifold>(CollidableReference eventSource, CollidablePair pair, ref TManifold contactManifold, int workerIndex) where TManifold : unmanaged, IContactManifold<TManifold>
-        {
-        }
+    /// <summary>
+    /// Fires whenever a pair is updated. Will not fire for sleeping pairs.
+    /// </summary>
+    /// <typeparam name="TManifold">Type of the contact manifold detected.</typeparam>
+    /// <param name="eventSource">Collidable that the event was attached to.</param>
+    /// <param name="pair">Collidable pair triggering the event.</param>
+    /// <param name="contactManifold">Set of remaining contacts in the collision.</param>
+    /// <param name="workerIndex">Index of the worker thread that fired this event.</param>
+    void OnPairUpdated<TManifold>(CollidableReference eventSource, CollidablePair pair, ref TManifold contactManifold, int workerIndex) where TManifold : unmanaged, IContactManifold<TManifold>
+    {
+    }
 
-        /// <summary>
-        /// Fires whenever a pair is updated. Will not fire for sleeping pairs.
-        /// </summary>
-        /// <typeparam name="TManifold">Type of the contact manifold detected.</typeparam>
-        /// <param name="eventSource">Collidable that the event was attached to.</param>
-        /// <param name="pair">Collidable pair triggering the event.</param>
-        /// <param name="contactManifold">Set of remaining contacts in the collision.</param>
-        /// <param name="workerIndex">Index of the worker thread that fired this event.</param>
-        void OnPairUpdated<TManifold>(CollidableReference eventSource, CollidablePair pair, ref TManifold contactManifold, int workerIndex) where TManifold : unmanaged, IContactManifold<TManifold>
-        {
-        }
+    /// <summary>
+    /// Fires when a pair ends.
+    /// </summary>
+    /// <typeparam name="TManifold">Type of the contact manifold detected.</typeparam>
+    /// <param name="eventSource">Collidable that the event was attached to.</param>
+    /// <param name="pair">Collidable pair triggering the event.</param>
+    void OnPairEnded(CollidableReference eventSource, CollidablePair pair)
+    {
+    }
+}
 
-        /// <summary>
-        /// Fires when a pair ends.
-        /// </summary>
-        /// <typeparam name="TManifold">Type of the contact manifold detected.</typeparam>
-        /// <param name="eventSource">Collidable that the event was attached to.</param>
-        /// <param name="pair">Collidable pair triggering the event.</param>
-        void OnPairEnded(CollidableReference eventSource, CollidablePair pair)
-        {
-        }
+/// <summary>
+/// Watches a set of bodies and statics for contact changes and reports events.
+/// </summary>
+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)]
+    struct PreviousCollision
+    {
+        public CollidableReference Collidable;
+        public bool Fresh;
+        public bool WasTouching;
+        public int ContactCount;
+        //FeatureIds are identifiers encoding what features on the involved shapes contributed to the contact. We store up to 4 feature ids, one for each potential contact.
+        //A "feature" is things like a face, vertex, or edge. There is no single interpretation for what a feature is- the mapping is defined on a per collision pair level.
+        //In this demo, we only care to check whether a given contact in the current frame maps onto a contact from a previous frame.
+        //We can use this to only emit 'contact added' events when a new contact with an unrecognized id is reported.
+        public int FeatureId0;
+        public int FeatureId1;
+        public int FeatureId2;
+        public int FeatureId3;
     }
 
+    Simulation simulation;
+    IThreadDispatcher threadDispatcher;
+    BufferPool pool;
+
+    //We'll use a handle->index mapping in a CollidableProperty to point at our contiguously stored listeners (in the later listeners array).
+    //Note that there's also IndexSets for the statics and bodies; those will be checked first before accessing the listenerIndices.
+    //The CollidableProperty is quite barebones- it doesn't try to stop all invalid accesses, and the backing memory isn't guaranteed to be zero initialized.
+    //IndexSets are tightly bitpacked and are cheap to access, so they're an easy way to check if a collidable can trigger an event before doing any further processing.
+    CollidableProperty<int> listenerIndices;
+    IndexSet staticListenerFlags;
+    IndexSet bodyListenerFlags;
+    int listenerCount;
+
+    //For the purpose of this demo, we'll use some regular ol' interfaces rather than using the struct-implementing-interface for specialization.
+    //This array will be GC tracked as a result, but that should be mostly fine. If you've got hundreds of thousands of event handlers, you may want to consider alternatives.
+    struct Listener
+    {
+        public CollidableReference Source;
+        public IContactEventHandler Handler;
+        public QuickList<PreviousCollision> PreviousCollisions;
+    }
+    Listener[] listeners;
+
+    //The callbacks are invoked from a multithreaded context, and we don't know how many pairs will exist. 
+    //Rather than attempting to synchronize all accesses, every worker thread spits out the results into a worker-local list to be processed later by the main thread flush.
+    struct PendingWorkerAdd
+    {
+        public int ListenerIndex;
+        public PreviousCollision Collision;
+    }
+    QuickList<PendingWorkerAdd>[] pendingWorkerAdds;
+
     /// <summary>
-    /// Watches a set of bodies and statics for contact changes and reports events.
+    /// Creates a new contact events stream.
     /// </summary>
-    public unsafe class ContactEvents : IDisposable
+    /// <param name="threadDispatcher">Thread dispatcher to pull per-thread buffer pools from, if any.</param>
+    /// <param name="pool">Buffer pool used to manage resources internally. If null, the simulation's pool will be used.</param>
+    /// <param name="initialListenerCapacity">Number of listeners to allocate space for initially.</param>
+    public ContactEvents(IThreadDispatcher threadDispatcher = null, BufferPool pool = null, int initialListenerCapacity = 64)
     {
-        //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)]
-        struct PreviousCollision
-        {
-            public CollidableReference Collidable;
-            public bool Fresh;
-            public bool WasTouching;
-            public int ContactCount;
-            //FeatureIds are identifiers encoding what features on the involved shapes contributed to the contact. We store up to 4 feature ids, one for each potential contact.
-            //A "feature" is things like a face, vertex, or edge. There is no single interpretation for what a feature is- the mapping is defined on a per collision pair level.
-            //In this demo, we only care to check whether a given contact in the current frame maps onto a contact from a previous frame.
-            //We can use this to only emit 'contact added' events when a new contact with an unrecognized id is reported.
-            public int FeatureId0;
-            public int FeatureId1;
-            public int FeatureId2;
-            public int FeatureId3;
-        }
+        this.threadDispatcher = threadDispatcher;
+        this.pool = pool;
+        listeners = new Listener[initialListenerCapacity];
+    }
 
-        Simulation simulation;
-        IThreadDispatcher threadDispatcher;
-        BufferPool pool;
-
-        //We'll use a handle->index mapping in a CollidableProperty to point at our contiguously stored listeners (in the later listeners array).
-        //Note that there's also IndexSets for the statics and bodies; those will be checked first before accessing the listenerIndices.
-        //The CollidableProperty is quite barebones- it doesn't try to stop all invalid accesses, and the backing memory isn't guaranteed to be zero initialized.
-        //IndexSets are tightly bitpacked and are cheap to access, so they're an easy way to check if a collidable can trigger an event before doing any further processing.
-        CollidableProperty<int> listenerIndices;
-        IndexSet staticListenerFlags;
-        IndexSet bodyListenerFlags;
-        int listenerCount;
-
-        //For the purpose of this demo, we'll use some regular ol' interfaces rather than using the struct-implementing-interface for specialization.
-        //This array will be GC tracked as a result, but that should be mostly fine. If you've got hundreds of thousands of event handlers, you may want to consider alternatives.
-        struct Listener
-        {
-            public CollidableReference Source;
-            public bool WasAwake;
-            public IContactEventHandler Handler;
-            public QuickList<PreviousCollision> PreviousCollisions;
-        }
-        Listener[] listeners;
+    BufferPool GetPoolForWorker(int workerIndex)
+    {
+        return threadDispatcher == null ? pool : threadDispatcher.WorkerPools[workerIndex];
+    }
 
-        //The callbacks are invoked from a multithreaded context, and we don't know how many pairs will exist. 
-        //Rather than attempting to synchronize all accesses, every worker thread spits out the results into a worker-local list to be processed later by the main thread flush.
-        struct PendingWorkerAdd
-        {
-            public int ListenerIndex;
-            public PreviousCollision Collision;
-        }
-        QuickList<PendingWorkerAdd>[] pendingWorkerAdds;
-
-        /// <summary>
-        /// Creates a new contact events stream.
-        /// </summary>
-        /// <param name="threadDispatcher">Thread dispatcher to pull per-thread buffer pools from, if any.</param>
-        /// <param name="pool">Buffer pool used to manage resources internally. If null, the simulation's pool will be used.</param>
-        /// <param name="initialListenerCapacity">Number of listeners to allocate space for initially.</param>
-        public ContactEvents(IThreadDispatcher threadDispatcher = null, BufferPool pool = null, int initialListenerCapacity = 64)
-        {
-            this.threadDispatcher = threadDispatcher;
-            this.pool = pool;
-            listeners = new Listener[initialListenerCapacity];
-        }
+    /// <summary>
+    /// Initializes the contact events system with a simulation.
+    /// </summary>
+    /// <param name="simulation">Simulation to use with the contact events demo.</param>
+    /// <remarks>The constructor and initialization are split because of how this class is expected to be used. 
+    /// It will be passed into a simulation's constructor as a part of its contact callbacks, so there is no simulation available at the time of construction.</remarks>
+    public void Initialize(Simulation simulation)
+    {
+        this.simulation = simulation;
+        if (pool == null)
+            pool = simulation.BufferPool;
+        simulation.Timestepper.BeforeCollisionDetection += SetFreshnessForCurrentActivityStatus;
+        listenerIndices = new CollidableProperty<int>(simulation, pool);
+        pendingWorkerAdds = new QuickList<PendingWorkerAdd>[threadDispatcher == null ? 1 : threadDispatcher.ThreadCount];
+    }
 
-        /// <summary>
-        /// Initializes the contact events system with a simulation.
-        /// </summary>
-        /// <param name="simulation">Simulation to use with the contact events demo.</param>
-        /// <remarks>The constructor and initialization are split because of how this class is expected to be used. 
-        /// It will be passed into a simulation's constructor as a part of its contact callbacks, so there is no simulation available at the time of construction.</remarks>
-        public void Initialize(Simulation simulation)
-        {
-            this.simulation = simulation;
-            if (pool == null)
-                pool = simulation.BufferPool;
-            listenerIndices = new CollidableProperty<int>(simulation, pool);
-            pendingWorkerAdds = new QuickList<PendingWorkerAdd>[threadDispatcher == null ? 1 : threadDispatcher.ThreadCount];
-        }
+    /// <summary>
+    /// Begins listening for events related to the given collidable.
+    /// </summary>
+    /// <param name="collidable">Collidable to monitor for events.</param>
+    /// <param name="handler">Handlers to use for the collidable.</param>
+    public void Register(CollidableReference collidable, IContactEventHandler handler)
+    {
+        Debug.Assert(!IsListener(collidable), "Should only try to register listeners that weren't previously registered");
+        if (collidable.Mobility == CollidableMobility.Static)
+            staticListenerFlags.Add(collidable.RawHandleValue, pool);
+        else
+            bodyListenerFlags.Add(collidable.RawHandleValue, pool);
+        if (listenerCount >= listeners.Length)
+        {
+            Array.Resize(ref listeners, listeners.Length * 2);
+        }
+        //Note that allocations for the previous collision list are deferred until they actually exist.
+        listeners[listenerCount] = new Listener { Handler = handler, Source = collidable };
+        listenerIndices[collidable] = listenerCount;
+        ++listenerCount;
+    }
 
-        /// <summary>
-        /// Begins listening for events related to the given collidable.
-        /// </summary>
-        /// <param name="collidable">Collidable to monitor for events.</param>
-        /// <param name="handler">Handlers to use for the collidable.</param>
-        public void Register(CollidableReference collidable, IContactEventHandler handler)
-        {
-            Debug.Assert(!IsListener(collidable), "Should only try to register listeners that weren't previously registered");
-            if (collidable.Mobility == CollidableMobility.Static)
-                staticListenerFlags.Add(collidable.RawHandleValue, pool);
-            else
-                bodyListenerFlags.Add(collidable.RawHandleValue, pool);
-            if (listenerCount > listeners.Length)
-            {
-                Array.Resize(ref listeners, listeners.Length * 2);
-            }
-            //Note that allocations for the previous collision list are deferred until they actually exist.
-            listeners[listenerCount] = new Listener { Handler = handler, Source = collidable };
-            listenerIndices[collidable] = listenerCount;
-            ++listenerCount;
-        }
+    /// <summary>
+    /// Begins listening for events related to the given body.
+    /// </summary>
+    /// <param name="body">Body to monitor for events.</param>
+    /// <param name="handler">Handlers to use for the body.</param>
+    public void Register(BodyHandle body, IContactEventHandler handler)
+    {
+        Register(simulation.Bodies[body].CollidableReference, handler);
+    }
+
+    /// <summary>
+    /// Begins listening for events related to the given static.
+    /// </summary>
+    /// <param name="staticHandle">Static to monitor for events.</param>
+    /// <param name="handler">Handlers to use for the static.</param>
+    public void Register(StaticHandle staticHandle, IContactEventHandler handler)
+    {
+        Register(new CollidableReference(staticHandle), handler);
+    }
 
-        /// <summary>
-        /// Begins listening for events related to the given body.
-        /// </summary>
-        /// <param name="body">Body to monitor for events.</param>
-        /// <param name="handler">Handlers to use for the body.</param>
-        public void Register(BodyHandle body, IContactEventHandler handler)
+    /// <summary>
+    /// Stops listening for events related to the given collidable.
+    /// </summary>
+    /// <param name="collidable">Collidable to stop listening for.</param>
+    public void Unregister(CollidableReference collidable)
+    {
+        Debug.Assert(IsListener(collidable), "Should only try to unregister listeners that actually exist.");
+        if (collidable.Mobility == CollidableMobility.Static)
         {
-            Register(simulation.Bodies.GetBodyReference(body).CollidableReference, handler);
+            staticListenerFlags.Remove(collidable.RawHandleValue);
         }
-
-        /// <summary>
-        /// Begins listening for events related to the given static.
-        /// </summary>
-        /// <param name="staticHandle">Static to monitor for events.</param>
-        /// <param name="handler">Handlers to use for the static.</param>
-        public void Register(StaticHandle staticHandle, IContactEventHandler handler)
+        else
         {
-            Register(new CollidableReference(staticHandle), handler);
+            bodyListenerFlags.Remove(collidable.RawHandleValue);
         }
-
-        /// <summary>
-        /// Stops listening for events related to the given collidable.
-        /// </summary>
-        /// <param name="collidable">Collidable to stop listening for.</param>
-        public void Unregister(CollidableReference collidable)
+        var index = listenerIndices[collidable];
+        --listenerCount;
+        ref var removedSlot = ref listeners[index];
+        if (removedSlot.PreviousCollisions.Span.Allocated)
+            removedSlot.PreviousCollisions.Dispose(pool);
+        ref var lastSlot = ref listeners[listenerCount];
+        if (index < listenerCount)
         {
-            Debug.Assert(IsListener(collidable), "Should only try to unregister listeners that actually exist.");
-            if (collidable.Mobility == CollidableMobility.Static)
-            {
-                staticListenerFlags.Remove(collidable.RawHandleValue);
-            }
-            else
-            {
-                bodyListenerFlags.Remove(collidable.RawHandleValue);
-            }
-            var index = listenerIndices[collidable];
-            --listenerCount;
-            ref var removedSlot = ref listeners[index];
-            if (removedSlot.PreviousCollisions.Span.Allocated)
-                removedSlot.PreviousCollisions.Dispose(pool);
-            ref var lastSlot = ref listeners[listenerCount];
-            if (index < listenerCount)
-            {
-                listenerIndices[lastSlot.Source] = index;
-                removedSlot = lastSlot;
-            }
-            lastSlot = default;
+            listenerIndices[lastSlot.Source] = index;
+            removedSlot = lastSlot;
         }
+        lastSlot = default;
+    }
 
-        /// <summary>
-        /// Stops listening for events related to the given body.
-        /// </summary>
-        /// <param name="body">Body to stop listening for.</param>
-        public void Unregister(BodyHandle body)
+    /// <summary>
+    /// Stops listening for events related to the given body.
+    /// </summary>
+    /// <param name="body">Body to stop listening for.</param>
+    public void Unregister(BodyHandle body)
+    {
+        Unregister(simulation.Bodies[body].CollidableReference);
+    }
+
+    /// <summary>
+    /// Stops listening for events related to the given static.
+    /// </summary>
+    /// <param name="staticHandle">Static to stop listening for.</param>
+    public void Unregister(StaticHandle staticHandle)
+    {
+        Unregister(new CollidableReference(staticHandle));
+    }
+
+    /// <summary>
+    /// Checks if a collidable is registered as a listener.
+    /// </summary>
+    /// <param name="collidable">Collidable to check.</param>
+    /// <returns>True if the collidable has been registered as a listener, false otherwise.</returns>
+    public bool IsListener(CollidableReference collidable)
+    {
+        if (collidable.Mobility == CollidableMobility.Static)
         {
-            Unregister(simulation.Bodies.GetBodyReference(body).CollidableReference);
+            return staticListenerFlags.Contains(collidable.RawHandleValue);
         }
-
-        /// <summary>
-        /// Stops listening for events related to the given static.
-        /// </summary>
-        /// <param name="staticHandle">Static to stop listening for.</param>
-        public void Unregister(StaticHandle staticHandle)
+        else
         {
-            Unregister(new CollidableReference(staticHandle));
+            return bodyListenerFlags.Contains(collidable.RawHandleValue);
         }
+    }
 
-        /// <summary>
-        /// Checks if a collidable is registered as a listener.
-        /// </summary>
-        /// <param name="collidable">Collidable to check.</param>
-        /// <returns>True if the collidable has been registered as a listener, false otherwise.</returns>
-        public bool IsListener(CollidableReference collidable)
-        {
-            if (collidable.Mobility == CollidableMobility.Static)
+    /// <summary>
+    /// 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.
+    /// </summary>
+    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)
             {
-                return staticListenerFlags.Contains(collidable.RawHandleValue);
+                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
             {
-                return bodyListenerFlags.Contains(collidable.RawHandleValue);
+                //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<TManifold>(ref PreviousCollision collision, ref TManifold manifold, bool isTouching) where TManifold : unmanaged, IContactManifold<TManifold>
+    [MethodImpl(MethodImplOptions.AggressiveInlining)]
+    void UpdatePreviousCollision<TManifold>(ref PreviousCollision collision, ref TManifold manifold, bool isTouching) where TManifold : unmanaged, IContactManifold<TManifold>
+    {
+        Debug.Assert(manifold.Count <= 4, "This demo was built on the assumption that nonconvex manifolds will have a maximum of four contacts, but that might have changed.");
+        //If the above assert gets hit because of a change to nonconvex manifold capacities, the packed feature id representation this uses will need to be updated.
+        //I very much doubt the nonconvex manifold will ever use more than 8 contacts, so addressing this wouldn't require much of a change.
+        for (int j = 0; j < manifold.Count; ++j)
         {
-            Debug.Assert(manifold.Count <= 4, "This demo was built on the assumption that nonconvex manifolds will have a maximum of four contacts, but that might have changed.");
-            //If the above assert gets hit because of a change to nonconvex manifold capacities, the packed feature id representation this uses will need to be updated.
-            //I very much doubt the nonconvex manifold will ever use more than 8 contacts, so addressing this wouldn't require much of a change.
-            for (int j = 0; j < manifold.Count; ++j)
-            {
-                Unsafe.Add(ref collision.FeatureId0, j) = manifold.GetFeatureId(j);
-            }
-            collision.ContactCount = manifold.Count;
-            collision.Fresh = true;
-            collision.WasTouching = isTouching;
+            Unsafe.Add(ref collision.FeatureId0, j) = manifold.GetFeatureId(j);
         }
+        collision.ContactCount = manifold.Count;
+        collision.Fresh = true;
+        collision.WasTouching = isTouching;
+    }
 
-        void HandleManifoldForCollidable<TManifold>(int workerIndex, CollidableReference source, CollidableReference other, CollidablePair pair, ref TManifold manifold) where TManifold : unmanaged, IContactManifold<TManifold>
+    void HandleManifoldForCollidable<TManifold>(int workerIndex, CollidableReference source, CollidableReference other, CollidablePair pair, ref TManifold manifold) where TManifold : unmanaged, IContactManifold<TManifold>
+    {
+        //The "source" refers to the object that an event handler was (potentially) attached to, so we look for listeners registered for it.
+        //(This function is called for both orders of the pair, so we'll catch listeners for either.)
+        if (IsListener(source))
         {
-            //The "source" refers to the object that an event handler was (potentially) attached to, so we look for listeners registered for it.
-            //(This function is called for both orders of the pair, so we'll catch listeners for either.)
-            if (IsListener(source))
+            var listenerIndex = listenerIndices[source];
+            //This collidable is registered. Is the opposing collidable present?
+            ref var listener = ref listeners[listenerIndex];
+
+            int previousCollisionIndex = -1;
+            bool isTouching = false;
+            for (int i = 0; i < listener.PreviousCollisions.Count; ++i)
             {
-                var listenerIndex = listenerIndices[source];
-                //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)
+                ref var collision = ref listener.PreviousCollisions[i];
+                //Since the 'Packed' field contains both the handle type (dynamic, kinematic, or static) and the handle index packed into a single bitfield, an equal value guarantees we are dealing with the same collidable.
+                if (collision.Collidable.Packed == other.Packed)
                 {
-                    ref var collision = ref listener.PreviousCollisions[i];
-                    //Since the 'Packed' field contains both the handle type (dynamic, kinematic, or static) and the handle index packed into a single bitfield, an equal value guarantees we are dealing with the same collidable.
-                    if (collision.Collidable.Packed == other.Packed)
+                    previousCollisionIndex = i;
+                    //This manifold is associated with an existing collision.
+                    //For every contact in the old collsion still present (by feature id), set a flag in this bitmask so we can know when a contact is removed.
+                    int previousContactsStillExist = 0;
+                    for (int contactIndex = 0; contactIndex < manifold.Count; ++contactIndex)
                     {
-                        previousCollisionIndex = i;
-                        //This manifold is associated with an existing collision.
-                        //For every contact in the old collsion still present (by feature id), set a flag in this bitmask so we can know when a contact is removed.
-                        int previousContactsStillExist = 0;
-                        for (int contactIndex = 0; contactIndex < manifold.Count; ++contactIndex)
+                        //We can check if each contact was already present in the previous frame by looking at contact feature ids. See the 'PreviousCollision' type for a little more info on FeatureIds.
+                        var featureId = manifold.GetFeatureId(contactIndex);
+                        var featureIdWasInPreviousCollision = false;
+                        for (int previousContactIndex = 0; previousContactIndex < collision.ContactCount; ++previousContactIndex)
                         {
-                            //We can check if each contact was already present in the previous frame by looking at contact feature ids. See the 'PreviousCollision' type for a little more info on FeatureIds.
-                            var featureId = manifold.GetFeatureId(contactIndex);
-                            var featureIdWasInPreviousCollision = false;
-                            for (int previousContactIndex = 0; previousContactIndex < collision.ContactCount; ++previousContactIndex)
+                            if (featureId == Unsafe.Add(ref collision.FeatureId0, previousContactIndex))
                             {
-                                if (featureId == Unsafe.Add(ref collision.FeatureId0, previousContactIndex))
-                                {
-                                    featureIdWasInPreviousCollision = true;
-                                    previousContactsStillExist |= 1 << previousContactIndex;
-                                    break;
-                                }
+                                featureIdWasInPreviousCollision = true;
+                                previousContactsStillExist |= 1 << previousContactIndex;
+                                break;
                             }
-                            if (!featureIdWasInPreviousCollision)
-                            {
-                                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)
-                                isTouching = true;
                         }
-                        if (previousContactsStillExist != (1 << collision.ContactCount) - 1)
+                        if (!featureIdWasInPreviousCollision)
                         {
-                            //At least one contact that used to exist no longer does.
-                            for (int previousContactIndex = 0; previousContactIndex < collision.ContactCount; ++previousContactIndex)
-                            {
-                                if ((previousContactsStillExist & (1 << previousContactIndex)) == 0)
-                                {
-                                    listener.Handler.OnContactRemoved(source, pair, ref manifold, Unsafe.Add(ref collision.FeatureId0, previousContactIndex), workerIndex);
-                                }
-                            }
+                            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 (!collision.WasTouching && isTouching)
-                        {
-                            listener.Handler.OnStartedTouching(source, pair, ref manifold, workerIndex);
-                        }
-                        else if (collision.WasTouching && !isTouching)
-                        {
-                            listener.Handler.OnStoppedTouching(source, pair, ref manifold, workerIndex);
-                        }
-                        if (isTouching)
+                        if (manifold.GetDepth(contactIndex) >= 0)
+                            isTouching = true;
+                    }
+                    if (previousContactsStillExist != (1 << collision.ContactCount) - 1)
+                    {
+                        //At least one contact that used to exist no longer does.
+                        for (int previousContactIndex = 0; previousContactIndex < collision.ContactCount; ++previousContactIndex)
                         {
-                            listener.Handler.OnTouching(source, pair, ref manifold, workerIndex);
+                            if ((previousContactsStillExist & (1 << previousContactIndex)) == 0)
+                            {
+                                listener.Handler.OnContactRemoved(source, pair, ref manifold, Unsafe.Add(ref collision.FeatureId0, previousContactIndex), workerIndex);
+                            }
                         }
-                        UpdatePreviousCollision(ref collision, ref manifold, isTouching);
-                        break;
                     }
-                }
-                if (previousCollisionIndex < 0)
-                {
-                    //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);
-                    ref var pendingAdd = ref addsforWorker.AllocateUnsafely();
-                    pendingAdd.ListenerIndex = listenerIndex;
-                    pendingAdd.Collision.Collidable = other;
-                    listener.Handler.OnPairCreated(source, pair, ref manifold, workerIndex);
-                    //Dispatch events for all contacts in this new manifold.
-                    for (int i = 0; i < manifold.Count; ++i)
+                    if (!collision.WasTouching && isTouching)
                     {
-                        manifold.GetContact(i, out var offset, out var normal, out var depth, out var featureId);
-                        listener.Handler.OnContactAdded(source, pair, ref manifold, offset, normal, depth, featureId, i, workerIndex);
-                        if (depth >= 0)
-                            isTouching = true;
+                        listener.Handler.OnStartedTouching(source, pair, ref manifold, workerIndex);
+                    }
+                    else if (collision.WasTouching && !isTouching)
+                    {
+                        listener.Handler.OnStoppedTouching(source, pair, ref manifold, workerIndex);
                     }
                     if (isTouching)
                     {
-                        listener.Handler.OnStartedTouching(source, pair, ref manifold, workerIndex);
                         listener.Handler.OnTouching(source, pair, ref manifold, workerIndex);
                     }
-                    UpdatePreviousCollision(ref pendingAdd.Collision, ref manifold, isTouching);
+                    UpdatePreviousCollision(ref collision, ref manifold, isTouching);
+                    break;
                 }
-                listener.Handler.OnPairUpdated(source, pair, ref manifold, workerIndex);
-
             }
-        }
+            if (previousCollisionIndex < 0)
+            {
+                //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), GetPoolForWorker(workerIndex));
+                ref var pendingAdd = ref addsforWorker.AllocateUnsafely();
+                pendingAdd.ListenerIndex = listenerIndex;
+                pendingAdd.Collision.Collidable = other;
+                listener.Handler.OnPairCreated(source, pair, ref manifold, workerIndex);
+                //Dispatch events for all contacts in this new manifold.
+                for (int i = 0; i < manifold.Count; ++i)
+                {
+                    manifold.GetContact(i, out var offset, out var normal, out var depth, out var featureId);
+                    listener.Handler.OnContactAdded(source, pair, ref manifold, offset, normal, depth, featureId, i, workerIndex);
+                    if (depth >= 0)
+                        isTouching = true;
+                }
+                if (isTouching)
+                {
+                    listener.Handler.OnStartedTouching(source, pair, ref manifold, workerIndex);
+                    listener.Handler.OnTouching(source, pair, ref manifold, workerIndex);
+                }
+                UpdatePreviousCollision(ref pendingAdd.Collision, ref manifold, isTouching);
+            }
+            listener.Handler.OnPairUpdated(source, pair, ref manifold, workerIndex);
 
-        [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public void HandleManifold<TManifold>(int workerIndex, CollidablePair pair, ref TManifold manifold) where TManifold : unmanaged, IContactManifold<TManifold>
-        {
-            HandleManifoldForCollidable(workerIndex, pair.A, pair.B, pair, ref manifold);
-            HandleManifoldForCollidable(workerIndex, pair.B, pair.A, pair, ref manifold);
         }
+    }
 
-        //For final events fired by the flush that still expect a manifold, we'll provide a special empty type.
-        struct EmptyManifold : IContactManifold<EmptyManifold>
-        {
-            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(); }
-        }
+    [MethodImpl(MethodImplOptions.AggressiveInlining)]
+    public void HandleManifold<TManifold>(int workerIndex, CollidablePair pair, ref TManifold manifold) where TManifold : unmanaged, IContactManifold<TManifold>
+    {
+        HandleManifoldForCollidable(workerIndex, pair.A, pair.B, pair, ref manifold);
+        HandleManifoldForCollidable(workerIndex, pair.B, pair.A, pair, ref manifold);
+    }
 
-        public void Flush()
-        {
-            //For simplicity, this is completely sequential. Note that it's technically possible to extract more parallelism, but the complexity cost is high and you would need
-            //very large numbers of events being processed to make it worth it.
+    //For final events fired by the flush that still expect a manifold, we'll provide a special empty type.
+    struct EmptyManifold : IContactManifold<EmptyManifold>
+    {
+        public int Count => 0;
+        public bool Convex => true;
+        //This type never has any contacts, so there's no need for any property grabbers.
+        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()
+    {
+        //For simplicity, this is completely sequential. Note that it's technically possible to extract more parallelism, but the complexity cost is high and you would need
+        //very large numbers of events being processed to make it worth it.
 
-            //Remove any stale collisions. Stale collisions are those which should have received a new manifold update but did not because the manifold is no longer active.
-            for (int i = 0; i < listenerCount; ++i)
+        //Remove any stale collisions. Stale collisions are those which should have received a new manifold update but did not because the manifold is no longer active.
+        for (int i = 0; i < listenerCount; ++i)
+        {
+            ref var listener = ref listeners[i];
+            //Note reverse order. We remove during iteration.
+            for (int j = listener.PreviousCollisions.Count - 1; j >= 0; --j)
             {
-                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];
+                if (!collision.Fresh)
                 {
-                    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)
+                    //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 pair = new CollidablePair(listener.Source, collision.Collidable);
-                        if (collision.ContactCount > 0)
+                        var emptyManifold = new EmptyManifold();
+                        for (int previousContactCount = 0; previousContactCount < collision.ContactCount; ++previousContactCount)
                         {
-                            var emptyManifold = new EmptyManifold();
-                            for (int previousContactCount = 0; previousContactCount < collision.ContactCount; ++previousContactCount)
-                            {
-                                listener.Handler.OnContactRemoved(listener.Source, pair, ref emptyManifold, Unsafe.Add(ref collision.FeatureId0, previousContactCount), 0);
-                            }
-                            if (collision.WasTouching)
-                                listener.Handler.OnStoppedTouching(listener.Source, pair, ref emptyManifold, 0);
-                        }
-                        listener.Handler.OnPairEnded(collision.Collidable, pair);
-                        //This collision was not updated since the last flush despite being active. It should be removed.
-                        listener.PreviousCollisions.FastRemoveAt(j);
-                        if (listener.PreviousCollisions.Count == 0)
-                        {
-                            listener.PreviousCollisions.Dispose(pool);
-                            listener.PreviousCollisions = default;
+                            listener.Handler.OnContactRemoved(listener.Source, pair, ref emptyManifold, Unsafe.Add(ref collision.FeatureId0, previousContactCount), 0);
                         }
+                        if (collision.WasTouching)
+                            listener.Handler.OnStoppedTouching(listener.Source, pair, ref emptyManifold, 0);
                     }
-                    else
+                    listener.Handler.OnPairEnded(collision.Collidable, pair);
+                    //This collision was not updated since the last flush despite being active. It should be removed.
+                    listener.PreviousCollisions.FastRemoveAt(j);
+                    if (listener.PreviousCollisions.Count == 0)
                     {
-                        collision.Fresh = false;
+                        listener.PreviousCollisions.Dispose(pool);
+                        listener.PreviousCollisions = default;
                     }
                 }
-            }
-
-            for (int i = 0; i < pendingWorkerAdds.Length; ++i)
-            {
-                ref var pendingAdds = ref pendingWorkerAdds[i];
-                for (int j = 0; j < pendingAdds.Count; ++j)
+                else
                 {
-                    ref var add = ref pendingAdds[j];
-                    ref var collisions = ref listeners[add.ListenerIndex].PreviousCollisions;
-                    //Ensure capacity will initialize the slot if necessary.
-                    collisions.EnsureCapacity(Math.Max(8, collisions.Count + 1), pool);
-                    collisions.AllocateUnsafely() = pendingAdds[j].Collision;
+                    collision.Fresh = false;
                 }
-                if (pendingAdds.Span.Allocated)
-                    pendingAdds.Dispose(threadDispatcher == null ? pool : threadDispatcher.GetThreadMemoryPool(i));
-                //We rely on zeroing out the count for lazy initialization.
-                pendingAdds = default;
             }
         }
 
-        public void Dispose()
+        for (int i = 0; i < pendingWorkerAdds.Length; ++i)
         {
-            bodyListenerFlags.Dispose(pool);
-            staticListenerFlags.Dispose(pool);
-            listenerIndices.Dispose();
-            for (int i = 0; i < pendingWorkerAdds.Length; ++i)
+            ref var pendingAdds = ref pendingWorkerAdds[i];
+            for (int j = 0; j < pendingAdds.Count; ++j)
             {
-                Debug.Assert(!pendingWorkerAdds[i].Span.Allocated, "The pending worker adds should have been disposed by the previous flush.");
+                ref var add = ref pendingAdds[j];
+                ref var collisions = ref listeners[add.ListenerIndex].PreviousCollisions;
+                //Ensure capacity will initialize the slot if necessary.
+                collisions.EnsureCapacity(Math.Max(8, collisions.Count + 1), pool);
+                collisions.AllocateUnsafely() = pendingAdds[j].Collision;
             }
+            if (pendingAdds.Span.Allocated)
+                pendingAdds.Dispose(GetPoolForWorker(i));
+            //We rely on zeroing out the count for lazy initialization.
+            pendingAdds = default;
         }
     }
 
-    //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 void Dispose()
     {
-        ContactEvents events;
-
-        public ContactEventCallbacks(ContactEvents events)
+        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)
         {
-            this.events = events;
+            Debug.Assert(!pendingWorkerAdds[i].Span.Allocated, "The pending worker adds should have been disposed by the previous flush.");
         }
+    }
+}
 
-        [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public bool AllowContactGeneration(int workerIndex, CollidableReference a, CollidableReference b)
-        {
-            return true;
-        }
+//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 struct ContactEventCallbacks : INarrowPhaseCallbacks
+{
+    ContactEvents events;
 
-        [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public bool AllowContactGeneration(int workerIndex, CollidablePair pair, int childIndexA, int childIndexB)
-        {
-            return true;
-        }
+    public ContactEventCallbacks(ContactEvents events)
+    {
+        this.events = events;
+    }
 
-        [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public unsafe bool ConfigureContactManifold<TManifold>(int workerIndex, CollidablePair pair, ref TManifold manifold, out PairMaterialProperties pairMaterial) where TManifold : unmanaged, IContactManifold<TManifold>
-        {
-            pairMaterial.FrictionCoefficient = 1f;
-            pairMaterial.MaximumRecoveryVelocity = 2f;
-            pairMaterial.SpringSettings = new SpringSettings(30, 1);
-            events.HandleManifold(workerIndex, pair, ref manifold);
-            return true;
-        }
+    [MethodImpl(MethodImplOptions.AggressiveInlining)]
+    public bool AllowContactGeneration(int workerIndex, CollidableReference a, CollidableReference b, ref float speculativeMargin)
+    {
+        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 AllowContactGeneration(int workerIndex, CollidablePair pair, int childIndexA, int childIndexB)
+    {
+        return true;
+    }
 
-        public void Initialize(Simulation simulation)
-        {
-            events.Initialize(simulation);
-        }
+    [MethodImpl(MethodImplOptions.AggressiveInlining)]
+    public bool ConfigureContactManifold<TManifold>(int workerIndex, CollidablePair pair, ref TManifold manifold, out PairMaterialProperties pairMaterial) where TManifold : unmanaged, IContactManifold<TManifold>
+    {
+        pairMaterial.FrictionCoefficient = 1f;
+        pairMaterial.MaximumRecoveryVelocity = 2f;
+        pairMaterial.SpringSettings = new SpringSettings(30, 1);
+        events.HandleManifold(workerIndex, pair, ref manifold);
+        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 Initialize(Simulation simulation)
+    {
+        events.Initialize(simulation);
     }
 
-    /// <summary>
-    /// Shows how to use the contact event handler above to respond to new collisions.
-    /// </summary>
-    public class ContactEventsDemo : Demo
+    public void Dispose()
     {
-        struct ContactResponseParticle
-        {
-            public Vector3 Position;
-            public float Age;
-            public Vector3 Normal;
-        }
+    }
 
-        class EventHandler : IContactEventHandler
-        {
-            public Simulation Simulation;
-            public BufferPool Pool;
-            public QuickList<ContactResponseParticle> Particles;
+}
 
-            public EventHandler(Simulation simulation, BufferPool pool)
-            {
-                Simulation = simulation;
-                Particles = new QuickList<ContactResponseParticle>(128, pool);
-                Pool = pool;
-            }
+/// <summary>
+/// Shows how to use the contact event handler above to respond to new collisions.
+/// </summary>
+public class ContactEventsDemo : Demo
+{
+    struct ContactResponseParticle
+    {
+        public Vector3 Position;
+        public float Age;
+        public Vector3 Normal;
+    }
 
-            public void OnContactAdded<TManifold>(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<TManifold>
-            {
-                //Simply ignore any particles beyond the allocated space.
-                var index = Interlocked.Increment(ref Particles.Count) - 1;
-                if (index < Particles.Span.Length)
-                {
-                    ref var particle = ref Particles[index];
-
-                    //Contact data is calibrated according to the order of the pair, so using A's position is important.
-                    particle.Position = contactOffset + (pair.A.Mobility == CollidableMobility.Static ?
-                        new StaticReference(pair.A.StaticHandle, Simulation.Statics).Pose.Position :
-                        new BodyReference(pair.A.BodyHandle, Simulation.Bodies).Pose.Position);
-                    particle.Age = 0;
-                    particle.Normal = contactNormal;
-                }
-            }
+    class EventHandler : IContactEventHandler
+    {
+        public Simulation Simulation;
+        public BufferPool Pool;
+        public QuickList<ContactResponseParticle> Particles;
 
-            public void Dispose()
+        public EventHandler(Simulation simulation, BufferPool pool)
+        {
+            Simulation = simulation;
+            Particles = new QuickList<ContactResponseParticle>(128, pool);
+            Pool = pool;
+        }
+
+        public void OnContactAdded<TManifold>(CollidableReference eventSource, CollidablePair pair, ref TManifold contactManifold,
+            Vector3 contactOffset, Vector3 contactNormal, float depth, int featureId, int contactIndex, int workerIndex) where TManifold : unmanaged, IContactManifold<TManifold>
+        {
+            //Simply ignore any particles beyond the allocated space.
+            var index = Interlocked.Increment(ref Particles.Count) - 1;
+            if (index < Particles.Span.Length)
             {
-                //In the demo we won't actually call this, since it's going to persist until the demo dies. At that point, the buffer pool will be dropped and all its allocations will be cleaned up anyway.
-                Particles.Dispose(Pool);
+                ref var particle = ref Particles[index];
+
+                //Contact data is calibrated according to the order of the pair, so using A's position is important.
+                particle.Position = contactOffset + (pair.A.Mobility == CollidableMobility.Static ?
+                    new StaticReference(pair.A.StaticHandle, Simulation.Statics).Pose.Position :
+                    new BodyReference(pair.A.BodyHandle, Simulation.Bodies).Pose.Position);
+                particle.Age = 0;
+                particle.Normal = contactNormal;
             }
+        }
 
+        public void Dispose()
+        {
+            //In the demo we won't actually call this, since it's going to persist until the demo dies. At that point, the buffer pool will be dropped and all its allocations will be cleaned up anyway.
+            Particles.Dispose(Pool);
         }
 
-        ContactEvents events;
-        EventHandler eventHandler;
+    }
 
+    ContactEvents events;
+    EventHandler eventHandler;
 
-        public override void Initialize(ContentArchive content, Camera camera)
-        {
-            camera.Position = new Vector3(0, 8, -20);
-            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());
-            eventHandler = new EventHandler(Simulation, BufferPool);
+    public override void Initialize(ContentArchive content, Camera camera)
+    {
+        camera.Position = new Vector3(0, 8, -20);
+        camera.Yaw = MathHelper.Pi;
 
-            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 = new ContactEvents(ThreadDispatcher, BufferPool);
+        Simulation = Simulation.Create(BufferPool, new ContactEventCallbacks(events), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(8, 1));
+        eventHandler = new EventHandler(Simulation, BufferPool);
 
-            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);
+        var listenedBody1 = Simulation.Bodies.Add(BodyDescription.CreateConvexDynamic(new Vector3(0, 5, 0), 1, Simulation.Shapes, new Box(1, 2, 3)));
+        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[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), 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)
-        {
-            base.Update(window, camera, input, dt);
-            //Can't forget to flush the events!
-            //base.Update includes a call to the Simulation.Timestep, which will have called the event handlers if necessary.
-            //Any newly observed collisions need to be pushed into the main storage, and any dead collisions need to be cleaned out.
-            events.Flush();
-
-            //Age and scoot the particles we created for new contacts for the animation.
-            ref var particles = ref eventHandler.Particles;
-            //The count was incremented across multiple threads; it may have gone beyond the buffer size. Ignore the extra.
-            if (particles.Count > particles.Span.Length)
-                particles.Count = particles.Span.Length;
-            for (int i = particles.Count - 1; i >= 0; --i)
+    public override void Update(Window window, Camera camera, Input input, float dt)
+    {
+        base.Update(window, camera, input, dt);
+        //Can't forget to flush the events!
+        //base.Update includes a call to the Simulation.Timestep, which will have called the event handlers if necessary.
+        //Any newly observed collisions need to be pushed into the main storage, and any dead collisions need to be cleaned out.
+        events.Flush();
+
+        //Age and scoot the particles we created for new contacts for the animation.
+        ref var particles = ref eventHandler.Particles;
+        //The count was incremented across multiple threads; it may have gone beyond the buffer size. Ignore the extra.
+        if (particles.Count > particles.Span.Length)
+            particles.Count = particles.Span.Length;
+        for (int i = particles.Count - 1; i >= 0; --i)
+        {
+            ref var particle = ref particles[i];
+            particle.Age += dt;
+            if (particle.Age > 0.7325f)
             {
-                ref var particle = ref particles[i];
-                particle.Age += dt;
-                if (particle.Age > 0.7325f)
-                {
-                    particles.FastRemoveAt(i);
-                }
-                else
-                {
-                    particle.Position += particle.Normal * (2 * dt);
-                }
-
+                particles.FastRemoveAt(i);
+            }
+            else
+            {
+                particle.Position += particle.Normal * (2 * dt);
             }
+
         }
+    }
 
-        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)
+    {
+        ref var particles = ref eventHandler.Particles;
+        for (int i = particles.Count - 1; i >= 0; --i)
         {
-            ref var particles = ref eventHandler.Particles;
-            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, ref pose, new Vector3(0, 1, 0));
-            }
-            base.Render(renderer, camera, input, text, font);
+            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("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."), 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);
+
+        base.Render(renderer, camera, input, text, font);
+    }
+
+    protected override void OnDispose()
+    {
+        base.OnDispose();
+        events.Dispose();
     }
 }
diff --git a/Demos/Demos/ContinuousCollisionDetectionDemo.cs b/Demos/Demos/ContinuousCollisionDetectionDemo.cs
index e23f2346c..8f2283b0f 100644
--- a/Demos/Demos/ContinuousCollisionDetectionDemo.cs
+++ b/Demos/Demos/ContinuousCollisionDetectionDemo.cs
@@ -8,102 +8,152 @@
 using DemoUtilities;
 using DemoRenderer.UI;
 
-namespace Demos.Demos
+namespace Demos.Demos;
+
+/// <summary>
+/// Shows how different kinds of continuous collision detection work.
+/// See the ContinuousCollisionDetection.md documentation for more information.
+/// </summary>
+public class ContinuousCollisionDetectionDemo : Demo
 {
-    public class ContinuousCollisionDetectionDemo : Demo
+    ConstraintHandle spinnerMotorDefaultA, spinnerMotorDefaultB, spinnerMotorSweepA, spinnerMotorSweepB;
+    RolloverInfo rolloverInfo;
+
+    ConstraintHandle BuildSpinner(Vector3 initialPosition, float rotationSpeed, float maximumSpeculativeMargin, ContinuousDetection continuousDetection)
     {
-        ConstraintHandle spinnerMotorA;
-        ConstraintHandle spinnerMotorB;
-        RolloverInfo rolloverInfo;
+        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);
+        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.
+        //The blades are extremely thin and spinning very quickly, so that kind of precision is helpful.
+        //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 for this type of use case, since collisions with a duration below the minimum progression duration may be missed entirely.
 
-        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 bladeShape = new Box(5, 0.01f, 1);
-            bladeShape.ComputeInertia(1, out var bladeInertia);
-            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.
-            //The blades are extremely thin and spinning very quickly, so that kind of precision is helpful.
-            //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.
-
-            //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)));
-            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) });
-        }
+        //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.)
+
+        //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, 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) });
+    }
 
-        public override void Initialize(ContentArchive content, Camera camera)
+    public override void Initialize(ContentArchive content, Camera camera)
+    {
+        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
+        //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 (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(new SpringSettings(120, 1), maximumRecoveryVelocity: 1f),
+            new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(8, 1));
+
+        var shape = new Box(1, 1, 1);
+        var inertia = shape.ComputeInertia(1);
+        var shapeIndex = Simulation.Shapes.Add(shape);
+        for (int i = 0; i < 10; ++i)
         {
-            camera.Position = new Vector3(0, 10, 40);
-            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
-            //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(120, 1) }, new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new PositionFirstTimestepper());
-
-            var shape = new Box(1, 1, 1);
-            shape.ComputeInertia(1, out var inertia);
-            var shapeIndex = Simulation.Shapes.Add(shape);
-            for (int i = 0; i < 10; ++i)
+            for (int j = 0; j < 10; ++j)
             {
-                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, 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.
-                    //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, 0.01f, ContinuousDetectionSettings.Continuous(1e-3f, 1e-2f)), new BodyActivityDescription(0.01f)));
-                }
-            }
-            rolloverInfo = new RolloverInfo();
-            rolloverInfo.Add(new Vector3(-12, 2, 0), "Discrete");
-            rolloverInfo.Add(new Vector3(12, 2, 0), "Continuous");
+                //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, 0.01f, ContinuousDetection.Discrete), 0.01f));
 
-            //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");
+                //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));
 
-            Simulation.Statics.Add(new StaticDescription(new Vector3(0, -5f, 0), new CollidableDescription(Simulation.Shapes.Add(new Box(300, 10, 300)), 0.1f)));
+                //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(17 + 2 * j, 100 + (i + j) * 2, -30 + i * 2), new Vector3(0, -150, 0), inertia,
+                    new(shapeIndex, 0.01f, ContinuousDetection.Continuous(1e-3f, 1e-2f)), 0.01f));
+            }
         }
+        rolloverInfo = new RolloverInfo();
+        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");
 
-        double time;
-        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);
-            time += dt;
-            base.Update(window, camera, input, dt);
-        }
+        //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, 0.2f, onlySpeculativeMargin);
+        spinnerMotorDefaultB = BuildSpinner(new Vector3(-10, 14, 0), 59, 0.2f, onlySpeculativeMargin);
+        rolloverInfo.Add(new Vector3(-15, 14, -5), "Unlimited speculative margin");
 
-        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 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))));
+    }
+
+    double time;
+    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) };
+        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);
+    }
+
+    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/CustomVoxelCollidableDemo.cs b/Demos/Demos/CustomVoxelCollidableDemo.cs
index 1c1dbf5d0..8f622907c 100644
--- a/Demos/Demos/CustomVoxelCollidableDemo.cs
+++ b/Demos/Demos/CustomVoxelCollidableDemo.cs
@@ -14,451 +14,450 @@
 using DemoUtilities;
 using System;
 using BepuPhysics.CollisionDetection.SweepTasks;
+using BepuPhysics.Constraints;
 
-namespace Demos.Demos
+namespace Demos.Demos;
+
+//There are a variety of types related to collision pairs designed to work with particular interfaces.
+//We don't want to make a bunch of special cases, so we'll implement some of the interfaces.
+//Note that we don't just use ICompoundShape, even though that would lessen the amount of special case work needed-
+//ICompoundShape is designed to work in a more general case where every child may have a unique shape.
+//Meshes and voxel sets don't behave this way- all children are a single type, hence 'homogeneous'.
+struct Voxels : IHomogeneousCompoundShape<Box, BoxWide>
 {
-    //There are a variety of types related to collision pairs designed to work with particular interfaces.
-    //We don't want to make a bunch of special cases, so we'll implement some of the interfaces.
-    //Note that we don't just use ICompoundShape, even though that would lessen the amount of special case work needed-
-    //ICompoundShape is designed to work in a more general case where every child may have a unique shape.
-    //Meshes and voxel sets don't behave this way- all children are a single type, hence 'homogeneous'.
-    struct Voxels : IHomogeneousCompoundShape<Box, BoxWide>
+    //Type ids should be unique across all shape types in a simulation.
+    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.
+    //If you wanted maximum efficiency for some specific use case- like a world containing
+    //~infinite numbers of voxels with very fast modifications- then you may want to consider an alternative.
+    //(The tree based version could still work if the data was organized into streamed chunks instead of 
+    //a single giant collidable, but it probably won't match a specialized structure that takes 
+    //advantage of regular voxel grids' unique properties.)
+    public Tree Tree;
+
+    /// <summary>
+    /// List of the voxels in the voxel set by three dimensional index.
+    /// </summary>
+    public QuickList<Vector3> VoxelIndices;
+
+    /// <summary>
+    /// Size of each individual voxel. Updating this value requires updating the tree leaf bounds and refitting.
+    /// </summary>
+    public Vector3 VoxelSize;
+
+    public readonly int ChildCount => VoxelIndices.Count;
+
+    public Voxels(QuickList<Vector3> voxelIndices, Vector3 voxelSize, BufferPool pool)
     {
-        //Type ids should be unique across all shape types in a simulation.
-        public readonly 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.
-        //If you wanted maximum efficiency for some specific use case- like a world containing
-        //~infinite numbers of voxels with very fast modifications- then you may want to consider an alternative.
-        //(The tree based version could still work if the data was organized into streamed chunks instead of 
-        //a single giant collidable, but it probably won't match a specialized structure that takes 
-        //advantage of regular voxel grids' unique properties.)
-        public Tree Tree;
-
-        /// <summary>
-        /// List of the voxels in the voxel set by three dimensional index.
-        /// </summary>
-        public QuickList<Vector3> VoxelIndices;
-
-        /// <summary>
-        /// Size of each individual voxel. Updating this value requires updating the tree leaf bounds and refitting.
-        /// </summary>
-        public Vector3 VoxelSize;
-
-        public readonly int ChildCount => VoxelIndices.Count;
-
-        public Voxels(QuickList<Vector3> voxelIndices, Vector3 voxelSize, BufferPool pool)
+        VoxelIndices = voxelIndices;
+        VoxelSize = voxelSize;
+        Tree = new Tree(pool, voxelIndices.Count);
+        //Could stackalloc here, but the assumption is that there could be quite a few voxels.
+        //Quite possible to overflow the stack, so we instead resort to heap allocation.
+        pool.Take(voxelIndices.Count, out Buffer<BoundingBox> bounds);
+        for (int i = 0; i < voxelIndices.Count; ++i)
         {
-            VoxelIndices = voxelIndices;
-            VoxelSize = voxelSize;
-            Tree = new Tree(pool, voxelIndices.Count);
-            //Could stackalloc here, but the assumption is that there could be quite a few voxels.
-            //Quite possible to overflow the stack, so we instead resort to heap allocation.
-            pool.Take(voxelIndices.Count, out Buffer<BoundingBox> bounds);
-            for (int i = 0; i < voxelIndices.Count; ++i)
-            {
-                ref var voxel = ref voxelIndices[i];
-                ref var voxelBounds = ref bounds[i];
-                //Note that the voxel scale is baked into the tree. That's different than the Mesh, which allows sharing the same tree across different scaled shapes.
-                //You could do something similar with the voxel set if you wanted to; check ou tthe Mesh for an example.
-                voxelBounds.Min = voxel * VoxelSize;
-                voxelBounds.Max = voxelBounds.Min + VoxelSize;
-            }
-            Tree.SweepBuild(pool, bounds);
-            pool.Return(ref bounds);
+            ref var voxel = ref voxelIndices[i];
+            ref var voxelBounds = ref bounds[i];
+            //Note that the voxel scale is baked into the tree. That's different than the Mesh, which allows sharing the same tree across different scaled shapes.
+            //You could do something similar with the voxel set if you wanted to; check ou tthe Mesh for an example.
+            voxelBounds.Min = voxel * VoxelSize;
+            voxelBounds.Max = voxelBounds.Min + VoxelSize;
         }
+        Tree.SweepBuild(pool, bounds);
+        pool.Return(ref bounds);
+    }
 
-        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<TShape>, compound shapes a CompoundShapeBatch<TShape>,
-            //and then we have the HomogeneousCompoundShapeBatch... I don't love this name, but it just means
-            //that every child of the compound shape has the same type. So a mesh is a 'homogeneous compound'
-            //because its children are all triangles. Likewise, a voxel set is too, because every child is just a box.
-            return new HomogeneousCompoundShapeBatch<Voxels, Box, BoxWide>(pool, initialCapacity);
-        }
+    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<TShape>, compound shapes a CompoundShapeBatch<TShape>,
+        //and then we have the HomogeneousCompoundShapeBatch... I don't love this name, but it just means
+        //that every child of the compound shape has the same type. So a mesh is a 'homogeneous compound'
+        //because its children are all triangles. Likewise, a voxel set is too, because every child is just a box.
+        return new HomogeneousCompoundShapeBatch<Voxels, Box, BoxWide>(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);
+        max = new Vector3(float.MinValue);
+        for (int i = 0; i < VoxelIndices.Count; ++i)
         {
-            Matrix3x3.CreateFromQuaternion(orientation, out var basis);
-            min = new Vector3(float.MaxValue);
-            max = new Vector3(float.MinValue);
-            for (int i = 0; i < VoxelIndices.Count; ++i)
-            {
-                var localVoxelPosition = (VoxelIndices[i] + new Vector3(0.5f)) * VoxelSize;
-                Matrix3x3.Transform(localVoxelPosition, basis, out var rotatedPosition);
-                min = Vector3.Min(rotatedPosition, min);
-                max = Vector3.Max(rotatedPosition, max);
-            }
-            //All children have the same shape and orientation, so we can simply expand the centroids bounding box.
-            var box = new Box(VoxelSize.X, VoxelSize.Y, VoxelSize.Z);
-            box.ComputeBounds(orientation, out var childLocalMin, out var childLocalMax);
-            min += childLocalMin;
-            max += childLocalMax;
+            var localVoxelPosition = (VoxelIndices[i] + new Vector3(0.5f)) * VoxelSize;
+            Matrix3x3.Transform(localVoxelPosition, basis, out var rotatedPosition);
+            min = Vector3.Min(rotatedPosition, min);
+            max = Vector3.Max(rotatedPosition, max);
         }
+        //All children have the same shape and orientation, so we can simply expand the centroids bounding box.
+        var box = new Box(VoxelSize.X, VoxelSize.Y, VoxelSize.Z);
+        box.ComputeBounds(orientation, out var childLocalMin, out var childLocalMax);
+        min += childLocalMin;
+        max += childLocalMax;
+    }
 
-        unsafe struct HitLeafTester<T> : IRayLeafTester where T : IShapeRayHitHandler
+    unsafe struct HitLeafTester<T> : IRayLeafTester where T : IShapeRayHitHandler
+    {
+        public QuickList<Vector3> VoxelIndices;
+        public Vector3 VoxelSize;
+        public Box VoxelShape;
+        public T HitHandler;
+        public Matrix3x3 Orientation;
+        public RayData OriginalRay;
+
+        public void TestLeaf(int leafIndex, RayData* ray, float* maximumT, BufferPool pool)
         {
-            public QuickList<Vector3> VoxelIndices;
-            public Vector3 VoxelSize;
-            public Box VoxelShape;
-            public T HitHandler;
-            public Matrix3x3 Orientation;
-            public RayData OriginalRay;
-
-            [MethodImpl(MethodImplOptions.AggressiveInlining)]
-            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(voxelIndex + new Vector3(0.5f) * VoxelSize, ray->Origin, ray->Direction, out var t, out var normal) && t <= *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)
-                {
-                    //Bring the ray normal back into world space.
-                    Matrix3x3.Transform(normal, Orientation, out normal);
-                    //Use the original ray to avoid leaking the fact that we were working in local space.
-                    HitHandler.OnRayHit(OriginalRay, ref *maximumT, t, normal, leafIndex);
-                }
+                //Bring the ray normal back into world space.
+                Matrix3x3.Transform(normal, Orientation, out normal);
+                //Use the original ray to avoid leaking the fact that we were working in local space.
+                HitHandler.OnRayHit(OriginalRay, ref *maximumT, t, normal, leafIndex);
             }
         }
+    }
 
-        /// <summary>
-        /// Casts a ray against the voxels. Executes a callback for every test candidate and every hit.
-        /// </summary>
-        /// <typeparam name="TRayHitHandler">Type of the callback to execute for every test candidate and hit.</typeparam>
-        /// <param name="pose">Pose of the voxels during the ray test.</param>
-        /// <param name="ray">Ray to test against the voxels.</param>
-        /// <param name="maximumT">Maximum length of the ray in units of the ray direction length.</param>
-        /// <param name="hitHandler">Callback to execute for every hit.</param>
-        public readonly unsafe void RayTest<TRayHitHandler>(in RigidPose pose, in RayData ray, ref float maximumT, ref TRayHitHandler hitHandler) where TRayHitHandler : struct, IShapeRayHitHandler
-        {
-            HitLeafTester<TRayHitHandler> leafTester;
-            leafTester.VoxelIndices = VoxelIndices;
-            leafTester.VoxelSize = VoxelSize;
-            leafTester.VoxelShape = new Box(VoxelSize.X, VoxelSize.Y, VoxelSize.Z);
-            leafTester.HitHandler = hitHandler;
-            Matrix3x3.CreateFromQuaternion(pose.Orientation, out leafTester.Orientation);
-            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);
-            //The leaf tester could have mutated the hit handler; copy it back over.
-            hitHandler = leafTester.HitHandler;
-        }
+    /// <summary>
+    /// Casts a ray against the voxels. Executes a callback for every test candidate and every hit.
+    /// </summary>
+    /// <typeparam name="TRayHitHandler">Type of the callback to execute for every test candidate and hit.</typeparam>
+    /// <param name="pose">Pose of the voxels during the ray test.</param>
+    /// <param name="ray">Ray to test against the voxels.</param>
+    /// <param name="maximumT">Maximum length of the ray in units of the ray direction length.</param>
+    /// <param name="hitHandler">Callback to execute for every hit.</param>
+    /// <param name="pool">Pool used for temporary allocations required by the test, if any.</param>
+    public readonly void RayTest<TRayHitHandler>(in RigidPose pose, in RayData ray, ref float maximumT, BufferPool pool, ref TRayHitHandler hitHandler) where TRayHitHandler : struct, IShapeRayHitHandler
+    {
+        HitLeafTester<TRayHitHandler> leafTester;
+        leafTester.VoxelIndices = VoxelIndices;
+        leafTester.VoxelSize = VoxelSize;
+        leafTester.VoxelShape = new Box(VoxelSize.X, VoxelSize.Y, VoxelSize.Z);
+        leafTester.HitHandler = hitHandler;
+        Matrix3x3.CreateFromQuaternion(pose.Orientation, out leafTester.Orientation);
+        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, pool, ref leafTester);
+        //The leaf tester could have mutated the hit handler; copy it back over.
+        hitHandler = leafTester.HitHandler;
+    }
 
-        //Some shapes take advantage of multiple simultaneous incoming rays to perform batch execution.
-        //That can improve performance quite a bit in some cases. We don't bother taking advantage of it here, but you could add it if you wanted!
-        /// <summary>
-        /// Casts a bunch of rays against the voxels at the same time, executing a callback for every test candidate and every hit.
-        /// </summary>
-        /// <typeparam name="TRayHitHandler">Type of the callback to execute for every ray test candidate and every hit.</typeparam>
-        /// <param name="pose">Pose of the voxels during the ray test.</param>
-        /// <param name="rays">Set of rays to cast against the voxels.</param>
-        /// <param name="hitHandler">Callbacks to execute.</param>
-        public readonly unsafe void RayTest<TRayHitHandler>(in RigidPose pose, ref RaySource rays, ref TRayHitHandler hitHandler) where TRayHitHandler : struct, IShapeRayHitHandler
+    //Some shapes take advantage of multiple simultaneous incoming rays to perform batch execution.
+    //That can improve performance quite a bit in some cases. We don't bother taking advantage of it here, but you could add it if you wanted!
+    /// <summary>
+    /// Casts a bunch of rays against the voxels at the same time, executing a callback for every test candidate and every hit.
+    /// </summary>
+    /// <typeparam name="TRayHitHandler">Type of the callback to execute for every ray test candidate and every hit.</typeparam>
+    /// <param name="pose">Pose of the voxels during the ray test.</param>
+    /// <param name="rays">Set of rays to cast against the voxels.</param>
+    /// <param name="hitHandler">Callbacks to execute.</param>
+    /// <param name="pool">Pool used for temporary allocations required by the test, if any.</param>
+    public readonly unsafe void RayTest<TRayHitHandler>(in RigidPose pose, ref RaySource rays, BufferPool pool, ref TRayHitHandler hitHandler) where TRayHitHandler : struct, IShapeRayHitHandler
+    {
+        HitLeafTester<TRayHitHandler> leafTester;
+        leafTester.VoxelIndices = VoxelIndices;
+        leafTester.VoxelSize = VoxelSize;
+        leafTester.VoxelShape = new Box(VoxelSize.X, VoxelSize.Y, VoxelSize.Z);
+        leafTester.HitHandler = hitHandler;
+        Matrix3x3.CreateFromQuaternion(pose.Orientation, out leafTester.Orientation);
+        Matrix3x3.Transpose(leafTester.Orientation, out var inverseOrientation);
+        for (int i = 0; i < rays.RayCount; ++i)
         {
-            HitLeafTester<TRayHitHandler> leafTester;
-            leafTester.VoxelIndices = VoxelIndices;
-            leafTester.VoxelSize = VoxelSize;
-            leafTester.VoxelShape = new Box(VoxelSize.X, VoxelSize.Y, VoxelSize.Z);
-            leafTester.HitHandler = hitHandler;
-            Matrix3x3.CreateFromQuaternion(pose.Orientation, out leafTester.Orientation);
-            Matrix3x3.Transpose(leafTester.Orientation, out var inverseOrientation);
-            for (int i = 0; i < rays.RayCount; ++i)
-            {
-                rays.GetRay(i, out var ray, out var maximumT);
-                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);
-            }
-            //The leaf tester could have mutated the hit handler; copy it back over.
-            hitHandler = leafTester.HitHandler;
+            rays.GetRay(i, out var ray, out var maximumT);
+            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, pool, ref leafTester);
         }
+        //The leaf tester could have mutated the hit handler; copy it back over.
+        hitHandler = leafTester.HitHandler;
+    }
 
-        [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public readonly void GetLocalChild(int childIndex, out Box childShape)
-        {
-            var halfSize = VoxelSize * 0.5f;
-            childShape.HalfWidth = halfSize.X;
-            childShape.HalfHeight = halfSize.Y;
-            childShape.HalfLength = halfSize.Z;
-        }
+    public readonly void GetLocalChild(int childIndex, out Box childShape)
+    {
+        var halfSize = VoxelSize * 0.5f;
+        childShape.HalfWidth = halfSize.X;
+        childShape.HalfHeight = halfSize.Y;
+        childShape.HalfLength = halfSize.Z;
+    }
 
-        [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        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);
-        }
+    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.
-            //We are to fill in the first lane and ignore the others.
-            var halfSize = VoxelSize * 0.5f;
-            GatherScatter.GetFirst(ref shapeWide.HalfWidth) = halfSize.X;
-            GatherScatter.GetFirst(ref shapeWide.HalfHeight) = halfSize.Y;
-            GatherScatter.GetFirst(ref shapeWide.HalfLength) = halfSize.Z;
-        }
+    public readonly void GetLocalChild(int childIndex, ref BoxWide shapeWide)
+    {
+        //This function provides a reference to a lane in an AOSOA structure.
+        //We are to fill in the first lane and ignore the others.
+        var halfSize = VoxelSize * 0.5f;
+        GatherScatter.GetFirst(ref shapeWide.HalfWidth) = halfSize.X;
+        GatherScatter.GetFirst(ref shapeWide.HalfHeight) = halfSize.Y;
+        GatherScatter.GetFirst(ref shapeWide.HalfLength) = halfSize.Z;
+    }
 
 
-        [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public readonly unsafe void FindLocalOverlaps<TOverlaps, TSubpairOverlaps>(ref Buffer<OverlapQueryForPair> pairs, BufferPool pool, Shapes shapes, ref TOverlaps overlaps)
-             where TOverlaps : struct, ICollisionTaskOverlaps<TSubpairOverlaps>
-             where TSubpairOverlaps : struct, ICollisionTaskSubpairOverlaps
+    public readonly unsafe void FindLocalOverlaps<TOverlaps, TSubpairOverlaps>(ref Buffer<OverlapQueryForPair> pairs, BufferPool pool, Shapes shapes, ref TOverlaps overlaps)
+         where TOverlaps : struct, ICollisionTaskOverlaps<TSubpairOverlaps>
+         where TSubpairOverlaps : struct, ICollisionTaskSubpairOverlaps
+    {
+        //Just traverse every child against the tree sequentially.
+        //This sequentializes a whole lot of cache misses. You could probably get some benefit out of traversing all pairs 'simultaneously'- that is, 
+        //using the fact that we have lots of independent queries to ensure the CPU always has something to do. But for the sake of this demo, we'll do it the simple way.
+
+        //All this enumerator does is take an overlap reported by the GetOverlaps function and add it to the overlaps list.
+        ShapeTreeOverlapEnumerator<TSubpairOverlaps> enumerator;
+        enumerator.Pool = pool;
+        for (int i = 0; i < pairs.Length; ++i)
         {
-            //Just traverse every child against the tree sequentially.
-            //This sequentializes a whole lot of cache misses. You could probably get some benefit out of traversing all pairs 'simultaneously'- that is, 
-            //using the fact that we have lots of independent queries to ensure the CPU always has something to do. But for the sake of this demo, we'll do it the simple way.
-
-            //All this enumerator does is take an overlap reported by the GetOverlaps function and add it to the overlaps list.
-            ShapeTreeOverlapEnumerator<TSubpairOverlaps> enumerator;
-            enumerator.Pool = pool;
-            for (int i = 0; i < pairs.Length; ++i)
-            {
-                ref var pair = ref pairs[i];
-                ref var voxelsSet = ref Unsafe.AsRef<Voxels>(pair.Container);
-                enumerator.Overlaps = Unsafe.AsPointer(ref overlaps.GetOverlapsForPair(i));
-                voxelsSet.Tree.GetOverlaps(pair.Min, pair.Max, ref enumerator);
-            }
+            ref var pair = ref pairs[i];
+            ref var voxelsSet = ref Unsafe.AsRef<Voxels>(pair.Container);
+            enumerator.Overlaps = Unsafe.AsPointer(ref overlaps.GetOverlapsForPair(i));
+            voxelsSet.Tree.GetOverlaps(pair.Min, pair.Max, pool, ref enumerator);
         }
+    }
 
-        public readonly unsafe void FindLocalOverlaps<TOverlaps>(in Vector3 min, in Vector3 max, in 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*.
-            //It will when C# allows it.
-            ShapeTreeSweepLeafTester<TOverlaps> enumerator;
-            enumerator.Pool = pool;
-            enumerator.Overlaps = overlaps;
-            Tree.Sweep(min, max, sweep, maximumT, ref enumerator);
-        }
+    public readonly void FindLocalOverlaps<TEnumerator>(Vector3 min, Vector3 max, BufferPool pool, Shapes shapes, ref TEnumerator enumerator)
+        where TEnumerator : IBreakableForEach<int>
+    {
+        Tree.GetOverlaps(min, max, pool, ref enumerator);
+    }
 
-        public void Dispose(BufferPool pool)
-        {
-            Tree.Dispose(pool);
-            VoxelIndices.Dispose(pool);
-        }
+    public readonly unsafe void FindLocalOverlaps<TOverlaps>(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*.
+        //It will when C# allows it.
+        ShapeTreeSweepLeafTester<TOverlaps> enumerator;
+        enumerator.Pool = pool;
+        enumerator.Overlaps = overlaps;
+        Tree.Sweep(min, max, sweep, maximumT, pool, ref enumerator);
     }
 
-    //"Continuations" tell the collision batcher what to do with the collision detection results after completing a batch.
-    //For simple convex-convex pairs in the usual narrow phase pipeline, they just report the manifold for constraint generation.
-    //For more complex types like compounds, there may be multiple subpairs, each with their own contact manifold.
-    //Those manifolds are combined in a postprocessing step. For most compounds, this is a "NonconvexReduction".
-    //There is also a "MeshReduction" which is a bit more involved- it tries to smooth out collisions at the boundaries of triangles to avoid bumps during sliding.
-    //For our voxel set, we'll just use the NonconvexReduction despite the fact that it'll allow bumps at shape boundaries during sliding.
-    //(I'll leave boundary smoothing for voxels as a not-easy exercise for the highly motivated reader.)
-    public struct ConvexVoxelsContinuations : IConvexCompoundContinuationHandler<NonconvexReduction>
+    public void Dispose(BufferPool pool)
     {
-        public CollisionContinuationType CollisionContinuationType => CollisionContinuationType.NonconvexReduction;
+        Tree.Dispose(pool);
+        VoxelIndices.Dispose(pool);
+    }
+}
 
-        [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public ref NonconvexReduction CreateContinuation<TCallbacks>(
-            ref CollisionBatcher<TCallbacks> collisionBatcher, int childCount, in BoundsTestedPair pair, in OverlapQueryForPair pairQuery, out int continuationIndex)
-            where TCallbacks : struct, ICollisionCallbacks
-        {
-            return ref collisionBatcher.NonconvexReductions.CreateContinuation(childCount, collisionBatcher.Pool, out continuationIndex);
-        }
+//"Continuations" tell the collision batcher what to do with the collision detection results after completing a batch.
+//For simple convex-convex pairs in the usual narrow phase pipeline, they just report the manifold for constraint generation.
+//For more complex types like compounds, there may be multiple subpairs, each with their own contact manifold.
+//Those manifolds are combined in a postprocessing step. For most compounds, this is a "NonconvexReduction".
+//There is also a "MeshReduction" which is a bit more involved- it tries to smooth out collisions at the boundaries of triangles to avoid bumps during sliding.
+//For our voxel set, we'll just use the NonconvexReduction despite the fact that it'll allow bumps at shape boundaries during sliding.
+//(I'll leave boundary smoothing for voxels as a not-easy exercise for the highly motivated reader.)
+public struct ConvexVoxelsContinuations : IConvexCompoundContinuationHandler<NonconvexReduction>
+{
+    public CollisionContinuationType CollisionContinuationType => CollisionContinuationType.NonconvexReduction;
+
+    public ref NonconvexReduction CreateContinuation<TCallbacks>(
+        ref CollisionBatcher<TCallbacks> collisionBatcher, int childCount, in BoundsTestedPair pair, in OverlapQueryForPair pairQuery, out int continuationIndex)
+        where TCallbacks : struct, ICollisionCallbacks
+    {
+        return ref collisionBatcher.NonconvexReductions.CreateContinuation(childCount, collisionBatcher.Pool, out continuationIndex);
+    }
+
+    public static unsafe void GetChildData<TCallbacks>(ref CollisionBatcher<TCallbacks> collisionBatcher, ref NonconvexReductionChild continuationChild,
+        in BoundsTestedPair pair, int shapeTypeA, int childIndexB, out RigidPose childPoseB, out int childTypeB, out void* childShapeDataB)
+        where TCallbacks : struct, ICollisionCallbacks
+    {
+        ref var voxels = ref Unsafe.AsRef<Voxels>(pair.B);
+        ref var voxelIndex = ref voxels.VoxelIndices[childIndexB];
+        var localPosition = (voxelIndex + new Vector3(0.5f)) * voxels.VoxelSize;
+        QuaternionEx.TransformWithoutOverlap(localPosition, pair.OrientationB, out childPoseB.Position);
+        childPoseB.Orientation = Quaternion.Identity;
+        childTypeB = Box.Id;
+        //The collision batcher accumulates pairs to process by pointer, since in almost every other case the shape data is available by pointer already.
+        //But in the voxel set case, we don't actually have an explicit shape (or a secondary storage location as we have in MeshReductions).
+        //We can't just allocate a shape on the stack and return a pointer to it- the data needs to be valid until the collision batcher flushes that type batch.
+        //Fortunately, the collision batcher exposes a handy per-pair-type heap allocated memory blob that we can use to store the shape data.
+        //When the collision batcher flushes, it'll automatically get cleaned up.
+        var halfSize = voxels.VoxelSize * 0.5f;
+        //This reinterprets the vector3 as a Box to copy into the cache, which is a bit gross.
+        //The shape cache doesn't actually have any type information- it's just strategically placed memory.
+        //In other words, we're just giving a place for these 12 bytes to live until the flush.
+        collisionBatcher.CacheShapeB(shapeTypeA, childTypeB, Unsafe.AsPointer(ref halfSize), 12, out childShapeDataB);
+    }
 
-        [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public static unsafe void GetChildData<TCallbacks>(ref CollisionBatcher<TCallbacks> collisionBatcher, ref NonconvexReductionChild continuationChild,
-            in BoundsTestedPair pair, int shapeTypeA, int childIndexB, out RigidPose childPoseB, out int childTypeB, out void* childShapeDataB)
-            where TCallbacks : struct, ICollisionCallbacks
+    public unsafe void ConfigureContinuationChild<TCallbacks>(
+        ref CollisionBatcher<TCallbacks> collisionBatcher, ref NonconvexReduction continuation, int continuationChildIndex, in BoundsTestedPair pair, int shapeTypeA, int childIndexB,
+        out RigidPose childPoseB, out int childTypeB, out void* childShapeDataB)
+        where TCallbacks : struct, ICollisionCallbacks
+    {
+        ref var continuationChild = ref continuation.Children[continuationChildIndex];
+        GetChildData(ref collisionBatcher, ref continuationChild, pair, shapeTypeA, childIndexB, out childPoseB, out childTypeB, out childShapeDataB);
+        //Collision processors expect data to be provided in a specific order. The flip mask is used to make sure we're giving the data in the proper order.
+        //The collision batcher also takes into account the flip mask when reporting collision data through callbacks to preserve original user order.
+        if (pair.FlipMask < 0)
         {
-            ref var voxels = ref Unsafe.AsRef<Voxels>(pair.B);
-            ref var voxelIndex = ref voxels.VoxelIndices[childIndexB];
-            var localPosition = (voxelIndex + new Vector3(0.5f)) * voxels.VoxelSize;
-            QuaternionEx.TransformWithoutOverlap(localPosition, pair.OrientationB, out childPoseB.Position);
-            childPoseB.Orientation = Quaternion.Identity;
-            childTypeB = Box.Id;
-            //The collision batcher accumulates pairs to process by pointer, since in almost every other case the shape data is available by pointer already.
-            //But in the voxel set case, we don't actually have an explicit shape (or a secondary storage location as we have in MeshReductions).
-            //We can't just allocate a shape on the stack and return a pointer to it- the data needs to be valid until the collision batcher flushes that type batch.
-            //Fortunately, the collision batcher exposes a handy per-pair-type heap allocated memory blob that we can use to store the shape data.
-            //When the collision batcher flushes, it'll automatically get cleaned up.
-            var halfSize = voxels.VoxelSize * 0.5f;
-            //This reinterprets the vector3 as a Box to copy into the cache, which is a bit gross.
-            //The shape cache doesn't actually have any type information- it's just strategically placed memory.
-            //In other words, we're just giving a place for these 12 bytes to live until the flush.
-            collisionBatcher.CacheShapeB(shapeTypeA, childTypeB, Unsafe.AsPointer(ref halfSize), 12, out childShapeDataB);
+            continuationChild.ChildIndexA = childIndexB;
+            continuationChild.ChildIndexB = 0;
+            continuationChild.OffsetA = childPoseB.Position;
+            continuationChild.OffsetB = default;
         }
-
-        [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public unsafe void ConfigureContinuationChild<TCallbacks>(
-            ref CollisionBatcher<TCallbacks> collisionBatcher, ref NonconvexReduction continuation, int continuationChildIndex, in BoundsTestedPair pair, int shapeTypeA, int childIndexB,
-            out RigidPose childPoseB, out int childTypeB, out void* childShapeDataB)
-            where TCallbacks : struct, ICollisionCallbacks
+        else
         {
-            ref var continuationChild = ref continuation.Children[continuationChildIndex];
-            GetChildData(ref collisionBatcher, ref continuationChild, pair, shapeTypeA, childIndexB, out childPoseB, out childTypeB, out childShapeDataB);
-            //Collision processors expect data to be provided in a specific order. The flip mask is used to make sure we're giving the data in the proper order.
-            //The collision batcher also takes into account the flip mask when reporting collision data through callbacks to preserve original user order.
-            if (pair.FlipMask < 0)
-            {
-                continuationChild.ChildIndexA = childIndexB;
-                continuationChild.ChildIndexB = 0;
-                continuationChild.OffsetA = childPoseB.Position;
-                continuationChild.OffsetB = default;
-            }
-            else
-            {
-                continuationChild.ChildIndexA = 0;
-                continuationChild.ChildIndexB = childIndexB;
-                continuationChild.OffsetA = default;
-                continuationChild.OffsetB = childPoseB.Position;
-            }
+            continuationChild.ChildIndexA = 0;
+            continuationChild.ChildIndexB = childIndexB;
+            continuationChild.OffsetA = default;
+            continuationChild.OffsetB = childPoseB.Position;
         }
     }
+}
+
+public unsafe struct CompoundVoxelsContinuations<TCompoundA> : ICompoundPairContinuationHandler<NonconvexReduction>
+    where TCompoundA : ICompoundShape
+{
+    public CollisionContinuationType CollisionContinuationType => CollisionContinuationType.NonconvexReduction;
 
-    public unsafe struct CompoundVoxelsContinuations<TCompoundA> : ICompoundPairContinuationHandler<NonconvexReduction>
-        where TCompoundA : ICompoundShape
+    public ref NonconvexReduction CreateContinuation<TCallbacks>(
+        ref CollisionBatcher<TCallbacks> collisionBatcher, int totalChildCount, ref Buffer<ChildOverlapsCollection> pairOverlaps, ref Buffer<OverlapQueryForPair> pairQueries, in BoundsTestedPair pair, out int continuationIndex)
+        where TCallbacks : struct, ICollisionCallbacks
     {
-        public CollisionContinuationType CollisionContinuationType => CollisionContinuationType.NonconvexReduction;
+        return ref collisionBatcher.NonconvexReductions.CreateContinuation(totalChildCount, collisionBatcher.Pool, out continuationIndex);
+    }
 
-        [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public ref NonconvexReduction CreateContinuation<TCallbacks>(
-            ref CollisionBatcher<TCallbacks> collisionBatcher, int totalChildCount, ref Buffer<ChildOverlapsCollection> pairOverlaps, ref Buffer<OverlapQueryForPair> pairQueries, in BoundsTestedPair pair, out int continuationIndex)
-            where TCallbacks : struct, ICollisionCallbacks
-        {
-            return ref collisionBatcher.NonconvexReductions.CreateContinuation(totalChildCount, collisionBatcher.Pool, out continuationIndex);
-        }
+    public void GetChildAData<TCallbacks>(ref CollisionBatcher<TCallbacks> collisionBatcher, ref NonconvexReduction continuation, in BoundsTestedPair pair, int childIndexA,
+        out RigidPose childPoseA, out int childTypeA, out void* childShapeDataA)
+        where TCallbacks : struct, ICollisionCallbacks
+    {
+        ref var compoundA = ref Unsafe.AsRef<TCompoundA>(pair.A);
+        ref var compoundChildA = ref compoundA.GetChild(childIndexA);
+        Compound.GetRotatedChildPose(compoundChildA.AsPose(), pair.OrientationA, out childPoseA);
+        childTypeA = compoundChildA.ShapeIndex.Type;
+        collisionBatcher.Shapes[childTypeA].GetShapeData(compoundChildA.ShapeIndex.Index, out childShapeDataA, out _);
+    }
 
-        [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public unsafe void GetChildAData<TCallbacks>(ref CollisionBatcher<TCallbacks> collisionBatcher, ref NonconvexReduction continuation, in BoundsTestedPair pair, int childIndexA,
-            out RigidPose childPoseA, out int childTypeA, out void* childShapeDataA)
-            where TCallbacks : struct, ICollisionCallbacks
+    public void ConfigureContinuationChild<TCallbacks>(
+        ref CollisionBatcher<TCallbacks> 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
+    {
+        ref var continuationChild = ref continuation.Children[continuationChildIndex];
+
+        ConvexVoxelsContinuations.GetChildData(ref collisionBatcher, ref continuationChild, pair, childTypeA, childIndexB, out childPoseB, out childTypeB, out childShapeDataB);
+        if (pair.FlipMask < 0)
         {
-            ref var compoundA = ref Unsafe.AsRef<TCompoundA>(pair.A);
-            ref var compoundChildA = ref compoundA.GetChild(childIndexA);
-            Compound.GetRotatedChildPose(compoundChildA.LocalPose, pair.OrientationA, out childPoseA);
-            childTypeA = compoundChildA.ShapeIndex.Type;
-            collisionBatcher.Shapes[childTypeA].GetShapeData(compoundChildA.ShapeIndex.Index, out childShapeDataA, out _);
+            continuationChild.ChildIndexA = childIndexB;
+            continuationChild.ChildIndexB = childIndexA;
+            continuationChild.OffsetA = childPoseB.Position;
+            continuationChild.OffsetB = childPoseA.Position;
         }
-
-        [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public unsafe void ConfigureContinuationChild<TCallbacks>(
-            ref CollisionBatcher<TCallbacks> 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
+        else
         {
-            ref var continuationChild = ref continuation.Children[continuationChildIndex];
-
-            ConvexVoxelsContinuations.GetChildData(ref collisionBatcher, ref continuationChild, pair, childTypeA, childIndexB, out childPoseB, out childTypeB, out childShapeDataB);
-            if (pair.FlipMask < 0)
-            {
-                continuationChild.ChildIndexA = childIndexB;
-                continuationChild.ChildIndexB = childIndexA;
-                continuationChild.OffsetA = childPoseB.Position;
-                continuationChild.OffsetB = childPoseA.Position;
-            }
-            else
-            {
-                continuationChild.ChildIndexA = childIndexA;
-                continuationChild.ChildIndexB = childIndexB;
-                continuationChild.OffsetA = childPoseA.Position;
-                continuationChild.OffsetB = childPoseB.Position;
-            }
+            continuationChild.ChildIndexA = childIndexA;
+            continuationChild.ChildIndexB = childIndexB;
+            continuationChild.OffsetA = childPoseA.Position;
+            continuationChild.OffsetB = childPoseB.Position;
         }
-
     }
 
-    public class CustomVoxelCollidableDemo : Demo
+}
+
+public class CustomVoxelCollidableDemo : Demo
+{
+    Voxels voxels;
+    StaticHandle handle;
+    public override void Initialize(ContentArchive content, Camera camera)
     {
-        Voxels voxels;
-        StaticHandle handle;
-        public unsafe override void Initialize(ContentArchive content, Camera camera)
+        camera.Position = new Vector3(-40, 40, -40);
+        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(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.
+        Simulation.NarrowPhase.CollisionTaskRegistry.Register(new ConvexCompoundCollisionTask<Sphere, Voxels, ConvexCompoundOverlapFinder<Sphere, SphereWide, Voxels>, ConvexVoxelsContinuations, NonconvexReduction>());
+        Simulation.NarrowPhase.CollisionTaskRegistry.Register(new ConvexCompoundCollisionTask<Capsule, Voxels, ConvexCompoundOverlapFinder<Capsule, CapsuleWide, Voxels>, ConvexVoxelsContinuations, NonconvexReduction>());
+        Simulation.NarrowPhase.CollisionTaskRegistry.Register(new ConvexCompoundCollisionTask<Box, Voxels, ConvexCompoundOverlapFinder<Box, BoxWide, Voxels>, ConvexVoxelsContinuations, NonconvexReduction>());
+        Simulation.NarrowPhase.CollisionTaskRegistry.Register(new ConvexCompoundCollisionTask<Triangle, Voxels, ConvexCompoundOverlapFinder<Triangle, TriangleWide, Voxels>, ConvexVoxelsContinuations, NonconvexReduction>());
+        Simulation.NarrowPhase.CollisionTaskRegistry.Register(new ConvexCompoundCollisionTask<Cylinder, Voxels, ConvexCompoundOverlapFinder<Cylinder, CylinderWide, Voxels>, ConvexVoxelsContinuations, NonconvexReduction>());
+        Simulation.NarrowPhase.CollisionTaskRegistry.Register(new ConvexCompoundCollisionTask<ConvexHull, Voxels, ConvexCompoundOverlapFinder<ConvexHull, ConvexHullWide, Voxels>, ConvexVoxelsContinuations, NonconvexReduction>());
+
+        Simulation.NarrowPhase.CollisionTaskRegistry.Register(new CompoundPairCollisionTask<Compound, Voxels, CompoundPairOverlapFinder<Compound, Voxels>, CompoundVoxelsContinuations<Compound>, NonconvexReduction>());
+        Simulation.NarrowPhase.CollisionTaskRegistry.Register(new CompoundPairCollisionTask<BigCompound, Voxels, CompoundPairOverlapFinder<BigCompound, Voxels>, CompoundVoxelsContinuations<BigCompound>, NonconvexReduction>());
+
+        //Note that this demo excludes mesh-voxels and voxels-voxels pairs. Those get a little more complicated since there's some gaps in the pre-built helpers.
+        //If you wanted to make your own, look into the various types related to meshes. They're a good starting point, although I'm not exactly happy with the complexity of the
+        //current design. They might receive some significant changes- keep that in mind if you create anything which depends heavily on their current implementation.
+
+        //To support sweep tests, we must also register sweep tasks. No extra work is required to support these; the interface implementation on the shape is good enough.
+        Simulation.NarrowPhase.SweepTaskRegistry.Register(new ConvexHomogeneousCompoundSweepTask<Sphere, SphereWide, Voxels, Box, BoxWide, ConvexCompoundSweepOverlapFinder<Sphere, Voxels>>());
+        Simulation.NarrowPhase.SweepTaskRegistry.Register(new ConvexHomogeneousCompoundSweepTask<Capsule, CapsuleWide, Voxels, Box, BoxWide, ConvexCompoundSweepOverlapFinder<Capsule, Voxels>>());
+        Simulation.NarrowPhase.SweepTaskRegistry.Register(new ConvexHomogeneousCompoundSweepTask<Box, BoxWide, Voxels, Box, BoxWide, ConvexCompoundSweepOverlapFinder<Box, Voxels>>());
+        Simulation.NarrowPhase.SweepTaskRegistry.Register(new ConvexHomogeneousCompoundSweepTask<Triangle, TriangleWide, Voxels, Box, BoxWide, ConvexCompoundSweepOverlapFinder<Triangle, Voxels>>());
+        Simulation.NarrowPhase.SweepTaskRegistry.Register(new ConvexHomogeneousCompoundSweepTask<Cylinder, CylinderWide, Voxels, Box, BoxWide, ConvexCompoundSweepOverlapFinder<Cylinder, Voxels>>());
+        Simulation.NarrowPhase.SweepTaskRegistry.Register(new ConvexHomogeneousCompoundSweepTask<ConvexHull, ConvexHullWide, Voxels, Box, BoxWide, ConvexCompoundSweepOverlapFinder<ConvexHull, Voxels>>());
+
+        Simulation.NarrowPhase.SweepTaskRegistry.Register(new CompoundHomogeneousCompoundSweepTask<Compound, Voxels, Box, BoxWide, CompoundPairSweepOverlapFinder<Compound, Voxels>>());
+        Simulation.NarrowPhase.SweepTaskRegistry.Register(new CompoundHomogeneousCompoundSweepTask<BigCompound, Voxels, Box, BoxWide, CompoundPairSweepOverlapFinder<BigCompound, Voxels>>());
+        //Supporting voxels-mesh and voxels-voxels would again require a bit more effort, though a bit less than the collision task equivalents would.
+
+
+        var widthInVoxels = 40;
+        var heightInVoxels = 30;
+        var voxelIndices = new QuickList<Vector3>(widthInVoxels * heightInVoxels * widthInVoxels, BufferPool);
+        for (int i = 0; i < widthInVoxels; ++i)
         {
-            camera.Position = new Vector3(-40, 40, -40);
-            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());
-
-            //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.
-            Simulation.NarrowPhase.CollisionTaskRegistry.Register(new ConvexCompoundCollisionTask<Sphere, Voxels, ConvexCompoundOverlapFinder<Sphere, SphereWide, Voxels>, ConvexVoxelsContinuations, NonconvexReduction>());
-            Simulation.NarrowPhase.CollisionTaskRegistry.Register(new ConvexCompoundCollisionTask<Capsule, Voxels, ConvexCompoundOverlapFinder<Capsule, CapsuleWide, Voxels>, ConvexVoxelsContinuations, NonconvexReduction>());
-            Simulation.NarrowPhase.CollisionTaskRegistry.Register(new ConvexCompoundCollisionTask<Box, Voxels, ConvexCompoundOverlapFinder<Box, BoxWide, Voxels>, ConvexVoxelsContinuations, NonconvexReduction>());
-            Simulation.NarrowPhase.CollisionTaskRegistry.Register(new ConvexCompoundCollisionTask<Triangle, Voxels, ConvexCompoundOverlapFinder<Triangle, TriangleWide, Voxels>, ConvexVoxelsContinuations, NonconvexReduction>());
-            Simulation.NarrowPhase.CollisionTaskRegistry.Register(new ConvexCompoundCollisionTask<Cylinder, Voxels, ConvexCompoundOverlapFinder<Cylinder, CylinderWide, Voxels>, ConvexVoxelsContinuations, NonconvexReduction>());
-            Simulation.NarrowPhase.CollisionTaskRegistry.Register(new ConvexCompoundCollisionTask<ConvexHull, Voxels, ConvexCompoundOverlapFinder<ConvexHull, ConvexHullWide, Voxels>, ConvexVoxelsContinuations, NonconvexReduction>());
-
-            Simulation.NarrowPhase.CollisionTaskRegistry.Register(new CompoundPairCollisionTask<Compound, Voxels, CompoundPairOverlapFinder<Compound, Voxels>, CompoundVoxelsContinuations<Compound>, NonconvexReduction>());
-            Simulation.NarrowPhase.CollisionTaskRegistry.Register(new CompoundPairCollisionTask<BigCompound, Voxels, CompoundPairOverlapFinder<BigCompound, Voxels>, CompoundVoxelsContinuations<BigCompound>, NonconvexReduction>());
-
-            //Note that this demo excludes mesh-voxels and voxels-voxels pairs. Those get a little more complicated since there's some gaps in the pre-built helpers.
-            //If you wanted to make your own, look into the various types related to meshes. They're a good starting point, although I'm not exactly happy with the complexity of the
-            //current design. They might receive some significant changes- keep that in mind if you create anything which depends heavily on their current implementation.
-
-            //To support sweep tests, we must also register sweep tasks. No extra work is required to support these; the interface implementation on the shape is good enough.
-            Simulation.NarrowPhase.SweepTaskRegistry.Register(new ConvexHomogeneousCompoundSweepTask<Sphere, SphereWide, Voxels, Box, BoxWide, ConvexCompoundSweepOverlapFinder<Sphere, Voxels>>());
-            Simulation.NarrowPhase.SweepTaskRegistry.Register(new ConvexHomogeneousCompoundSweepTask<Capsule, CapsuleWide, Voxels, Box, BoxWide, ConvexCompoundSweepOverlapFinder<Capsule, Voxels>>());
-            Simulation.NarrowPhase.SweepTaskRegistry.Register(new ConvexHomogeneousCompoundSweepTask<Box, BoxWide, Voxels, Box, BoxWide, ConvexCompoundSweepOverlapFinder<Box, Voxels>>());
-            Simulation.NarrowPhase.SweepTaskRegistry.Register(new ConvexHomogeneousCompoundSweepTask<Triangle, TriangleWide, Voxels, Box, BoxWide, ConvexCompoundSweepOverlapFinder<Triangle, Voxels>>());
-            Simulation.NarrowPhase.SweepTaskRegistry.Register(new ConvexHomogeneousCompoundSweepTask<Cylinder, CylinderWide, Voxels, Box, BoxWide, ConvexCompoundSweepOverlapFinder<Cylinder, Voxels>>());
-            Simulation.NarrowPhase.SweepTaskRegistry.Register(new ConvexHomogeneousCompoundSweepTask<ConvexHull, ConvexHullWide, Voxels, Box, BoxWide, ConvexCompoundSweepOverlapFinder<ConvexHull, Voxels>>());
-
-            Simulation.NarrowPhase.SweepTaskRegistry.Register(new CompoundHomogeneousCompoundSweepTask<Compound, Voxels, Box, BoxWide, CompoundPairSweepOverlapFinder<Compound, Voxels>>());
-            Simulation.NarrowPhase.SweepTaskRegistry.Register(new CompoundHomogeneousCompoundSweepTask<BigCompound, Voxels, Box, BoxWide, CompoundPairSweepOverlapFinder<BigCompound, Voxels>>());
-            //Supporting voxels-mesh and voxels-voxels would again require a bit more effort, though a bit less than the collision task equivalents would.
-
-
-            var widthInVoxels = 40;
-            var heightInVoxels = 30;
-            var voxelIndices = new QuickList<Vector3>(widthInVoxels * heightInVoxels * widthInVoxels, BufferPool);
-            for (int i = 0; i < widthInVoxels; ++i)
+            for (int j = 0; j < heightInVoxels; ++j)
             {
-                for (int j = 0; j < heightInVoxels; ++j)
+                for (int k = 0; k < widthInVoxels; ++k)
                 {
-                    for (int k = 0; k < widthInVoxels; ++k)
-                    {
-                        //Create some sine wave based noise for a slightly interesting environment.
-                        var octave0 = MathF.Cos((i + 78) * 0.8f) + MathF.Cos((j + 37) * 0.8f) + MathF.Cos((k + 131) * 0.8f);
-                        var octave1 = MathF.Cos((i + 59) * 0.4f) + MathF.Cos((j + 100) * 0.4f) + MathF.Cos((k + 131) * 0.4f);
-                        var octave2 = MathF.Cos((i + 43) * 0.1f) + MathF.Cos((j + 200) * 0.1f) + MathF.Cos((k + 281) * 0.1f);
-                        var octave3 = MathF.Cos((i + 647) * 0.025f) + MathF.Cos((j + 1553) * 0.025f) + MathF.Cos((k + 53) * 0.025f);
-                        var density = octave0 + octave1 + octave2 + octave3;
-                        if (density > 0)
-                            voxelIndices.AllocateUnsafely() = new Vector3(i, j, k);
-                    }
+                    //Create some sine wave based noise for a slightly interesting environment.
+                    var octave0 = MathF.Cos((i + 78) * 0.8f) + MathF.Cos((j + 37) * 0.8f) + MathF.Cos((k + 131) * 0.8f);
+                    var octave1 = MathF.Cos((i + 59) * 0.4f) + MathF.Cos((j + 100) * 0.4f) + MathF.Cos((k + 131) * 0.4f);
+                    var octave2 = MathF.Cos((i + 43) * 0.1f) + MathF.Cos((j + 200) * 0.1f) + MathF.Cos((k + 281) * 0.1f);
+                    var octave3 = MathF.Cos((i + 647) * 0.025f) + MathF.Cos((j + 1553) * 0.025f) + MathF.Cos((k + 53) * 0.025f);
+                    var density = octave0 + octave1 + octave2 + octave3;
+                    if (density > 0)
+                        voxelIndices.AllocateUnsafely() = new Vector3(i, j, k);
                 }
             }
-            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)));
-
-            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));
-            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);
-            }
+        }
+        voxels = new Voxels(voxelIndices, new Vector3(1, 1, 1), BufferPool);
+        handle = Simulation.Statics.Add(new StaticDescription(new Vector3(0, 0, 0), Simulation.Shapes.Add(voxels)));
 
-            Simulation.Statics.Add(new StaticDescription(new Vector3(0, -0.5f, 0), new CollidableDescription(Simulation.Shapes.Add(new Box(300, 1, 300)), 0.1f)));
+        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 < 4096; ++i)
+        {
+            descriptionToDrop.Pose.Position = new Vector3(15 + 10 * random.NextSingle(), 45 + 150 * random.NextSingle(), 15 + 10 * random.NextSingle());
+            Simulation.Bodies.Add(descriptionToDrop);
         }
 
-        public override unsafe void Render(Renderer renderer, Camera camera, Input input, TextBuilder text, Font font)
+        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)
+    {
+        //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[handle].Pose;
+        for (int i = 0; i < voxels.ChildCount; ++i)
         {
-            //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]];
-            for (int i = 0; i < voxels.ChildCount; ++i)
-            {
-                var localPose = new RigidPose((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));
-            }
-            base.Render(renderer, camera, input, text, font);
+            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, 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);
     }
 }
 
diff --git a/Demos/Demos/Dancers/DancerDemo.cs b/Demos/Demos/Dancers/DancerDemo.cs
new file mode 100644
index 000000000..794a46eef
--- /dev/null
+++ b/Demos/Demos/Dancers/DancerDemo.cs
@@ -0,0 +1,193 @@
+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.Dancers;
+
+/// <summary>
+/// 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 <see cref="DemoDancers"/> implementation, this provides a starting point for cosmetic cloth attached to characters. 
+/// </summary>
+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<ClothCollisionFilter> 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<ClothCollisionFilter> 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 = 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<Capsule>(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 override void Initialize(ContentArchive content, Camera camera)
+    {
+        camera.Position = new Vector3(0, 2, 10);
+        camera.Yaw = 0;
+        camera.Pitch = 0;
+
+        var collisionFilters = new CollidableProperty<SubgroupCollisionFilter>();
+        Simulation = Simulation.Create(BufferPool, new SubgroupFilteredCallbacks(collisionFilters), new DemoPoseIntegratorCallbacks(new Vector3(0, 0, 0)), new SolveDescription(8, 1));
+
+        dancers = new DemoDancers().Initialize<ClothCallbacks, ClothCollisionFilter>(16, 16, 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 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..cd846a100
--- /dev/null
+++ b/Demos/Demos/Dancers/DemoDancers.cs
@@ -0,0 +1,469 @@
+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 UpperLeftLeg;
+    public BodyHandle LowerLeftLeg;
+    public BodyHandle UpperRightLeg;
+    public BodyHandle LowerRightLeg;
+    public BodyHandle UpperLeftArm;
+    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<BodyHandle> AsBuffer(DancerBodyHandles* sourceBodyHandles)
+    {
+        return new Buffer<BodyHandle>(sourceBodyHandles, 12);
+    }
+}
+
+/// <summary>
+/// Controls one of the main dancer's limbs by yanking it around.
+/// </summary>
+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 });
+    }
+
+}
+
+/// <summary>
+/// Marks an <see cref="INarrowPhaseCallbacks"/> as usable with the DemoDancers.
+/// </summary>
+/// <typeparam name="TCallbacks">Type of the callbacks to use.</typeparam>
+/// <typeparam name="TFilter">Type of the callback filters to use.</typeparam>
+public interface IDancerNarrowPhaseCallbacks<TCallbacks, TFilter> where TCallbacks : INarrowPhaseCallbacks, IDancerNarrowPhaseCallbacks<TCallbacks, TFilter> where TFilter : unmanaged
+{
+    static abstract TCallbacks Create(CollidableProperty<TFilter> filters, PairMaterialProperties pairMaterialProperties, int minimumDistanceForSelfCollisions);
+}
+
+/// <summary>
+/// Used by <see cref="DancerDemo"/> and <see cref="PlumpDancerDemo"/> to coordinate lots of background dancers.
+/// </summary>
+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 HistoryLength = 256;
+    public int DancerGridWidth = 16;
+    public int DancerGridLength = 16;
+
+
+    DancerControl hipsControl;
+    DancerControl leftFootControl;
+    DancerControl rightFootControl;
+    DancerControl leftHandControl;
+    DancerControl rightHandControl;
+
+    public delegate void DressUpDancer<TCollisionFilter>(Simulation simulation, CollidableProperty<TCollisionFilter> filters, DancerBodyHandles bodyHandles, int dancerIndex, int dancerGridWidth, float levelOfDetail) where TCollisionFilter : unmanaged;
+    public DemoDancers Initialize<TNarrowPhaseCallbacks, TCollisionFilter>(
+        int dancerGridWidth, int dancerGridLength, Simulation mainSimulation, CollidableProperty<SubgroupCollisionFilter> mainCollisionFilters,
+        IThreadDispatcher threadDispatcher, BufferPool pool, SolveDescription dancerSolveDescription, DressUpDancer<TCollisionFilter> dressUpDancer, TCollisionFilter filterForDancerBodies)
+        where TNarrowPhaseCallbacks : struct, INarrowPhaseCallbacks, IDancerNarrowPhaseCallbacks<TNarrowPhaseCallbacks, TCollisionFilter>
+        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);
+        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<MotionState>(HistoryLength * 12, pool);
+
+        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<TCollisionFilter> filters, TCollisionFilter bodyFilter)
+        {
+            var description = sourceSimulation.Bodies.GetDescription(sourceHandle);
+            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;
+        }
+
+        for (int i = 0; i < Handles.Length; ++i)
+        {
+            ref var dancer = ref Handles[i];
+            var dancerFilters = new CollidableProperty<TCollisionFilter>();
+            //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.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 = 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. 
+                //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, 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, 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, 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, dancerGridWidth, 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<SubgroupCollisionFilter> 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, int dancerGridWidth)
+    {
+        var rowIndex = dancerIndex / dancerGridWidth;
+        return (dancerIndex - rowIndex * dancerGridWidth, rowIndex);
+    }
+
+    public QuickQueue<MotionState> MotionHistory;
+    public static Vector3 GetOffsetForDancer(int i, int dancerGridWidth)
+    {
+        const float spacing = 2;
+        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, int dancerGridWidth)
+    {
+        var (columnIndex, rowIndex) = GetRowAndColumnForDancer(dancerIndex, dancerGridWidth);
+        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, DancerGridWidth) * 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, DancerGridWidth);
+        }
+        //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/Dancers/PlumpDancerDemo.cs b/Demos/Demos/Dancers/PlumpDancerDemo.cs
new file mode 100644
index 000000000..277574e71
--- /dev/null
+++ b/Demos/Demos/Dancers/PlumpDancerDemo.cs
@@ -0,0 +1,251 @@
+using BepuPhysics;
+using BepuPhysics.Collidables;
+using BepuPhysics.Constraints;
+using BepuUtilities;
+using DemoContentLoader;
+using DemoRenderer;
+using DemoRenderer.UI;
+using DemoUtilities;
+using System;
+using System.Diagnostics;
+using System.Numerics;
+
+namespace Demos.Demos.Dancers;
+
+/// <summary>
+/// A bunch of somewhat overweight background dancers struggle to keep up with the masterful purple prancer.
+/// Combined with the <see cref="DemoDancers"/> implementation, this shows an example of how cosmetic deformable physics could be applied to characters.
+/// </summary>
+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<Capsule>(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<DeformableCollisionFilter> 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<TestCapsule> 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<DeformableCollisionFilter> 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<Capsule>(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<SubgroupCollisionFilter>();
+        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<DeformableCallbacks, DeformableCollisionFilter>(8, 8, Simulation, collisionFilters, ThreadDispatcher, BufferPool, new SolveDescription(1, 1), CreateFatSuit, new DeformableCollisionFilter(0, 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 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/Demos/FountainStressTestDemo.cs b/Demos/Demos/FountainStressTestDemo.cs
deleted file mode 100644
index 1a5032b9c..000000000
--- a/Demos/Demos/FountainStressTestDemo.cs
+++ /dev/null
@@ -1,374 +0,0 @@
-using BepuUtilities;
-using DemoRenderer;
-using DemoUtilities;
-using BepuPhysics;
-using BepuPhysics.Collidables;
-using System;
-using System.Numerics;
-using BepuUtilities.Memory;
-using BepuUtilities.Collections;
-using System.Diagnostics;
-using DemoContentLoader;
-
-namespace Demos.Demos
-{
-    public class FountainStressTestDemo : Demo
-    {
-        QuickQueue<StaticDescription> removedStatics;
-        QuickQueue<BodyHandle> dynamicHandles;
-        Random random;
-        public unsafe 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.
-            Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new PositionFirstTimestepper(), initialAllocationSizes:
-            new SimulationAllocationSizes
-            {
-                Bodies = 1,
-                ConstraintCountPerBodyEstimate = 1,
-                Constraints = 1,
-                ConstraintsPerTypeBatch = 1,
-                Islands = 1,
-                ShapesPerType = 1,
-                Statics = 1
-            });
-
-            Simulation.Deterministic = true;
-
-            const int planeWidth = 8;
-            const int planeHeight = 8;
-            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);
-            var staticShapeIndex = Simulation.Shapes.Add(staticShape);
-            const int staticGridWidthInInstances = 128;
-            const float staticSpacing = 8;
-            for (int i = 0; i < staticGridWidthInInstances; ++i)
-            {
-                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),
-                            new CollidableDescription(staticShapeIndex, 0.1f));
-                    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)),
-                            new CollidableDescription(kinematicShapeIndex, 0.1f),
-                            new BodyActivityDescription(0, 4));
-                kinematicHandles[i] = Simulation.Bodies.Add(description);
-            }
-
-            dynamicHandles = new QuickQueue<BodyHandle>(65536, BufferPool);
-            removedStatics = new QuickQueue<StaticDescription>(512, BufferPool);
-            random = new Random(5);
-        }
-
-        double time;
-        double t;
-        BodyHandle[] kinematicHandles;
-
-        void AddConvexShape<TConvex>(in TConvex convex, out TypedIndex shapeIndex, out BodyInertia inertia) where TConvex : unmanaged, IConvexShape
-        {
-            shapeIndex = Simulation.Shapes.Add(convex);
-            convex.ComputeInertia(1, out inertia);
-        }
-
-        ConvexHull CreateRandomHull()
-        {
-            const int pointCount = 32;
-            var points = new QuickList<Vector3>(pointCount, BufferPool);
-            for (int i = 0; i < pointCount; ++i)
-            {
-                points.AllocateUnsafely() = new Vector3(1 * (float)random.NextDouble(), 0.5f * (float)random.NextDouble(), 1.5f * (float)random.NextDouble());
-            }
-            var hull = new ConvexHull(points.Span.Slice(points.Count), BufferPool, out _);
-            points.Dispose(BufferPool);
-            return hull;
-        }
-
-        void CreateRandomCompound(out Buffer<CompoundChild> children, out BodyInertia inertia)
-        {
-            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 * (float)random.NextDouble()), 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);
-                            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);
-                            break;
-                        case 3:
-                            AddConvexShape(new Cylinder(0.1f + (float)random.NextDouble(), 0.2f + (float)random.NextDouble()), 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);
-                    float orientationLengthSquared;
-                    do
-                    {
-                        localPose.Orientation = new Quaternion((float)random.NextDouble(), (float)random.NextDouble(), (float)random.NextDouble(), (float)random.NextDouble());
-                    }
-                    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);
-            }
-        }
-
-        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, 7))
-                {
-                    default:
-                        {
-                            AddConvexShape(new Sphere(0.35f + 0.35f * (float)random.NextDouble()), 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);
-                        }
-                        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);
-                        }
-                        break;
-                    case 3:
-                        {
-                            AddConvexShape(new Cylinder(0.1f + 0.5f * (float)random.NextDouble(), 0.2f + (float)random.NextDouble()), 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;
-                }
-            }
-
-            description = new BodyDescription
-            {
-                Pose = pose,
-                LocalInertia = inertia,
-                Collidable = new CollidableDescription(shapeIndex, 5f),
-                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;
-            }
-        }
-
-        public override void Update(Window window, Camera camera, Input input, float dt)
-        {
-            var timestepDuration = 1f / 60f;
-            time += timestepDuration;
-
-            //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;
-
-            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.Poses[bodyLocation.Index].Position;
-                var distance = correction.Length();
-                if (distance > 1e-4)
-                {
-                    if (bodyLocation.SetIndex > 0)
-                    {
-                        //We're requesting a nonzero velocity, so it must be active.
-                        Simulation.Awakener.AwakenSet(bodyLocation.SetIndex);
-                    }
-                    if (distance > maxDisplacement)
-                    {
-                        correction *= maxDisplacement / distance;
-                    }
-                    Debug.Assert(bodyLocation.SetIndex == 0);
-                    Simulation.Bodies.ActiveSet.Velocities[bodyLocation.Index].Linear = correction * inverseDt;
-                }
-                else
-                {
-                    if (bodyLocation.SetIndex == 0)
-                    {
-                        Simulation.Bodies.ActiveSet.Velocities[bodyLocation.Index].Linear = new Vector3();
-                    }
-                }
-            }
-
-            //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);
-            }
-
-            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), (float)random.NextDouble() * 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 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;
-            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.NextDouble() < 0.1f)
-                {
-                    //Occasionally make a dynamic kinematic.
-                    newDescription.LocalInertia = 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/Demos/FrictionDemo.cs b/Demos/Demos/FrictionDemo.cs
new file mode 100644
index 000000000..b07cb19f6
--- /dev/null
+++ b/Demos/Demos/FrictionDemo.cs
@@ -0,0 +1,125 @@
+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;
+
+/// <summary>
+/// Shows how to configure things to slide at different rates.
+/// </summary>
+/// <remarks>
+/// For a similar example of bounciness, see the <see cref="BouncinessDemo"/>.
+/// </remarks>
+public class FrictionDemo : Demo
+{
+    public struct SimpleMaterial
+    {
+        public SpringSettings SpringSettings;
+        public float FrictionCoefficient;
+        public float MaximumRecoveryVelocity;
+    }
+    public struct FrictionCallbacks : INarrowPhaseCallbacks
+    {
+        /// <summary>
+        /// Maps <see cref="CollidableReference"/> entries to their <see cref="SimpleMaterial"/>.
+        /// </summary>
+        /// <remarks>
+        /// The narrow phase callbacks need some way to get the material data for this demo, but there's no requirement that you use the <see cref="CollidableProperty{T}"/> type.
+        /// It's just a fairly convenient and simple option.
+        /// </remarks>
+        public CollidableProperty<SimpleMaterial> 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 bool ConfigureContactManifold<TManifold>(int workerIndex, CollidablePair pair, ref TManifold manifold, out PairMaterialProperties pairMaterial) where TManifold : unmanaged, IContactManifold<TManifold>
+        {
+            //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 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<SimpleMaterial>();
+        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);
+    }
+}
diff --git a/Demos/Demos/NewtDemo.cs b/Demos/Demos/NewtDemo.cs
index b5256b80a..c8721721f 100644
--- a/Demos/Demos/NewtDemo.cs
+++ b/Demos/Demos/NewtDemo.cs
@@ -7,734 +7,758 @@
 using BepuUtilities.Memory;
 using DemoContentLoader;
 using DemoRenderer;
+using DemoRenderer.UI;
+using DemoUtilities;
 using System;
 using System.Diagnostics;
 using System.Numerics;
 using System.Runtime.CompilerServices;
 
-namespace Demos.Demos
+namespace Demos.Demos;
+
+//(You might notice that this demo is really large, uses some older idioms, and is a little out of place. I just pulled most of this stuff out of my older GPU deformable physics project.)
+using CellSet = QuickSet<Cell, CellComparer>;
+using CellList = QuickList<Cell>;
+public static class BoxTriangleCollider
 {
-    //(You might notice that this demo is really large, uses some older idioms, and is a little out of place. I just pulled most of this stuff out of my older GPU deformable physics project.)
-    using CellSet = QuickSet<Cell, CellComparer>;
-    using CellList = QuickList<Cell>;
-    public static class BoxTriangleCollider
+    private const float IntersectionEpsilon = 1e-4f;
+    private static bool OverlapsAlongAxis(ref Vector3 axis, ref Vector3 halfExtents, ref Vector3 a, ref Vector3 b, ref Vector3 c)
     {
-        private const float IntersectionEpsilon = 1e-4f;
-        private static bool OverlapsAlongAxis(ref Vector3 axis, ref Vector3 halfExtents, ref Vector3 a, ref Vector3 b, ref Vector3 c)
+        var da = Vector3.Dot(a, axis);
+        var db = Vector3.Dot(b, axis);
+        var dc = Vector3.Dot(c, axis);
+
+        float min, max;
+        if (da < db && da < dc)
+        {
+            min = da;
+            max = db > dc ? db : dc;
+        }
+        else if (db < dc)
         {
-            var da = Vector3.Dot(a, axis);
-            var db = Vector3.Dot(b, axis);
-            var dc = Vector3.Dot(c, axis);
+            min = db;
+            max = da > dc ? da : dc;
+        }
+        else
+        {
+            min = dc;
+            max = da > db ? da : db;
+        }
 
-            float min, max;
-            if (da < db && da < dc)
-            {
-                min = da;
-                max = db > dc ? db : dc;
-            }
-            else if (db < dc)
-            {
-                min = db;
-                max = da > dc ? da : dc;
-            }
-            else
-            {
-                min = dc;
-                max = da > db ? da : db;
-            }
+        Vector3 boxExtremePoint;
+        if (axis.X > 0)
+            boxExtremePoint.X = halfExtents.X;
+        else
+            boxExtremePoint.X = -halfExtents.X;
 
-            Vector3 boxExtremePoint;
-            if (axis.X > 0)
-                boxExtremePoint.X = halfExtents.X;
-            else
-                boxExtremePoint.X = -halfExtents.X;
+        if (axis.Y > 0)
+            boxExtremePoint.Y = halfExtents.Y;
+        else
+            boxExtremePoint.Y = -halfExtents.Y;
 
-            if (axis.Y > 0)
-                boxExtremePoint.Y = halfExtents.Y;
-            else
-                boxExtremePoint.Y = -halfExtents.Y;
+        if (axis.Z > 0)
+            boxExtremePoint.Z = halfExtents.Z;
+        else
+            boxExtremePoint.Z = -halfExtents.Z;
 
-            if (axis.Z > 0)
-                boxExtremePoint.Z = halfExtents.Z;
-            else
-                boxExtremePoint.Z = -halfExtents.Z;
+        var boxMax = Vector3.Dot(boxExtremePoint, axis);
+        var boxMin = -boxMax;
 
-            var boxMax = Vector3.Dot(boxExtremePoint, axis);
-            var boxMin = -boxMax;
+        return !(max + IntersectionEpsilon < boxMin || min - IntersectionEpsilon > boxMax);
 
-            return !(max + IntersectionEpsilon < boxMin || min - IntersectionEpsilon > boxMax);
+    }
 
+    /// <summary>
+    /// Determines if a triangle in a box's local space intersects that box.
+    /// </summary>
+    /// <param name="halfExtents">Half extents of the box.</param>
+    /// <param name="a">First vertex of the triangle in the box's local space.</param>
+    /// <param name="b">Second vertex of the triangle in the box's local space.</param>
+    /// <param name="c">Third vertex of the triangle in the box's local space.</param>
+    /// <returns>True if the triangle intersects the box, false otherwise.</returns>
+    public static bool Intersecting(ref Vector3 halfExtents, ref Vector3 a, ref Vector3 b, ref Vector3 c)
+    {
+        //Need to test 3 box faces, 1 triangle face, and 3 * 3 edges.
+
+        //Test each of the box's faces.
+        //NOTE: In dermocat, this condition will never be hit because we only select cells which have an overlapping bounding box.
+        //Despite that, this will stay in for correctness. It's extremely cheap anyway.
+        Vector3 expandedHalfExtents;
+        expandedHalfExtents.X = halfExtents.X + IntersectionEpsilon;
+        expandedHalfExtents.Y = halfExtents.Y + IntersectionEpsilon;
+        expandedHalfExtents.Z = halfExtents.Z + IntersectionEpsilon;
+        if ((a.X > expandedHalfExtents.X && b.X > expandedHalfExtents.X && c.X > expandedHalfExtents.X) ||
+            (a.Y > expandedHalfExtents.Y && b.Y > expandedHalfExtents.Y && c.Y > expandedHalfExtents.Y) ||
+            (a.Z > expandedHalfExtents.Z && b.Z > expandedHalfExtents.Z && c.Z > expandedHalfExtents.Z) ||
+            (a.X < -expandedHalfExtents.X && b.X < -expandedHalfExtents.X && c.X < -expandedHalfExtents.X) ||
+            (a.Y < -expandedHalfExtents.Y && b.Y < -expandedHalfExtents.Y && c.Y < -expandedHalfExtents.Y) ||
+            (a.Z < -expandedHalfExtents.Z && b.Z < -expandedHalfExtents.Z && c.Z < -expandedHalfExtents.Z))
+        {
+            return false;
         }
 
-        /// <summary>
-        /// Determines if a triangle in a box's local space intersects that box.
-        /// </summary>
-        /// <param name="halfExtents">Half extents of the box.</param>
-        /// <param name="a">First vertex of the triangle in the box's local space.</param>
-        /// <param name="b">Second vertex of the triangle in the box's local space.</param>
-        /// <param name="c">Third vertex of the triangle in the box's local space.</param>
-        /// <returns>True if the triangle intersects the box, false otherwise.</returns>
-        public static bool Intersecting(ref Vector3 halfExtents, ref Vector3 a, ref Vector3 b, ref Vector3 c)
+        //Test the triangle face.
+        //Note that we don't use the axis overlap test here.
+        //We can do better since we know that all triangle vertices have the same value.
+        var ab = b - a;
+        var ac = c - a;
+        var normal = Vector3.Cross(ab, ac);
+        var d = Vector3.Dot(normal, a);
+        if (d < 0)
         {
-            //Need to test 3 box faces, 1 triangle face, and 3 * 3 edges.
-
-            //Test each of the box's faces.
-            //NOTE: In dermocat, this condition will never be hit because we only select cells which have an overlapping bounding box.
-            //Despite that, this will stay in for correctness. It's extremely cheap anyway.
-            Vector3 expandedHalfExtents;
-            expandedHalfExtents.X = halfExtents.X + IntersectionEpsilon;
-            expandedHalfExtents.Y = halfExtents.Y + IntersectionEpsilon;
-            expandedHalfExtents.Z = halfExtents.Z + IntersectionEpsilon;
-            if ((a.X > expandedHalfExtents.X && b.X > expandedHalfExtents.X && c.X > expandedHalfExtents.X) ||
-                (a.Y > expandedHalfExtents.Y && b.Y > expandedHalfExtents.Y && c.Y > expandedHalfExtents.Y) ||
-                (a.Z > expandedHalfExtents.Z && b.Z > expandedHalfExtents.Z && c.Z > expandedHalfExtents.Z) ||
-                (a.X < -expandedHalfExtents.X && b.X < -expandedHalfExtents.X && c.X < -expandedHalfExtents.X) ||
-                (a.Y < -expandedHalfExtents.Y && b.Y < -expandedHalfExtents.Y && c.Y < -expandedHalfExtents.Y) ||
-                (a.Z < -expandedHalfExtents.Z && b.Z < -expandedHalfExtents.Z && c.Z < -expandedHalfExtents.Z))
-            {
-                return false;
-            }
+            //Ensure that the normal points away from the origin (direction choice is arbitrary, just need to be consistent).
+            normal = -normal;
+            d = -d;
+        }
 
-            //Test the triangle face.
-            //Note that we don't use the axis overlap test here.
-            //We can do better since we know that all triangle vertices have the same value.
-            var ab = b - a;
-            var ac = c - a;
-            var normal = Vector3.Cross(ab, ac);
-            var d = Vector3.Dot(normal, a);
-            if (d < 0)
-            {
-                //Ensure that the normal points away from the origin (direction choice is arbitrary, just need to be consistent).
-                normal = -normal;
-                d = -d;
-            }
+        Vector3 boxExtremePoint;
+        if (normal.X > 0)
+            boxExtremePoint.X = halfExtents.X;
+        else
+            boxExtremePoint.X = -halfExtents.X;
+
+        if (normal.Y > 0)
+            boxExtremePoint.Y = halfExtents.Y;
+        else
+            boxExtremePoint.Y = -halfExtents.Y;
 
-            Vector3 boxExtremePoint;
-            if (normal.X > 0)
-                boxExtremePoint.X = halfExtents.X;
-            else
-                boxExtremePoint.X = -halfExtents.X;
+        if (normal.Z > 0)
+            boxExtremePoint.Z = halfExtents.Z;
+        else
+            boxExtremePoint.Z = -halfExtents.Z;
 
-            if (normal.Y > 0)
-                boxExtremePoint.Y = halfExtents.Y;
-            else
-                boxExtremePoint.Y = -halfExtents.Y;
+        float extremePointDot = Vector3.Dot(boxExtremePoint, normal);
+        if (extremePointDot + IntersectionEpsilon < d)
+        {
+            //No collision.
+            return false;
+        }
 
-            if (normal.Z > 0)
-                boxExtremePoint.Z = halfExtents.Z;
-            else
-                boxExtremePoint.Z = -halfExtents.Z;
+        //Test every edge direction.
+        //The three box directions all have two zeroes and one one, so the cross product simplifies a lot.
+        var bc = c - b;
+        Vector3 direction;
+        //(1,0,0) x ab:
+        direction = new Vector3(0, -ab.Z, ab.Y);
+        if (!OverlapsAlongAxis(ref direction, ref halfExtents, ref a, ref b, ref c))
+            return false;
+        //(1,0,0) x ac
+        direction = new Vector3(0, -ac.Z, ac.Y);
+        if (!OverlapsAlongAxis(ref direction, ref halfExtents, ref a, ref b, ref c))
+            return false;
+        //(1,0,0) x bc:
+        direction = new Vector3(0, -bc.Z, bc.Y);
+        if (!OverlapsAlongAxis(ref direction, ref halfExtents, ref a, ref b, ref c))
+            return false;
 
-            float extremePointDot = Vector3.Dot(boxExtremePoint, normal);
-            if (extremePointDot + IntersectionEpsilon < d)
-            {
-                //No collision.
-                return false;
-            }
+        //(0,1,0) x ab:
+        direction = new Vector3(ab.Z, 0, -ab.X);
+        if (!OverlapsAlongAxis(ref direction, ref halfExtents, ref a, ref b, ref c))
+            return false;
+        //(0,1,0) x ac
+        direction = new Vector3(ac.Z, 0, -ac.X);
+        if (!OverlapsAlongAxis(ref direction, ref halfExtents, ref a, ref b, ref c))
+            return false;
+        //(0,1,0) x bc:
+        direction = new Vector3(bc.Z, 0, -bc.X);
+        if (!OverlapsAlongAxis(ref direction, ref halfExtents, ref a, ref b, ref c))
+            return false;
 
-            //Test every edge direction.
-            //The three box directions all have two zeroes and one one, so the cross product simplifies a lot.
-            var bc = c - b;
-            Vector3 direction;
-            //(1,0,0) x ab:
-            direction = new Vector3(0, -ab.Z, ab.Y);
-            if (!OverlapsAlongAxis(ref direction, ref halfExtents, ref a, ref b, ref c))
-                return false;
-            //(1,0,0) x ac
-            direction = new Vector3(0, -ac.Z, ac.Y);
-            if (!OverlapsAlongAxis(ref direction, ref halfExtents, ref a, ref b, ref c))
-                return false;
-            //(1,0,0) x bc:
-            direction = new Vector3(0, -bc.Z, bc.Y);
-            if (!OverlapsAlongAxis(ref direction, ref halfExtents, ref a, ref b, ref c))
-                return false;
-
-            //(0,1,0) x ab:
-            direction = new Vector3(ab.Z, 0, -ab.X);
-            if (!OverlapsAlongAxis(ref direction, ref halfExtents, ref a, ref b, ref c))
-                return false;
-            //(0,1,0) x ac
-            direction = new Vector3(ac.Z, 0, -ac.X);
-            if (!OverlapsAlongAxis(ref direction, ref halfExtents, ref a, ref b, ref c))
-                return false;
-            //(0,1,0) x bc:
-            direction = new Vector3(bc.Z, 0, -bc.X);
-            if (!OverlapsAlongAxis(ref direction, ref halfExtents, ref a, ref b, ref c))
-                return false;
-
-            //(0,0,1) x ab:
-            direction = new Vector3(-ab.Y, ab.X, 0);
-            if (!OverlapsAlongAxis(ref direction, ref halfExtents, ref a, ref b, ref c))
-                return false;
-            //(0,0,1) x ac
-            direction = new Vector3(-ac.Y, ac.X, 0);
-            if (!OverlapsAlongAxis(ref direction, ref halfExtents, ref a, ref b, ref c))
-                return false;
-            //(0,0,1) x bc:
-            direction = new Vector3(-bc.Y, bc.X, 0);
-            if (!OverlapsAlongAxis(ref direction, ref halfExtents, ref a, ref b, ref c))
-                return false;
+        //(0,0,1) x ab:
+        direction = new Vector3(-ab.Y, ab.X, 0);
+        if (!OverlapsAlongAxis(ref direction, ref halfExtents, ref a, ref b, ref c))
+            return false;
+        //(0,0,1) x ac
+        direction = new Vector3(-ac.Y, ac.X, 0);
+        if (!OverlapsAlongAxis(ref direction, ref halfExtents, ref a, ref b, ref c))
+            return false;
+        //(0,0,1) x bc:
+        direction = new Vector3(-bc.Y, bc.X, 0);
+        if (!OverlapsAlongAxis(ref direction, ref halfExtents, ref a, ref b, ref c))
+            return false;
 
-            return true;
-        }
+        return true;
     }
+}
 
-    internal static class TriangleRasterizer
+internal static class TriangleRasterizer
+{
+    public static void RasterizeTriangle(ref Vector3 a, ref Vector3 b, ref Vector3 c, float cellSize, ref Vector3 gridOrigin, BufferPool pool, ref QuickSet<Cell, CellComparer> cells)
     {
-        public static void RasterizeTriangle(ref Vector3 a, ref Vector3 b, ref Vector3 c, float cellSize, ref Vector3 gridOrigin, BufferPool pool, ref QuickSet<Cell, CellComparer> cells)
+        var gridA = a - gridOrigin;
+        var gridB = b - gridOrigin;
+        var gridC = c - gridOrigin;
+
+        //Compute the bounding box of the triangle.
+        var max = Vector3.Max(Vector3.Max(gridA, gridB), gridC);
+        var min = Vector3.Min(Vector3.Min(gridA, gridB), gridC);
+
+        var epsilon = new Vector3(1e-5f);
+        min -= epsilon;
+        max += epsilon;
+
+        //Discretize the bounding box.
+        //All indices are positive, so we can just truncate.
+        int startX, endX, startY, endY, startZ, endZ;
+        float inverseCellSize = 1f / cellSize;
+        startX = (int)Math.Floor(min.X * inverseCellSize);
+        endX = (int)Math.Floor(max.X * inverseCellSize);
+        startY = (int)Math.Floor(min.Y * inverseCellSize);
+        endY = (int)Math.Floor(max.Y * inverseCellSize);
+        startZ = (int)Math.Floor(min.Z * inverseCellSize);
+        endZ = (int)Math.Floor(max.Z * inverseCellSize);
+
+        //Test the triangle against each cell.
+        var halfExtents = new Vector3(cellSize * 0.5f);
+        for (int i = startX; i <= endX; ++i)
         {
-            var gridA = a - gridOrigin;
-            var gridB = b - gridOrigin;
-            var gridC = c - gridOrigin;
-
-            //Compute the bounding box of the triangle.
-            var max = Vector3.Max(Vector3.Max(gridA, gridB), gridC);
-            var min = Vector3.Min(Vector3.Min(gridA, gridB), gridC);
-
-            var epsilon = new Vector3(1e-5f);
-            min -= epsilon;
-            max += epsilon;
-
-            //Discretize the bounding box.
-            //All indices are positive, so we can just truncate.
-            int startX, endX, startY, endY, startZ, endZ;
-            float inverseCellSize = 1f / cellSize;
-            startX = (int)Math.Floor(min.X * inverseCellSize);
-            endX = (int)Math.Floor(max.X * inverseCellSize);
-            startY = (int)Math.Floor(min.Y * inverseCellSize);
-            endY = (int)Math.Floor(max.Y * inverseCellSize);
-            startZ = (int)Math.Floor(min.Z * inverseCellSize);
-            endZ = (int)Math.Floor(max.Z * inverseCellSize);
-
-            //Test the triangle against each cell.
-            var halfExtents = new Vector3(cellSize * 0.5f);
-            for (int i = startX; i <= endX; ++i)
+            for (int j = startY; j <= endY; ++j)
             {
-                for (int j = startY; j <= endY; ++j)
+                for (int k = startZ; k <= endZ; ++k)
                 {
-                    for (int k = startZ; k <= endZ; ++k)
+                    var cellIndex = new Vector3(i, j, k);
+                    var cellOrigin = cellSize * cellIndex + halfExtents;
+                    var shiftedA = gridA - cellOrigin;
+                    var shiftedB = gridB - cellOrigin;
+                    var shiftedC = gridC - cellOrigin;
+
+                    if (BoxTriangleCollider.Intersecting(ref halfExtents, ref shiftedA, ref shiftedB, ref shiftedC))
                     {
-                        var cellIndex = new Vector3(i, j, k);
-                        var cellOrigin = cellSize * cellIndex + halfExtents;
-                        var shiftedA = gridA - cellOrigin;
-                        var shiftedB = gridB - cellOrigin;
-                        var shiftedC = gridC - cellOrigin;
-
-                        if (BoxTriangleCollider.Intersecting(ref halfExtents, ref shiftedA, ref shiftedB, ref shiftedC))
-                        {
-                            cells.Add(new Cell { X = i, Y = j, Z = k }, pool);
-                        }
+                        cells.Add(new Cell { X = i, Y = j, Z = k }, pool);
                     }
                 }
             }
         }
+    }
+
+}
+public struct CellVertexIndices
+{
+    public int V000, V001, V010, V011, V100, V101, V110, V111;
+
+
+}
 
+public struct CellComparer : IEqualityComparerRef<Cell>
+{
+    [MethodImpl(MethodImplOptions.AggressiveInlining)]
+    public bool Equals(ref Cell a, ref Cell b)
+    {
+        return a.X == b.X && a.Y == b.Y && a.Z == b.Z;
     }
-    public struct CellVertexIndices
+
+    [MethodImpl(MethodImplOptions.AggressiveInlining)]
+    public int Hash(ref Cell cell)
     {
-        public int V000, V001, V010, V011, V100, V101, V110, V111;
+        return (452930477 * cell.X) ^ (122949829 * cell.Y) ^ (654188429 * cell.Z);
+    }
+}
 
 
-    }
+public struct Cell
+{
+    public int X, Y, Z;
+}
+
+public struct TetrahedronVertices
+{
+    public readonly int A, B, C, D;
 
-    public struct CellComparer : IEqualityComparerRef<Cell>
+    public TetrahedronVertices(int a, int b, int c, int d)
     {
-        [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public bool Equals(ref Cell a, ref Cell b)
-        {
-            return a.X == b.X && a.Y == b.Y && a.Z == b.Z;
-        }
+        A = a;
+        B = b;
+        C = c;
+        D = d;
+    }
+}
 
-        [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public int Hash(ref Cell cell)
+//Why dumb? Because in the original project, there was a less dumb variant. The less dumb variant was, unfortunately, way more complicated, so I didn't copy it over.
+//It's also pretty darn slow with denser cell sizes.
+public static class DumbTetrahedralizer
+{
+    private static void AddVertexSpatialIndex(ref Cell vertexSpatialIndex, BufferPool pool, ref CellSet vertexIndices, out int index)
+    {
+        index = vertexIndices.IndexOf(vertexSpatialIndex);
+        if (index < 0)
         {
-            return (452930477 * cell.X) ^ (122949829 * cell.Z) ^ (654188429 * cell.Z);
+            index = vertexIndices.Count;
+            vertexIndices.Add(vertexSpatialIndex, pool);
         }
     }
 
-
-    public struct Cell
+    private struct VoxelizationBounds
     {
-        public int X, Y, Z;
+        /// <summary>
+        /// Exclusive maximum voxel index along the X axis.
+        /// </summary>
+        public int X;
+        /// <summary>
+        /// Exclusive maximum voxel index along the Y axis.
+        /// </summary>
+        public int Y;
+        /// <summary>
+        /// Exclusive maximum voxel index along the Z axis.
+        /// </summary>
+        public int Z;
     }
 
-    public struct TetrahedronVertices
+    private static bool TryFloodFill(Cell cell, ref VoxelizationBounds bounds, BufferPool pool, ref CellSet occupiedCells, ref CellSet newlyFilledCells, ref CellList cellsToVisit)
     {
-        public readonly int A, B, C, D;
-
-        public TetrahedronVertices(int a, int b, int c, int d)
+        if (cell.X > bounds.X || cell.Y > bounds.Y || cell.Z > bounds.Z || cell.X < -1 || cell.Y < -1 || cell.Z < -1)
         {
-            A = a;
-            B = b;
-            C = c;
-            D = d;
+            //We've escaped the world; the start location was not inside a closed section. Abandon the flood fill.
+            return false;
         }
+        if (newlyFilledCells.Contains(cell) || occupiedCells.Contains(cell))
+        {
+            //We already traversed this cell before or during the current flood fill.
+            return true;
+        }
+
+        newlyFilledCells.Add(cell, pool);
+
+        cellsToVisit.Add(new Cell { X = cell.X, Y = cell.Y, Z = cell.Z - 1 }, pool);
+        cellsToVisit.Add(new Cell { X = cell.X, Y = cell.Y, Z = cell.Z + 1 }, pool);
+        cellsToVisit.Add(new Cell { X = cell.X, Y = cell.Y - 1, Z = cell.Z }, pool);
+        cellsToVisit.Add(new Cell { X = cell.X, Y = cell.Y + 1, Z = cell.Z }, pool);
+        cellsToVisit.Add(new Cell { X = cell.X - 1, Y = cell.Y, Z = cell.Z }, pool);
+        cellsToVisit.Add(new Cell { X = cell.X + 1, Y = cell.Y, Z = cell.Z }, pool);
+
+        return true;
     }
 
-    //Why dumb? Because in the original project, there was a less dumb variant. The less dumb variant was, unfortunately, way more complicated, so I didn't copy it over.
-    //It's also pretty darn slow with denser cell sizes.
-    public static class DumbTetrahedralizer
+    static void InitiateFloodFill(Cell cell, ref VoxelizationBounds bounds, BufferPool pool, ref CellSet occupiedCells, ref CellSet newlyFilledCells, ref CellList cellsToVisit)
     {
-        private static void AddVertexSpatialIndex(ref Cell vertexSpatialIndex, BufferPool pool, ref CellSet vertexIndices, out int index)
+        //Check to make sure that this cell isn't already occupied before starting a new fill.
+        if (occupiedCells.Contains(cell))
+            return;
+        cellsToVisit.Add(cell, pool);
+        while (cellsToVisit.Count > 0)
         {
-            index = vertexIndices.IndexOf(vertexSpatialIndex);
-            if (index < 0)
+            if (cellsToVisit.TryPop(out cell))
             {
-                index = vertexIndices.Count;
-                vertexIndices.Add(vertexSpatialIndex, pool);
+                if (!TryFloodFill(cell, ref bounds, pool, ref occupiedCells, ref newlyFilledCells, ref cellsToVisit))
+                {
+                    //The flood fill escaped the voxel bounds. Must be an open area; don't fill.
+                    cellsToVisit.Clear();
+                    newlyFilledCells.Clear();
+                    return;
+                }
             }
         }
-
-        private struct VoxelizationBounds
+        //Flood fill completed without reaching the voxel bounds. Dump newly filled cells.
+        for (int i = 0; i < newlyFilledCells.Count; ++i)
         {
-            /// <summary>
-            /// Exclusive maximum voxel index along the X axis.
-            /// </summary>
-            public int X;
-            /// <summary>
-            /// Exclusive maximum voxel index along the Y axis.
-            /// </summary>
-            public int Y;
-            /// <summary>
-            /// Exclusive maximum voxel index along the Z axis.
-            /// </summary>
-            public int Z;
+            occupiedCells.Add(newlyFilledCells[i], pool);
         }
+        newlyFilledCells.Clear();
+    }
 
-        private static bool TryFloodFill(Cell cell, ref VoxelizationBounds bounds, BufferPool pool, ref CellSet occupiedCells, ref CellSet newlyFilledCells, ref CellList cellsToVisit)
-        {
-            if (cell.X > bounds.X || cell.Y > bounds.Y || cell.Z > bounds.Z || cell.X < -1 || cell.Y < -1 || cell.Z < -1)
-            {
-                //We've escaped the world; the start location was not inside a closed section. Abandon the flood fill.
-                return false;
-            }
-            if (newlyFilledCells.Contains(cell) || occupiedCells.Contains(cell))
-            {
-                //We already traversed this cell before or during the current flood fill.
-                return true;
-            }
+    private static void FloodFillAdjacentCells(Cell cell, ref VoxelizationBounds bounds, BufferPool pool, ref CellSet occupiedCells, ref CellSet newlyFilledCells, ref CellList cellsToVisit)
+    {
+        InitiateFloodFill(new Cell { X = cell.X + 1, Y = cell.Y, Z = cell.Z }, ref bounds, pool, ref occupiedCells, ref newlyFilledCells, ref cellsToVisit);
+        InitiateFloodFill(new Cell { X = cell.X - 1, Y = cell.Y, Z = cell.Z }, ref bounds, pool, ref occupiedCells, ref newlyFilledCells, ref cellsToVisit);
+        InitiateFloodFill(new Cell { X = cell.X, Y = cell.Y + 1, Z = cell.Z }, ref bounds, pool, ref occupiedCells, ref newlyFilledCells, ref cellsToVisit);
+        InitiateFloodFill(new Cell { X = cell.X, Y = cell.Y - 1, Z = cell.Z }, ref bounds, pool, ref occupiedCells, ref newlyFilledCells, ref cellsToVisit);
+        InitiateFloodFill(new Cell { X = cell.X, Y = cell.Y, Z = cell.Z + 1 }, ref bounds, pool, ref occupiedCells, ref newlyFilledCells, ref cellsToVisit);
+        InitiateFloodFill(new Cell { X = cell.X, Y = cell.Y, Z = cell.Z - 1 }, ref bounds, pool, ref occupiedCells, ref newlyFilledCells, ref cellsToVisit);
+    }
 
-            newlyFilledCells.Add(cell, pool);
 
-            cellsToVisit.Add(new Cell { X = cell.X, Y = cell.Y, Z = cell.Z - 1 }, pool);
-            cellsToVisit.Add(new Cell { X = cell.X, Y = cell.Y, Z = cell.Z + 1 }, pool);
-            cellsToVisit.Add(new Cell { X = cell.X, Y = cell.Y - 1, Z = cell.Z }, pool);
-            cellsToVisit.Add(new Cell { X = cell.X, Y = cell.Y + 1, Z = cell.Z }, pool);
-            cellsToVisit.Add(new Cell { X = cell.X - 1, Y = cell.Y, Z = cell.Z }, pool);
-            cellsToVisit.Add(new Cell { X = cell.X + 1, Y = cell.Y, Z = cell.Z }, pool);
 
-            return true;
+    public static void Tetrahedralize(Span<TriangleContent> triangles, float cellSize, BufferPool pool,
+        out Buffer<Vector3> vertices, out CellSet vertexSpatialIndices, out Buffer<CellVertexIndices> cellVertexIndices, out Buffer<TetrahedronVertices> tetrahedraVertexIndices)
+    {
+        //Compute the size of the 3d grid by scanning all vertices.
+        Vector3 min = new(float.MaxValue), max = new(float.MinValue);
+        for (int i = 0; i < triangles.Length; ++i)
+        {
+            ref var triangle = ref triangles[i];
+            min = Vector3.Min(min, triangle.A);
+            min = Vector3.Min(min, triangle.B);
+            min = Vector3.Min(min, triangle.C);
+            max = Vector3.Max(max, triangle.A);
+            max = Vector3.Max(max, triangle.B);
+            max = Vector3.Max(max, triangle.C);
         }
+        //Add a little buffer.
+        var buffer = new Vector3(cellSize);
+        min -= buffer;
 
-        static void InitiateFloodFill(Cell cell, ref VoxelizationBounds bounds, BufferPool pool, ref CellSet occupiedCells, ref CellSet newlyFilledCells, ref CellList cellsToVisit)
+        var cells = new CellSet(triangles.Length, pool);
+        for (int i = 0; i < triangles.Length; ++i)
         {
-            //Check to make sure that this cell isn't already occupied before starting a new fill.
-            if (occupiedCells.Contains(cell))
-                return;
-            cellsToVisit.Add(cell, pool);
-            while (cellsToVisit.Count > 0)
-            {
-                if (cellsToVisit.TryPop(out cell))
-                {
-                    if (!TryFloodFill(cell, ref bounds, pool, ref occupiedCells, ref newlyFilledCells, ref cellsToVisit))
-                    {
-                        //The flood fill escaped the voxel bounds. Must be an open area; don't fill.
-                        cellsToVisit.Clear();
-                        newlyFilledCells.Clear();
-                        return;
-                    }
-                }
-            }
-            //Flood fill completed without reaching the voxel bounds. Dump newly filled cells.
-            for (int i = 0; i < newlyFilledCells.Count; ++i)
-            {
-                occupiedCells.Add(newlyFilledCells[i], pool);
-            }
-            newlyFilledCells.Clear();
+            ref var triangle = ref triangles[i];
+            //Rasterize each triangle onto the grid.
+            TriangleRasterizer.RasterizeTriangle(ref triangle.A, ref triangle.B, ref triangle.C, cellSize, ref min, pool, ref cells);
+
         }
 
-        private static void FloodFillAdjacentCells(Cell cell, ref VoxelizationBounds bounds, BufferPool pool, ref CellSet occupiedCells, ref CellSet newlyFilledCells, ref CellList cellsToVisit)
+        if (cells.Count == 0)
+            throw new ArgumentException("Mesh seems to have no volume; triangle rasterization occupied no cells.");
+
+        VoxelizationBounds bounds;
+        Vector3 size = max - min;
+        float inverseCellSize = 1f / cellSize;
+        bounds.X = (int)(Math.Ceiling(inverseCellSize * size.X));
+        bounds.Y = (int)(Math.Ceiling(inverseCellSize * size.Y));
+        bounds.Z = (int)(Math.Ceiling(inverseCellSize * size.Z));
+        //Perform a flood fill on every surface vertex.
+        //We can use the cells set directly, since it behaves like a regular list with regard to element placement (always at the end).
+        var floodFilledCells = new CellSet(32, pool);
+        var cellsToVisit = new CellList(32, pool);
+        for (int i = cells.Count - 1; i >= 0; --i)
         {
-            InitiateFloodFill(new Cell { X = cell.X + 1, Y = cell.Y, Z = cell.Z }, ref bounds, pool, ref occupiedCells, ref newlyFilledCells, ref cellsToVisit);
-            InitiateFloodFill(new Cell { X = cell.X - 1, Y = cell.Y, Z = cell.Z }, ref bounds, pool, ref occupiedCells, ref newlyFilledCells, ref cellsToVisit);
-            InitiateFloodFill(new Cell { X = cell.X, Y = cell.Y + 1, Z = cell.Z }, ref bounds, pool, ref occupiedCells, ref newlyFilledCells, ref cellsToVisit);
-            InitiateFloodFill(new Cell { X = cell.X, Y = cell.Y - 1, Z = cell.Z }, ref bounds, pool, ref occupiedCells, ref newlyFilledCells, ref cellsToVisit);
-            InitiateFloodFill(new Cell { X = cell.X, Y = cell.Y, Z = cell.Z + 1 }, ref bounds, pool, ref occupiedCells, ref newlyFilledCells, ref cellsToVisit);
-            InitiateFloodFill(new Cell { X = cell.X, Y = cell.Y, Z = cell.Z - 1 }, ref bounds, pool, ref occupiedCells, ref newlyFilledCells, ref cellsToVisit);
+            ref var cell = ref cells[i];
+            FloodFillAdjacentCells(cell, ref bounds, pool, ref cells, ref floodFilledCells, ref cellsToVisit);
         }
 
+        //Build the vertex list and per-cell vertex index lists.
+        vertexSpatialIndices = new CellSet(cells.Count * 4, pool);
+        int cellIndex = 0;
+        pool.Take(cells.Count, out cellVertexIndices);
+        for (int i = 0; i < cells.Count; ++i)
+        {
+            ref var cell = ref cells[i];
+            CellVertexIndices cellIndices;
+            var vertexSpatialIndex = cell;
+            AddVertexSpatialIndex(ref vertexSpatialIndex, pool, ref vertexSpatialIndices, out cellIndices.V000);
+            vertexSpatialIndex.X = cell.X;
+            vertexSpatialIndex.Y = cell.Y;
+            vertexSpatialIndex.Z = cell.Z + 1;
+            AddVertexSpatialIndex(ref vertexSpatialIndex, pool, ref vertexSpatialIndices, out cellIndices.V001);
+            vertexSpatialIndex.X = cell.X;
+            vertexSpatialIndex.Y = cell.Y + 1;
+            vertexSpatialIndex.Z = cell.Z;
+            AddVertexSpatialIndex(ref vertexSpatialIndex, pool, ref vertexSpatialIndices, out cellIndices.V010);
+            vertexSpatialIndex.X = cell.X;
+            vertexSpatialIndex.Y = cell.Y + 1;
+            vertexSpatialIndex.Z = cell.Z + 1;
+            AddVertexSpatialIndex(ref vertexSpatialIndex, pool, ref vertexSpatialIndices, out cellIndices.V011);
+            vertexSpatialIndex.X = cell.X + 1;
+            vertexSpatialIndex.Y = cell.Y;
+            vertexSpatialIndex.Z = cell.Z;
+            AddVertexSpatialIndex(ref vertexSpatialIndex, pool, ref vertexSpatialIndices, out cellIndices.V100);
+            vertexSpatialIndex.X = cell.X + 1;
+            vertexSpatialIndex.Y = cell.Y;
+            vertexSpatialIndex.Z = cell.Z + 1;
+            AddVertexSpatialIndex(ref vertexSpatialIndex, pool, ref vertexSpatialIndices, out cellIndices.V101);
+            vertexSpatialIndex.X = cell.X + 1;
+            vertexSpatialIndex.Y = cell.Y + 1;
+            vertexSpatialIndex.Z = cell.Z;
+            AddVertexSpatialIndex(ref vertexSpatialIndex, pool, ref vertexSpatialIndices, out cellIndices.V110);
+            vertexSpatialIndex.X = cell.X + 1;
+            vertexSpatialIndex.Y = cell.Y + 1;
+            vertexSpatialIndex.Z = cell.Z + 1;
+            AddVertexSpatialIndex(ref vertexSpatialIndex, pool, ref vertexSpatialIndices, out cellIndices.V111);
+
+            cellVertexIndices[cellIndex++] = cellIndices;
+        }
 
-
-        public static void Tetrahedralize(Span<TriangleContent> triangles, float cellSize, BufferPool pool,
-            out Buffer<Vector3> vertices, out CellSet vertexSpatialIndices, out Buffer<CellVertexIndices> cellVertexIndices, out Buffer<TetrahedronVertices> tetrahedraVertexIndices)
+        //Create the tetrahedra.
+        var tetrahedraCount = cellVertexIndices.Length * 5;
+        pool.Take(tetrahedraCount, out tetrahedraVertexIndices);
+        int tetrahedronIndex = 0;
+        for (int i = 0; i < cellVertexIndices.Length; ++i)
         {
-            //Compute the size of the 3d grid by scanning all vertices.
-            Vector3 min = new(float.MaxValue), max = new(float.MinValue);
-            for (int i = 0; i < triangles.Length; ++i)
-            {
-                ref var triangle = ref triangles[i];
-                min = Vector3.Min(min, triangle.A);
-                min = Vector3.Min(min, triangle.B);
-                min = Vector3.Min(min, triangle.C);
-                max = Vector3.Max(max, triangle.A);
-                max = Vector3.Max(max, triangle.B);
-                max = Vector3.Max(max, triangle.C);
-            }
-            //Add a little buffer.
-            var buffer = new Vector3(cellSize);
-            min -= buffer;
+            var cellIndices = cellVertexIndices[i];
+            tetrahedraVertexIndices[tetrahedronIndex++] = new TetrahedronVertices(cellIndices.V010, cellIndices.V111, cellIndices.V001, cellIndices.V100); //Central tetrahedron
+            tetrahedraVertexIndices[tetrahedronIndex++] = new TetrahedronVertices(cellIndices.V000, cellIndices.V001, cellIndices.V010, cellIndices.V100); //Origin tetrahedron
+            tetrahedraVertexIndices[tetrahedronIndex++] = new TetrahedronVertices(cellIndices.V010, cellIndices.V100, cellIndices.V111, cellIndices.V110);
+            tetrahedraVertexIndices[tetrahedronIndex++] = new TetrahedronVertices(cellIndices.V010, cellIndices.V001, cellIndices.V111, cellIndices.V011);
+            tetrahedraVertexIndices[tetrahedronIndex++] = new TetrahedronVertices(cellIndices.V101, cellIndices.V001, cellIndices.V100, cellIndices.V111);
+        }
 
-            var cells = new CellSet(triangles.Length, pool);
-            for (int i = 0; i < triangles.Length; ++i)
-            {
-                ref var triangle = ref triangles[i];
-                //Rasterize each triangle onto the grid.
-                TriangleRasterizer.RasterizeTriangle(ref triangle.A, ref triangle.B, ref triangle.C, cellSize, ref min, pool, ref cells);
+        //Create the vertices.
+        pool.Take(vertexSpatialIndices.Count, out vertices);
+        for (int i = 0; i < vertices.Length; ++i)
+        {
+            ref var index = ref vertexSpatialIndices[i];
+            vertices[i] = new Vector3(index.X, index.Y, index.Z) * cellSize + min;
+        }
 
-            }
 
-            if (cells.Count == 0)
-                throw new ArgumentException("Mesh seems to have no volume; triangle rasterization occupied no cells.");
-
-            VoxelizationBounds bounds;
-            Vector3 size = max - min;
-            float inverseCellSize = 1f / cellSize;
-            bounds.X = (int)(Math.Ceiling(inverseCellSize * size.X));
-            bounds.Y = (int)(Math.Ceiling(inverseCellSize * size.Y));
-            bounds.Z = (int)(Math.Ceiling(inverseCellSize * size.Z));
-            //Perform a flood fill on every surface vertex.
-            //We can use the cells set directly, since it behaves like a regular list with regard to element placement (always at the end).
-            var floodFilledCells = new CellSet(32, pool);
-            var cellsToVisit = new CellList(32, pool);
-            for (int i = cells.Count - 1; i >= 0; --i)
-            {
-                ref var cell = ref cells[i];
-                FloodFillAdjacentCells(cell, ref bounds, pool, ref cells, ref floodFilledCells, ref cellsToVisit);
-            }
+        //We can fail to dispose the quick collections. All of the buffers are getting GC'd anyway.
+        cells.Dispose(pool);
+        floodFilledCells.Dispose(pool);
+    }
+}
 
-            //Build the vertex list and per-cell vertex index lists.
-            vertexSpatialIndices = new CellSet(cells.Count * 4, pool);
-            int cellIndex = 0;
-            pool.Take(cells.Count, out cellVertexIndices);
-            for (int i = 0; i < cells.Count; ++i)
-            {
-                ref var cell = ref cells[i];
-                CellVertexIndices cellIndices;
-                var vertexSpatialIndex = cell;
-                AddVertexSpatialIndex(ref vertexSpatialIndex, pool, ref vertexSpatialIndices, out cellIndices.V000);
-                vertexSpatialIndex.X = cell.X;
-                vertexSpatialIndex.Y = cell.Y;
-                vertexSpatialIndex.Z = cell.Z + 1;
-                AddVertexSpatialIndex(ref vertexSpatialIndex, pool, ref vertexSpatialIndices, out cellIndices.V001);
-                vertexSpatialIndex.X = cell.X;
-                vertexSpatialIndex.Y = cell.Y + 1;
-                vertexSpatialIndex.Z = cell.Z;
-                AddVertexSpatialIndex(ref vertexSpatialIndex, pool, ref vertexSpatialIndices, out cellIndices.V010);
-                vertexSpatialIndex.X = cell.X;
-                vertexSpatialIndex.Y = cell.Y + 1;
-                vertexSpatialIndex.Z = cell.Z + 1;
-                AddVertexSpatialIndex(ref vertexSpatialIndex, pool, ref vertexSpatialIndices, out cellIndices.V011);
-                vertexSpatialIndex.X = cell.X + 1;
-                vertexSpatialIndex.Y = cell.Y;
-                vertexSpatialIndex.Z = cell.Z;
-                AddVertexSpatialIndex(ref vertexSpatialIndex, pool, ref vertexSpatialIndices, out cellIndices.V100);
-                vertexSpatialIndex.X = cell.X + 1;
-                vertexSpatialIndex.Y = cell.Y;
-                vertexSpatialIndex.Z = cell.Z + 1;
-                AddVertexSpatialIndex(ref vertexSpatialIndex, pool, ref vertexSpatialIndices, out cellIndices.V101);
-                vertexSpatialIndex.X = cell.X + 1;
-                vertexSpatialIndex.Y = cell.Y + 1;
-                vertexSpatialIndex.Z = cell.Z;
-                AddVertexSpatialIndex(ref vertexSpatialIndex, pool, ref vertexSpatialIndices, out cellIndices.V110);
-                vertexSpatialIndex.X = cell.X + 1;
-                vertexSpatialIndex.Y = cell.Y + 1;
-                vertexSpatialIndex.Z = cell.Z + 1;
-                AddVertexSpatialIndex(ref vertexSpatialIndex, pool, ref vertexSpatialIndices, out cellIndices.V111);
-
-                cellVertexIndices[cellIndex++] = cellIndices;
-            }
+struct DeformableCollisionFilter
+{
+    int localIndices;
+    int instanceId;
 
-            //Create the tetrahedra.
-            var tetrahedraCount = cellVertexIndices.Length * 5;
-            pool.Take(tetrahedraCount, out tetrahedraVertexIndices);
-            int tetrahedronIndex = 0;
-            for (int i = 0; i < cellVertexIndices.Length; ++i)
-            {
-                var cellIndices = cellVertexIndices[i];
-                tetrahedraVertexIndices[tetrahedronIndex++] = new TetrahedronVertices(cellIndices.V010, cellIndices.V111, cellIndices.V001, cellIndices.V100); //Central tetrahedron
-                tetrahedraVertexIndices[tetrahedronIndex++] = new TetrahedronVertices(cellIndices.V000, cellIndices.V001, cellIndices.V010, cellIndices.V100); //Origin tetrahedron
-                tetrahedraVertexIndices[tetrahedronIndex++] = new TetrahedronVertices(cellIndices.V010, cellIndices.V100, cellIndices.V111, cellIndices.V110);
-                tetrahedraVertexIndices[tetrahedronIndex++] = new TetrahedronVertices(cellIndices.V010, cellIndices.V001, cellIndices.V111, cellIndices.V011);
-                tetrahedraVertexIndices[tetrahedronIndex++] = new TetrahedronVertices(cellIndices.V101, cellIndices.V001, cellIndices.V100, cellIndices.V111);
-            }
+    [MethodImpl(MethodImplOptions.AggressiveInlining)]
+    public DeformableCollisionFilter(int x, int y, int z, int instanceId)
+    {
+        const int max = 1 << 10;
+        Debug.Assert(x >= 0 && x < max && y >= 0 && y < max && z >= 0 && z < max, "This filter packs local indices, so their range is limited.");
+        localIndices = x | (y << 10) | (z << 20);
+        this.instanceId = instanceId;
+    }
 
-            //Create the vertices.
-            pool.Take(vertexSpatialIndices.Count, out vertices);
-            for (int i = 0; i < vertices.Length; ++i)
-            {
-                ref var index = ref vertexSpatialIndices[i];
-                vertices[i] = new Vector3(index.X, index.Y, index.Z) * cellSize + min;
-            }
+    [MethodImpl(MethodImplOptions.AggressiveInlining)]
+    public static bool Test(in DeformableCollisionFilter a, in DeformableCollisionFilter 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 = 3;
+        const int mask = (1 << 10) - 1;
+        var ax = a.localIndices & mask;
+        var bx = b.localIndices & mask;
+        var differenceX = ax - bx;
+        if (differenceX < -minimumDistance || differenceX > minimumDistance)
+            return true;
+        var ay = (a.localIndices >> 10) & mask;
+        var by = (b.localIndices >> 10) & mask;
+        var differenceY = ay - by;
+        if (differenceY < -minimumDistance || differenceY > minimumDistance)
+            return true;
+        var az = (a.localIndices >> 20) & mask;
+        var bz = (b.localIndices >> 20) & mask;
+        var differenceZ = az - bz;
+        if (differenceZ < -minimumDistance || differenceZ > minimumDistance)
+            return true;
+        return false;
+    }
+}
 
 
-            //We can fail to dispose the quick collections. All of the buffers are getting GC'd anyway.
-            cells.Dispose(pool);
-            floodFilledCells.Dispose(pool);
-        }
+struct DeformableCallbacks : INarrowPhaseCallbacks, Dancers.IDancerNarrowPhaseCallbacks<DeformableCallbacks, DeformableCollisionFilter> //"IDancerNarrowPhaseCallbacks" just means this is a INarrowPhaseCallbacks usable with the DemoDancers.
+{
+    public CollidableProperty<DeformableCollisionFilter> Filters;
+    public PairMaterialProperties Material;
+    /// <summary>
+    /// 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.
+    /// </summary>
+    public int MinimumDistanceForSelfCollisions;
+    public void Initialize(Simulation simulation)
+    {
+        Filters.Initialize(simulation);
     }
 
-    struct DeformableCollisionFilter
+    public DeformableCallbacks(CollidableProperty<DeformableCollisionFilter> filters, PairMaterialProperties material, int minimumDistanceForSelfCollisions = 3)
+    {
+        Filters = filters;
+        Material = material;
+        MinimumDistanceForSelfCollisions = minimumDistanceForSelfCollisions;
+    }
+    public DeformableCallbacks(CollidableProperty<DeformableCollisionFilter> filters, int minimumDistanceForSelfCollisions = 3)
+        : this(filters, new PairMaterialProperties(1, 2, new SpringSettings(30, 1)), minimumDistanceForSelfCollisions)
     {
-        int localIndices;
-        int instanceId;
+    }
+    //This slightly awkward factory is just here for the dancer demos.
+    static DeformableCallbacks Dancers.IDancerNarrowPhaseCallbacks<DeformableCallbacks, DeformableCollisionFilter>.Create(CollidableProperty<DeformableCollisionFilter> filters, PairMaterialProperties pairMaterialProperties, int minimumDistanceForSelfCollisions)
+    {
+        return new DeformableCallbacks(filters, pairMaterialProperties, minimumDistanceForSelfCollisions);
+    }
 
-        [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public DeformableCollisionFilter(int x, int y, int z, int instanceId)
+    [MethodImpl(MethodImplOptions.AggressiveInlining)]
+    public bool AllowContactGeneration(int workerIndex, CollidableReference a, CollidableReference b, ref float speculativeMargin)
+    {
+        if (a.Mobility == CollidableMobility.Dynamic && b.Mobility == CollidableMobility.Dynamic)
         {
-            const int max = 1 << 10;
-            Debug.Assert(x >= 0 && x < max && y >= 0 && y < max && z >= 0 && z < max, "This filter packs local indices, so their range is limited.");
-            localIndices = x | (y << 10) | (z << 20);
-            this.instanceId = instanceId;
+            return DeformableCollisionFilter.Test(Filters[a.BodyHandle], Filters[b.BodyHandle]);
         }
+        return a.Mobility == CollidableMobility.Dynamic || b.Mobility == CollidableMobility.Dynamic;
+    }
 
-        [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public static bool Test(in DeformableCollisionFilter a, in DeformableCollisionFilter 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 = 3;
-            const int mask = (1 << 10) - 1;
-            var ax = a.localIndices & mask;
-            var bx = b.localIndices & mask;
-            var differenceX = ax - bx;
-            if (differenceX < -minimumDistance || differenceX > minimumDistance)
-                return true;
-            var ay = (a.localIndices >> 10) & mask;
-            var by = (b.localIndices >> 10) & mask;
-            var differenceY = ay - by;
-            if (differenceY < -minimumDistance || differenceY > minimumDistance)
-                return true;
-            var az = (a.localIndices >> 20) & mask;
-            var bz = (b.localIndices >> 20) & mask;
-            var differenceZ = az - bz;
-            if (differenceZ < -minimumDistance || differenceZ > minimumDistance)
-                return true;
-            return false;
-        }
+    [MethodImpl(MethodImplOptions.AggressiveInlining)]
+    public bool AllowContactGeneration(int workerIndex, CollidablePair pair, int childIndexA, int childIndexB)
+    {
+        return true;
     }
 
+    [MethodImpl(MethodImplOptions.AggressiveInlining)]
+    public bool ConfigureContactManifold<TManifold>(int workerIndex, CollidablePair pair, ref TManifold manifold, out PairMaterialProperties pairMaterial) where TManifold : unmanaged, IContactManifold<TManifold>
+    {
+        pairMaterial = Material;
+        return true;
+    }
 
-    struct DeformableCallbacks : INarrowPhaseCallbacks
+    [MethodImpl(MethodImplOptions.AggressiveInlining)]
+    public bool ConfigureContactManifold(int workerIndex, CollidablePair pair, int childIndexA, int childIndexB, ref ConvexContactManifold manifold)
     {
-        public CollidableProperty<DeformableCollisionFilter> Filters;
-        public void Initialize(Simulation simulation)
-        {
-            Filters.Initialize(simulation);
-        }
+        return true;
+    }
 
-        [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public bool AllowContactGeneration(int workerIndex, CollidableReference a, CollidableReference b)
-        {
-            if (a.Mobility == CollidableMobility.Dynamic && b.Mobility == CollidableMobility.Dynamic)
-            {
-                return DeformableCollisionFilter.Test(Filters[a.BodyHandle], Filters[b.BodyHandle]);
-            }
-            return a.Mobility == CollidableMobility.Dynamic || b.Mobility == CollidableMobility.Dynamic;
-        }
+    public void Dispose()
+    {
+        Filters.Dispose();
+    }
+}
 
-        [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public bool AllowContactGeneration(int workerIndex, CollidablePair pair, int childIndexA, int childIndexB)
-        {
-            return true;
-        }
+/// <summary>
+/// Some blobs composed of springy welds and volume preservation constraints.
+/// </summary>
+public class NewtDemo : Demo
+{
+    struct Edge : IEqualityComparerRef<Edge>
+    {
+        public int A;
+        public int B;
 
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public unsafe bool ConfigureContactManifold<TManifold>(int workerIndex, CollidablePair pair, ref TManifold manifold, out PairMaterialProperties pairMaterial) where TManifold : unmanaged, IContactManifold<TManifold>
+        public int Hash(ref Edge item)
         {
-            pairMaterial.FrictionCoefficient = 1;
-            pairMaterial.MaximumRecoveryVelocity = 2f;
-            pairMaterial.SpringSettings = new SpringSettings(30, 1);
-            return true;
+            return item.A + item.B;
         }
 
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public unsafe bool ConfigureContactManifold(int workerIndex, CollidablePair pair, int childIndexA, int childIndexB, ref ConvexContactManifold manifold)
+        public bool Equals(ref Edge a, ref Edge b)
         {
-            return true;
+            return (a.A == b.A && a.B == b.B) || (a.B == b.A && a.A == b.B);
         }
+    }
+
 
-        public void Dispose()
+    [MethodImpl(MethodImplOptions.AggressiveInlining)]
+    static void TryAddEdge(int a, int b, ref QuickSet<Edge, Edge> edges, ref Buffer<int> vertexEdgeCounts, BufferPool pool)
+    {
+        if (edges.Add(new Edge { A = a, B = b }, pool))
         {
-            Filters.Dispose();
+            ++vertexEdgeCounts[a];
+            ++vertexEdgeCounts[b];
         }
     }
 
-    /// <summary>
-    /// Some blobs composed of springy welds and volume preservation constraints.
-    /// </summary>
-    public class NewtDemo : Demo
+    private static int CreateTetrahedralUniqueEdgesList(ref Buffer<TetrahedronVertices> tetrahedraVertices,
+        ref Buffer<int> vertexEdgeCounts, BufferPool pool, ref QuickSet<Edge, Edge> cellEdges)
     {
-        struct Edge : IEqualityComparerRef<Edge>
+        for (int i = 0; i < tetrahedraVertices.Length; ++i)
         {
-            public int A;
-            public int B;
-
-            [MethodImpl(MethodImplOptions.AggressiveInlining)]
-            public int Hash(ref Edge item)
-            {
-                return item.A + item.B;
-            }
-
-            [MethodImpl(MethodImplOptions.AggressiveInlining)]
-            public bool Equals(ref Edge a, ref Edge b)
-            {
-                return (a.A == b.A && a.B == b.B) || (a.B == b.A && a.A == b.B);
-            }
+            //Collect all unique hexahedral edges. We're going to stick welds between all of them.
+            ref var tetrahedron = ref tetrahedraVertices[i];
+
+            TryAddEdge(tetrahedron.A, tetrahedron.B, ref cellEdges, ref vertexEdgeCounts, pool);
+            TryAddEdge(tetrahedron.A, tetrahedron.C, ref cellEdges, ref vertexEdgeCounts, pool);
+            TryAddEdge(tetrahedron.A, tetrahedron.D, ref cellEdges, ref vertexEdgeCounts, pool);
+            TryAddEdge(tetrahedron.B, tetrahedron.C, ref cellEdges, ref vertexEdgeCounts, pool);
+            TryAddEdge(tetrahedron.B, tetrahedron.D, ref cellEdges, ref vertexEdgeCounts, pool);
+            TryAddEdge(tetrahedron.C, tetrahedron.D, ref cellEdges, ref vertexEdgeCounts, pool);
         }
+        return 18;
+    }
 
-
-        [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        static void TryAddEdge(int a, int b, ref QuickSet<Edge, Edge> edges, ref Buffer<int> vertexEdgeCounts, BufferPool pool)
+    private static int CreateHexahedralUniqueEdgesList(ref Buffer<CellVertexIndices> cellVertexIndices,
+        ref Buffer<int> vertexEdgeCounts, BufferPool pool, ref QuickSet<Edge, Edge> cellEdges)
+    {
+        for (int i = 0; i < cellVertexIndices.Length; ++i)
         {
-            if (edges.Add(new Edge { A = a, B = b }, pool))
-            {
-                ++vertexEdgeCounts[a];
-                ++vertexEdgeCounts[b];
-            }
+            //Collect all unique hexahedral edges. We're going to stick welds between all of them.
+            ref var cell = ref cellVertexIndices[i];
+            TryAddEdge(cell.V000, cell.V001, ref cellEdges, ref vertexEdgeCounts, pool);
+            TryAddEdge(cell.V000, cell.V010, ref cellEdges, ref vertexEdgeCounts, pool);
+            TryAddEdge(cell.V000, cell.V100, ref cellEdges, ref vertexEdgeCounts, pool);
+            TryAddEdge(cell.V001, cell.V011, ref cellEdges, ref vertexEdgeCounts, pool);
+            TryAddEdge(cell.V001, cell.V101, ref cellEdges, ref vertexEdgeCounts, pool);
+            TryAddEdge(cell.V010, cell.V011, ref cellEdges, ref vertexEdgeCounts, pool);
+            TryAddEdge(cell.V010, cell.V110, ref cellEdges, ref vertexEdgeCounts, pool);
+            TryAddEdge(cell.V011, cell.V111, ref cellEdges, ref vertexEdgeCounts, pool);
+            TryAddEdge(cell.V100, cell.V101, ref cellEdges, ref vertexEdgeCounts, pool);
+            TryAddEdge(cell.V100, cell.V110, ref cellEdges, ref vertexEdgeCounts, pool);
+            TryAddEdge(cell.V101, cell.V111, ref cellEdges, ref vertexEdgeCounts, pool);
+            TryAddEdge(cell.V110, cell.V111, ref cellEdges, ref vertexEdgeCounts, pool);
         }
+        return 6;
+    }
 
-        private static unsafe int CreateTetrahedralUniqueEdgesList(ref Buffer<TetrahedronVertices> tetrahedraVertices,
-            ref Buffer<int> vertexEdgeCounts, BufferPool pool, ref QuickSet<Edge, Edge> cellEdges)
+    internal static void CreateDeformable(Simulation simulation, Vector3 position, Quaternion orientation, float density, float cellSize, in SpringSettings weldSpringiness, in SpringSettings volumeSpringiness, int instanceId, CollidableProperty<DeformableCollisionFilter> filters,
+        ref Buffer<Vector3> vertices, ref CellSet vertexSpatialIndices, ref Buffer<CellVertexIndices> cellVertexIndices, ref Buffer<TetrahedronVertices> tetrahedraVertexIndices)
+    {
+        var pool = simulation.BufferPool;
+        pool.TakeAtLeast<int>(vertices.Length, out var vertexEdgeCounts);
+        vertexEdgeCounts.Clear(0, vertices.Length);
+        var edges = new QuickSet<Edge, Edge>(vertices.Length * 3, pool);
+        var edgeCountForInternalVertex = CreateHexahedralUniqueEdgesList(ref cellVertexIndices, ref vertexEdgeCounts, pool, ref edges);
+        //var edgeCountForInternalVertex = CreateTetrahedralUniqueEdgesList(ref tetrahedraVertexIndices, ref vertexEdgeCounts, ref cellEdgePool, ref intPool, ref edges);
+
+        pool.TakeAtLeast<BodyHandle>(vertices.Length, out var vertexHandles);
+        var vertexShape = new Sphere(cellSize * 0.7f);
+        var massPerVertex = density * (cellSize * cellSize * cellSize);
+        var vertexInertia = vertexShape.ComputeInertia(massPerVertex);
+        var vertexShapeIndex = simulation.Shapes.Add(vertexShape);
+        for (int i = 0; i < vertices.Length; ++i)
         {
-            for (int i = 0; i < tetrahedraVertices.Length; ++i)
-            {
-                //Collect all unique hexahedral edges. We're going to stick welds between all of them.
-                ref var tetrahedron = ref tetrahedraVertices[i];
-
-                TryAddEdge(tetrahedron.A, tetrahedron.B, ref cellEdges, ref vertexEdgeCounts, pool);
-                TryAddEdge(tetrahedron.A, tetrahedron.C, ref cellEdges, ref vertexEdgeCounts, pool);
-                TryAddEdge(tetrahedron.A, tetrahedron.D, ref cellEdges, ref vertexEdgeCounts, pool);
-                TryAddEdge(tetrahedron.B, tetrahedron.C, ref cellEdges, ref vertexEdgeCounts, pool);
-                TryAddEdge(tetrahedron.B, tetrahedron.D, ref cellEdges, ref vertexEdgeCounts, pool);
-                TryAddEdge(tetrahedron.C, tetrahedron.D, ref cellEdges, ref vertexEdgeCounts, pool);
-            }
-            return 18;
+            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];
+            filters.Allocate(vertexHandles[i]) = new DeformableCollisionFilter(vertexSpatialIndex.X, vertexSpatialIndex.Y, vertexSpatialIndex.Z, instanceId);
         }
 
-        private static unsafe int CreateHexahedralUniqueEdgesList(ref Buffer<CellVertexIndices> cellVertexIndices,
-            ref Buffer<int> vertexEdgeCounts, BufferPool pool, ref QuickSet<Edge, Edge> cellEdges)
+        for (int i = 0; i < edges.Count; ++i)
         {
-            for (int i = 0; i < cellVertexIndices.Length; ++i)
-            {
-                //Collect all unique hexahedral edges. We're going to stick welds between all of them.
-                ref var cell = ref cellVertexIndices[i];
-                TryAddEdge(cell.V000, cell.V001, ref cellEdges, ref vertexEdgeCounts, pool);
-                TryAddEdge(cell.V000, cell.V010, ref cellEdges, ref vertexEdgeCounts, pool);
-                TryAddEdge(cell.V000, cell.V100, ref cellEdges, ref vertexEdgeCounts, pool);
-                TryAddEdge(cell.V001, cell.V011, ref cellEdges, ref vertexEdgeCounts, pool);
-                TryAddEdge(cell.V001, cell.V101, ref cellEdges, ref vertexEdgeCounts, pool);
-                TryAddEdge(cell.V010, cell.V011, ref cellEdges, ref vertexEdgeCounts, pool);
-                TryAddEdge(cell.V010, cell.V110, ref cellEdges, ref vertexEdgeCounts, pool);
-                TryAddEdge(cell.V011, cell.V111, ref cellEdges, ref vertexEdgeCounts, pool);
-                TryAddEdge(cell.V100, cell.V101, ref cellEdges, ref vertexEdgeCounts, pool);
-                TryAddEdge(cell.V100, cell.V110, ref cellEdges, ref vertexEdgeCounts, pool);
-                TryAddEdge(cell.V101, cell.V111, ref cellEdges, ref vertexEdgeCounts, pool);
-                TryAddEdge(cell.V110, cell.V111, ref cellEdges, ref vertexEdgeCounts, pool);
-            }
-            return 6;
+            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
+                });
         }
-
-        internal unsafe static void CreateDeformable(Simulation simulation, in Vector3 position, in Quaternion orientation, float density, float cellSize, in SpringSettings weldSpringiness, in SpringSettings volumeSpringiness, int instanceId, CollidableProperty<DeformableCollisionFilter> filters,
-            ref Buffer<Vector3> vertices, ref CellSet vertexSpatialIndices, ref Buffer<CellVertexIndices> cellVertexIndices, ref Buffer<TetrahedronVertices> tetrahedraVertexIndices)
+        //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)
         {
-            var pool = simulation.BufferPool;
-            pool.TakeAtLeast<int>(vertices.Length, out var vertexEdgeCounts);
-            vertexEdgeCounts.Clear(0, vertices.Length);
-            var edges = new QuickSet<Edge, Edge>(vertices.Length * 3, pool);
-            var edgeCountForInternalVertex = CreateHexahedralUniqueEdgesList(ref cellVertexIndices, ref vertexEdgeCounts, pool, ref edges);
-            //var edgeCountForInternalVertex = CreateTetrahedralUniqueEdgesList(ref tetrahedraVertexIndices, ref vertexEdgeCounts, ref cellEdgePool, ref intPool, ref edges);
-
-            pool.TakeAtLeast<BodyHandle>(vertices.Length, out var vertexHandles);
-            var vertexShape = new Sphere(cellSize * 0.7f);
-            var massPerVertex = density * (cellSize * cellSize * cellSize);
-            vertexShape.ComputeInertia(massPerVertex, out var vertexInertia);
-            //vertexInertia.InverseInertiaTensor = default;
-            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,
-                    //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)));
-                ref var vertexSpatialIndex = ref vertexSpatialIndices[i];
-                filters.Allocate(vertexHandles[i]) = new DeformableCollisionFilter(vertexSpatialIndex.X, vertexSpatialIndex.Y, vertexSpatialIndex.Z, instanceId);
-            }
-
-            for (int i = 0; i < edges.Count; ++i)
-            {
-                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));
-            }
-            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);
+            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);
+    }
 
 
+    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;
 
-        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<DeformableCollisionFilter>();
-            //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());
-
-            var meshContent = content.Load<MeshContent>("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)
-            {
-                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 filters = new CollidableProperty<DeformableCollisionFilter>();
+        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));
 
-            BufferPool.Return(ref vertices);
-            vertexSpatialIndices.Dispose(BufferPool);
-            BufferPool.Return(ref cellVertexIndices);
-            BufferPool.Return(ref tetrahedraVertexIndices);
+        var meshContent = content.Load<MeshContent>("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, 1f);
+        var volumeSpringiness = new SpringSettings(30f, 1);
 
-            Simulation.Bodies.Add(BodyDescription.CreateConvexDynamic(new Vector3(0, 100, -.5f), 10, Simulation.Shapes, new Sphere(5)));
+        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);
+        }
+        //Console.WriteLine($"body count: {Simulation.Bodies.ActiveSet.Count}");
+        //Console.WriteLine($"constraint count: {Simulation.Solver.CountConstraints()}");
 
-            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)));
+        BufferPool.Return(ref vertices);
+        vertexSpatialIndices.Dispose(BufferPool);
+        BufferPool.Return(ref cellVertexIndices);
+        BufferPool.Return(ref tetrahedraVertexIndices);
 
-        }
+        //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))));
 
     }
+    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/PerBodyGravityDemo.cs b/Demos/Demos/PerBodyGravityDemo.cs
new file mode 100644
index 000000000..cf48ab9d6
--- /dev/null
+++ b/Demos/Demos/PerBodyGravityDemo.cs
@@ -0,0 +1,164 @@
+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;
+
+
+/// <summary>
+/// Shows how to use custom velocity integration to implement per-body gravity.
+/// </summary>
+public class PerBodyGravityDemo : Demo
+{
+    internal struct PerBodyGravityDemoCallbacks : IPoseIntegratorCallbacks
+    {
+        /// <summary>
+        /// 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.
+        /// </summary>
+        public CollidableProperty<float> BodyGravities;
+        /// <summary>
+        /// Used to look up body handles using the callback-provided body indices.
+        /// </summary>
+        private Bodies bodies;
+
+        public PerBodyGravityDemoCallbacks(CollidableProperty<float> bodyGravities) : this()
+        {
+            BodyGravities = bodyGravities;
+        }
+
+        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<int> bodyIndices, Vector3Wide position, QuaternionWide orientation, BodyInertiaWide localInertia, Vector<int> integrationMask, int workerIndex, Vector<float> 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<float>(-10) * dt) is about 50-100 microseconds.
+            Span<float> gravityValues = stackalloc float[Vector<float>.Count];
+            for (int bundleSlotIndex = 0; bundleSlotIndex < Vector<int>.Count; ++bundleSlotIndex)
+            {
+                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];
+                    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<float>(gravityValues) * dt;
+        }
+    }
+
+
+    public 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<float>(BufferPool);
+        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))));
+
+        //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;
+        const int width = 20;
+        const int height = 20;
+        for (int i = 0; i < length; ++i)
+        {
+            for (int j = 0; j < height; ++j)
+            {
+                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 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);
+    }
+}
diff --git a/Demos/Demos/PlanetDemo.cs b/Demos/Demos/PlanetDemo.cs
index c56008915..c8afca299 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,82 +7,84 @@
 using DemoRenderer.UI;
 using DemoUtilities;
 using System;
-using System.Collections.Generic;
 using System.Numerics;
-using System.Runtime.CompilerServices;
-using System.Text;
 
-namespace Demos.Demos
-{
+namespace Demos.Demos;
+
 
-    /// <summary>
-    /// Shows how to use custom velocity integration to implement planetary gravity.
-    /// </summary>
-    public class PlanetDemo : Demo
+/// <summary>
+/// Shows how to use custom velocity integration to implement planetary gravity.
+/// </summary>
+public class PlanetDemo : Demo
+{
+    struct PlanetaryGravityCallbacks : IPoseIntegratorCallbacks
     {
-        struct PlanetaryGravityCallbacks : IPoseIntegratorCallbacks
-        {
-            public Vector3 PlanetCenter;
-            public float Gravity;
-            float gravityDt;
+        public Vector3 PlanetCenter;
+        public float Gravity;
 
-            public AngularIntegrationMode AngularIntegrationMode => AngularIntegrationMode.Nonconserving;
+        public readonly AngularIntegrationMode AngularIntegrationMode => AngularIntegrationMode.Nonconserving;
 
-            public void Initialize(Simulation simulation)
-            {
-            }
+        public readonly bool AllowSubstepsForUnconstrainedBodies => false;
 
-            public void PrepareForIntegration(float dt)
-            {
-                //No point in repeating this for every body; cache it.
-                gravityDt = dt * Gravity;
-            }
+        public readonly bool IntegrateVelocityForKinematics => false;
 
-            [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);
-                }
-            }
+        public void Initialize(Simulation simulation)
+        {
         }
 
-        public unsafe override void Initialize(ContentArchive content, Camera camera)
+        float gravityDt;
+        public void PrepareForIntegration(float dt)
         {
-            camera.Position = new Vector3(0, 0, -300);
-            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.Statics.Add(new StaticDescription(new Vector3(), new CollidableDescription(Simulation.Shapes.Add(new Sphere(50)), 0.1f)));
-
-            var orbiter = new Sphere(1f);
-            orbiter.ComputeInertia(1, out var inertia);
-            var collidable = new CollidableDescription(Simulation.Shapes.Add(orbiter), 0.1f);
-            var spacing = new Vector3(5);
-            const int length = 20;
-            for (int i = 0; i < length; ++i)
+            //No point in repeating this for every body; cache it.
+            gravityDt = dt * Gravity;
+        }
+
+        public void IntegrateVelocity(Vector<int> bodyIndices, Vector3Wide position, QuaternionWide orientation, BodyInertiaWide localInertia, Vector<int> integrationMask, int workerIndex, Vector<float> dt, ref BodyVelocityWide velocity)
+        {
+            var offset = position - Vector3Wide.Broadcast(PlanetCenter);
+            var distance = offset.Length();
+            velocity.Linear -= new Vector<float>(gravityDt) * offset / Vector.Max(Vector<float>.One, distance * distance * distance);
+        }
+    }
+
+
+    public override void Initialize(ContentArchive content, Camera camera)
+    {
+        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(4, 1));
+
+        Simulation.Statics.Add(new StaticDescription(new Vector3(), Simulation.Shapes.Add(new Sphere(50))));
+
+        var orbiter = new Sphere(1f);
+        var inertia = orbiter.ComputeInertia(1);
+        var orbiterShapeIndex = Simulation.Shapes.Add(orbiter);
+        var spacing = new Vector3(5);
+        const int length = 40;
+        for (int i = 0; i < length; ++i)
+        {
+            for (int j = 0; j < 20; ++j)
             {
-                for (int j = 0; j < 20; ++j)
+                const int width = 40;
+                var origin = new Vector3(-50, 95, 0) + spacing * new Vector3(length * -0.5f, 0, width * -0.5f);
+                for (int k = 0; k < width; ++k)
                 {
-                    const int width = 40;
-                    var origin = new Vector3(-50, 95, 0) + spacing * new Vector3(length * -0.5f, 0, width * -0.5f);
-                    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)));
-                    }
+                    Simulation.Bodies.Add(BodyDescription.CreateDynamic(
+                        origin + new Vector3(i, j, k) * spacing, new Vector3(30, 0, 0), inertia, orbiterShapeIndex, 0.01f));
                 }
             }
-
         }
 
     }
+
+    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);
+    }
 }
diff --git a/Demos/Demos/PyramidDemo.cs b/Demos/Demos/PyramidDemo.cs
index f060be8e3..07b4e11e5 100644
--- a/Demos/Demos/PyramidDemo.cs
+++ b/Demos/Demos/PyramidDemo.cs
@@ -1,88 +1,81 @@
 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
+namespace Demos.Demos;
+
+/// <summary>
+/// A pyramid of boxes, because you can't have a physics engine without pyramids of boxes.
+/// </summary>
+public class PyramidDemo : Demo
 {
-    /// <summary>
-    /// A pyramid of boxes, because you can't have a physics engine without pyramids of boxes.
-    /// </summary>
-    public class PyramidDemo : Demo
+    public override void Initialize(ContentArchive content, Camera camera)
     {
-        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 PositionFirstTimestepper());
+        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(8, 1));
 
-            var boxShape = new Box(1, 1, 1);
-            boxShape.ComputeInertia(1, out var boxInertia);
-            var boxIndex = Simulation.Shapes.Add(boxShape);
-            const int pyramidCount = 40;
-            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 = 40;
+        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),
-                            new BodyActivityDescription(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), new CollidableDescription(Simulation.Shapes.Add(new Box(2500, 1, 2500)), 0.1f)));
         }
 
-        //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(0.5f + 5 * (float)random.NextDouble());
-                //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, -130), new BodyVelocity(new Vector3(0, 0, 150)), bulletShape.Radius * bulletShape.Radius * bulletShape.Radius, 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(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(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:
+            //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, -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);
+    }
 
+    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/Demos/RagdollDemo.cs b/Demos/Demos/RagdollDemo.cs
index 21deb7a01..b77990e17 100644
--- a/Demos/Demos/RagdollDemo.cs
+++ b/Demos/Demos/RagdollDemo.cs
@@ -10,556 +10,565 @@
 using BepuPhysics.Constraints;
 using DemoContentLoader;
 
-namespace Demos.Demos
+namespace Demos.Demos;
+
+/// <summary>
+/// Bit masks which control whether different members of a group of objects can collide with each other.
+/// </summary>
+public struct SubgroupCollisionFilter
 {
     /// <summary>
-    /// Bit masks which control whether different members of a group of objects can collide with each other.
+    /// A mask of 16 bits, each set bit representing a collision group that an object belongs to.
     /// </summary>
-    public struct SubgroupCollisionFilter
-    {
-        /// <summary>
-        /// A mask of 16 bits, each set bit representing a collision group that an object belongs to.
-        /// </summary>
-        public ushort SubgroupMembership;
-        /// <summary>
-        /// A mask of 16 bits, each set bit representing a collision group that an object can interact with.
-        /// </summary>
-        public ushort CollidableSubgroups;
-        /// <summary>
-        /// Id of the owner of the object. Objects belonging to different groups always collide.
-        /// </summary>
-        public int GroupId;
-
-        /// <summary>
-        /// Initializes a collision filter that collides with everything in the group.
-        /// </summary>
-        /// <param name="groupId">Id of the group that this filter operates within.</param>
-        public SubgroupCollisionFilter(int groupId)
-        {
-            GroupId = groupId;
-            SubgroupMembership = ushort.MaxValue;
-            CollidableSubgroups = ushort.MaxValue;
-        }
-
-        /// <summary>
-        /// Initializes a collision filter that belongs to one specific subgroup and can collide with any other subgroup.
-        /// </summary>
-        /// <param name="groupId">Id of the group that this filter operates within.</param>
-        /// <param name="subgroupId">Id of the subgroup to put this collidable into.</param>
-        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 ushort SubgroupMembership;
+    /// <summary>
+    /// A mask of 16 bits, each set bit representing a collision group that an object can interact with.
+    /// </summary>
+    public ushort CollidableSubgroups;
+    /// <summary>
+    /// Id of the owner of the object. Objects belonging to different groups always collide.
+    /// </summary>
+    public int GroupId;
 
-        /// <summary>
-        /// Disables a collision between this filter and the specified subgroup.
-        /// </summary>
-        /// <param name="subgroupId">Subgroup id to disable collision with.</param>
-        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);
-        }
+    /// <summary>
+    /// Initializes a collision filter that collides with everything in the group.
+    /// </summary>
+    /// <param name="groupId">Id of the group that this filter operates within.</param>
+    public SubgroupCollisionFilter(int groupId)
+    {
+        GroupId = groupId;
+        SubgroupMembership = ushort.MaxValue;
+        CollidableSubgroups = ushort.MaxValue;
+    }
 
-        /// <summary>
-        /// Modifies the interactable subgroups such that filterB does not interact with the subgroups defined by filter a and vice versa.
-        /// </summary>
-        /// <param name="a">Filter from which to remove collisions with filter b's subgroups.</param>
-        /// <param name="b">Filter from which to remove collisions with filter a's subgroups.</param>
-        public static void DisableCollision(ref SubgroupCollisionFilter filterA, ref SubgroupCollisionFilter filterB)
-        {
-            filterA.CollidableSubgroups &= (ushort)~filterB.SubgroupMembership;
-            filterB.CollidableSubgroups &= (ushort)~filterA.SubgroupMembership;
-        }
+    /// <summary>
+    /// Initializes a collision filter that belongs to one specific subgroup and can collide with any other subgroup.
+    /// </summary>
+    /// <param name="groupId">Id of the group that this filter operates within.</param>
+    /// <param name="subgroupId">Id of the subgroup to put this collidable into.</param>
+    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;
+    }
 
-        /// <summary>
-        /// Checks if the filters can collide by checking if b's membership can be collided by a's collidable groups.
-        /// </summary>
-        /// <param name="a">First filter to test.</param>
-        /// <param name="b">Second filter to test.</param>
-        /// <returns>True if the filters can collide, false otherwise.</returns>
-        [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public static bool AllowCollision(in SubgroupCollisionFilter a, in SubgroupCollisionFilter b)
-        {
-            return a.GroupId != b.GroupId || (a.CollidableSubgroups & b.SubgroupMembership) > 0;
-        }
+    /// <summary>
+    /// Disables a collision between this filter and the specified subgroup.
+    /// </summary>
+    /// <param name="subgroupId">Subgroup id to disable collision with.</param>
+    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);
+    }
 
+    /// <summary>
+    /// Modifies the interactable subgroups such that filterB does not interact with the subgroups defined by filter a and vice versa.
+    /// </summary>
+    /// <param name="a">Filter from which to remove collisions with filter b's subgroups.</param>
+    /// <param name="b">Filter from which to remove collisions with filter a's subgroups.</param>
+    public static void DisableCollision(ref SubgroupCollisionFilter filterA, ref SubgroupCollisionFilter filterB)
+    {
+        filterA.CollidableSubgroups &= (ushort)~filterB.SubgroupMembership;
+        filterB.CollidableSubgroups &= (ushort)~filterA.SubgroupMembership;
     }
 
     /// <summary>
-    /// Narrow phase callbacks that prune out collisions between members of a group of objects.
+    /// Checks if the filters can collide by checking if b's membership can be collided by a's collidable groups.
     /// </summary>
-    struct SubgroupFilteredCallbacks : INarrowPhaseCallbacks
+    /// <param name="a">First filter to test.</param>
+    /// <param name="b">Second filter to test.</param>
+    /// <returns>True if the filters can collide, false otherwise.</returns>
+    [MethodImpl(MethodImplOptions.AggressiveInlining)]
+    public static bool AllowCollision(in SubgroupCollisionFilter a, in SubgroupCollisionFilter b)
     {
-        public CollidableProperty<SubgroupCollisionFilter> CollisionFilters;
-        public void Initialize(Simulation simulation)
-        {
-            CollisionFilters.Initialize(simulation);
-        }
+        return a.GroupId != b.GroupId || (a.CollidableSubgroups & b.SubgroupMembership) > 0;
+    }
 
-        [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public bool AllowContactGeneration(int workerIndex, CollidableReference a, CollidableReference b)
-        {
-            //It's impossible for two statics to collide, and pairs are sorted such that bodies always come before statics.
-            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;
-        }
+/// <summary>
+/// Narrow phase callbacks that prune out collisions between members of a group of objects.
+/// </summary>
+struct SubgroupFilteredCallbacks : INarrowPhaseCallbacks
+{
+    public CollidableProperty<SubgroupCollisionFilter> CollisionFilters;
+    public PairMaterialProperties Material;
 
-        [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public unsafe bool ConfigureContactManifold<TManifold>(int workerIndex, CollidablePair pair, ref TManifold manifold, out PairMaterialProperties pairMaterial) where TManifold : unmanaged, IContactManifold<TManifold>
-        {
-            pairMaterial.FrictionCoefficient = 1;
-            pairMaterial.MaximumRecoveryVelocity = 2f;
-            pairMaterial.SpringSettings = new SpringSettings(30, 1);
-            return true;
-        }
+    public SubgroupFilteredCallbacks(CollidableProperty<SubgroupCollisionFilter> filters)
+    {
+        CollisionFilters = filters;
+        Material = new PairMaterialProperties(1, 2, new SpringSettings(30, 1));
+    }
+    public SubgroupFilteredCallbacks(CollidableProperty<SubgroupCollisionFilter> filters, PairMaterialProperties material)
+    {
+        CollisionFilters = filters;
+        Material = material;
+    }
 
-        [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 void Initialize(Simulation simulation)
+    {
+        CollisionFilters.Initialize(simulation);
     }
 
-    public class RagdollDemo : Demo
+    [MethodImpl(MethodImplOptions.AggressiveInlining)]
+    public bool AllowContactGeneration(int workerIndex, CollidableReference a, CollidableReference b, ref float speculativeMargin)
     {
-        static BodyHandle AddBody<TShape>(TShape shape, float mass, in RigidPose pose, Simulation simulation) where TShape : unmanaged, IConvexShape
+        //It's impossible for two statics to collide, and pairs are sorted such that bodies always come before statics.
+        if (b.Mobility != CollidableMobility.Static)
         {
-            //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, new CollidableDescription(shapeIndex, 0.1f), new BodyActivityDescription(0.01f));
-            return simulation.Bodies.Add(description);
+            return SubgroupCollisionFilter.AllowCollision(CollisionFilters[a.BodyHandle], CollisionFilters[b.BodyHandle]);
         }
+        return a.Mobility == CollidableMobility.Dynamic || b.Mobility == CollidableMobility.Dynamic;
+    }
 
-        static RigidPose GetWorldPose(Vector3 localPosition, Quaternion localOrientation, RigidPose ragdollPose)
+    [MethodImpl(MethodImplOptions.AggressiveInlining)]
+    public bool AllowContactGeneration(int workerIndex, CollidablePair pair, int childIndexA, int childIndexB)
+    {
+        return true;
+    }
+
+    [MethodImpl(MethodImplOptions.AggressiveInlining)]
+    public bool ConfigureContactManifold<TManifold>(int workerIndex, CollidablePair pair, ref TManifold manifold, out PairMaterialProperties pairMaterial) where TManifold : unmanaged, IContactManifold<TManifold>
+    {
+        pairMaterial = Material;
+        return true;
+    }
+
+    [MethodImpl(MethodImplOptions.AggressiveInlining)]
+    public bool ConfigureContactManifold(int workerIndex, CollidablePair pair, int childIndexA, int childIndexB, ref ConvexContactManifold manifold)
+    {
+        return true;
+    }
+
+    public void Dispose()
+    {
+        CollisionFilters.Dispose();
+    }
+}
+
+public class RagdollDemo : Demo
+{
+    static BodyHandle AddBody<TShape>(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<SubgroupCollisionFilter> 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
         {
-            RigidPose worldPose;
-            RigidPose.Transform(localPosition, ragdollPose, out worldPose.Position);
-            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)
+            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
         {
-            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(in Vector3 z, in Vector3 x)
+            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
         {
-            //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()
+            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
         {
-            //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<SubgroupCollisionFilter> filters, SpringSettings constraintSpringSettings, Simulation simulation)
+            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
         {
-            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<SubgroupCollisionFilter> filters, SpringSettings constraintSpringSettings, Simulation simulation)
+            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
         {
-            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;
-        }
+            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;
+    }
 
-        public struct RagdollArmHandles
+    static RagdollLegHandles AddLeg(Vector3 localHip, RigidPose localHipsPose, BodyHandle hipsHandle, ref SubgroupCollisionFilter hipsFilter,
+        int limbBaseBitIndex, int ragdollIndex, RigidPose ragdollPose, CollidableProperty<SubgroupCollisionFilter> 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
         {
-            public BodyHandle UpperArm;
-            public BodyHandle LowerArm;
-            public BodyHandle Hand;
-        }
-        public struct RagdollLegHandles
+            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
         {
-            public BodyHandle UpperLeg;
-            public BodyHandle LowerLeg;
-            public BodyHandle Foot;
-        }
-        public struct RagdollHandles
+            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
         {
-            public BodyHandle Head;
-            public BodyHandle Chest;
-            public BodyHandle Abdomen;
-            public BodyHandle Hips;
-            public RagdollArmHandles LeftArm;
-            public RagdollArmHandles RightArm;
-            public RagdollLegHandles LeftLeg;
-            public RagdollLegHandles RightLeg;
-        }
+            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 static RagdollHandles AddRagdoll(Vector3 position, Quaternion orientation, int ragdollIndex, CollidableProperty<SubgroupCollisionFilter> collisionFilters, Simulation simulation)
+    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<SubgroupCollisionFilter> 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
         {
-            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;
-        }
+            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;
+    }
 
-        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;
+        camera.Pitch = MathHelper.Pi * 0.05f;
+        var collisionFilters = new CollidableProperty<SubgroupCollisionFilter>();
+        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);
+        int width = 8;
+        int height = 8;
+        int length = 8;
+        var origin = -0.5f * spacing * new Vector3(width, 0, length) + new Vector3(0, 0.2f, 0);
+        for (int i = 0; i < width; ++i)
         {
-            camera.Position = new Vector3(-20, 10, -20);
-            camera.Yaw = MathHelper.Pi * 3f / 4;
-            camera.Pitch = MathHelper.Pi * 0.05f;
-            var collisionFilters = new CollidableProperty<SubgroupCollisionFilter>();
-            //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());
-
-            int ragdollIndex = 0;
-            var spacing = new Vector3(2f, 3, 1);
-            int width = 8;
-            int height = 8;
-            int length = 8;
-            var origin = -0.5f * spacing * new Vector3(width, 0, length) + new Vector3(0, 0.2f, 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)
-                    {
-                        AddRagdoll(origin + spacing * new Vector3(i, j, k), QuaternionEx.CreateFromAxisAngle(new Vector3(0, 1, 0), MathHelper.Pi * 0.05f), ragdollIndex++, collisionFilters, Simulation);
-                    }
+                    AddRagdoll(origin + spacing * new Vector3(i, j, k), QuaternionEx.CreateFromAxisAngle(new Vector3(0, 1, 0), MathHelper.Pi * 0.05f), ragdollIndex++, collisionFilters, Simulation);
                 }
             }
-
-            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), Simulation.Shapes.Add(new Box(300, 1, 300))));
     }
+
 }
 
 
diff --git a/Demos/Demos/RagdollTubeDemo.cs b/Demos/Demos/RagdollTubeDemo.cs
new file mode 100644
index 000000000..efeeaf3c5
--- /dev/null
+++ b/Demos/Demos/RagdollTubeDemo.cs
@@ -0,0 +1,100 @@
+using BepuUtilities;
+using DemoRenderer;
+using BepuPhysics;
+using BepuPhysics.Collidables;
+using System.Numerics;
+using System;
+using DemoContentLoader;
+using DemoUtilities;
+using BepuPhysics.CollisionDetection;
+using BepuPhysics.Constraints;
+using DemoRenderer.UI;
+
+namespace Demos.Demos;
+
+/// <summary>
+/// Subjects a bunch of unfortunate ragdolls to a tumble dry cycle.
+/// </summary>
+public class RagdollTubeDemo : Demo
+{
+    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<SubgroupCollisionFilter>();
+        //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 = 44;
+        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, 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);
+
+
+    }
+    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);
+    }
+}
+
+
diff --git a/Demos/Demos/RayCastingDemo.cs b/Demos/Demos/RayCastingDemo.cs
index e76a1a5d5..0b903ae5c 100644
--- a/Demos/Demos/RayCastingDemo.cs
+++ b/Demos/Demos/RayCastingDemo.cs
@@ -16,555 +16,551 @@
 using System.Threading;
 using Demos.SpecializedTests;
 using DemoContentLoader;
-using Demos.Demos;
 using Helpers = DemoRenderer.Helpers;
 
-namespace Demos
+namespace Demos;
+
+public class RayCastingDemo : Demo
 {
-    public class RayCastingDemo : Demo
+    public struct NoCollisionCallbacks : INarrowPhaseCallbacks
     {
-        public unsafe 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)
-            {
-                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 unsafe bool ConfigureContactManifold<TManifold>(int workerIndex, CollidablePair pair, ref TManifold manifold, out PairMaterialProperties pairMaterial) where TManifold : unmanaged, IContactManifold<TManifold>
-            {
-                pairMaterial = new PairMaterialProperties();
-                return false;
-            }
+        [MethodImpl(MethodImplOptions.AggressiveInlining)]
+        public bool ConfigureContactManifold<TManifold>(int workerIndex, CollidablePair pair, ref TManifold manifold, out PairMaterialProperties pairMaterial) where TManifold : unmanaged, IContactManifold<TManifold>
+        {
+            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 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());
-
-           
-
-            var sphere = new Sphere(0.5f);
-            var capsule = new Capsule(0, 0.5f);
-            var box = new Box(0.5f, 1.5f, 1f);
-            var cylinder = new Cylinder(0.5f, 1);
-            const int pointCount = 16;
-            var points = new QuickList<Vector3>(pointCount, BufferPool);
-            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());
-            }
-            var hullShape = new ConvexHull(points, BufferPool, out _);
-            var sphereIndex = Simulation.Shapes.Add(sphere);
-            var capsuleIndex = Simulation.Shapes.Add(capsule);
-            var boxIndex = Simulation.Shapes.Add(box);
-            var cylinderIndex = Simulation.Shapes.Add(cylinder);
-            var hullIndex = Simulation.Shapes.Add(hullShape);
-            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;
-            for (int i = 0; i < width; ++i)
-            {
-                for (int j = 0; j < height; ++j)
-                {
-                    for (int k = 0; k < length; ++k)
-                    {
-                        var r = new Vector3((float)random.NextDouble(), (float)random.NextDouble(), (float)random.NextDouble());
-                        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();
-                        QuaternionEx.Normalize(ref orientation);
-                        var shapeIndex = ((i + j + k) % 5) switch
-                        {
-                            0 => boxIndex,
-                            1 => capsuleIndex,
-                            2 => sphereIndex,
-                            3 => cylinderIndex,
-                            _ => hullIndex,
-                        };
-                        if ((i + j + k) % 2 == 1)
-                        {
-                            Simulation.Bodies.Add(BodyDescription.CreateKinematic(new RigidPose(location, orientation), new CollidableDescription(shapeIndex, 0.1f), new BodyActivityDescription(-0.1f)));
-                        }
-                        else
-                        {
-                            Simulation.Statics.Add(new StaticDescription(location, orientation, new CollidableDescription(shapeIndex, 0.1f)));
-                        }
-                    }
-                }
-            }
+    }
+    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 Vector3(0, -10, 0)), new SolveDescription(8, 1));
 
-            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 / 4f) * MathF.Sin(y / 4f), y - planeHeight / 2);
-                }, 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)));
-
-            int raySourceCount = 3;
-            raySources = new QuickList<QuickList<TestRay>>(raySourceCount, BufferPool);
-            raySources.Count = raySourceCount;
-
-            //Spew rays all over the place, starting inside the shape cube.
-            int randomRayCount = 1 << 14;
-            ref var randomRays = ref raySources[0];
-            randomRays = new QuickList<TestRay>(randomRayCount, BufferPool);
-            for (int i = 0; i < randomRayCount; ++i)
-            {
-                var direction = GetDirection(random);
-                var originScale = (float)Math.Sqrt(random.NextDouble());
-                randomRays.AllocateUnsafely() = new TestRay
-                {
-                    Origin = originScale * GetDirection(random) * width * spacing * 0.25f,
-                    Direction = GetDirection(random),
-                    MaximumT = 50
-                };
-            }
 
-            //Send rays out matching a planar projection.
-            int frustumRayWidth = 128;
-            int frustumRayHeight = 128;
-            float aspectRatio = 1.6f;
-            float verticalFOV = MathHelper.Pi * 0.16f;
-            var unitZScreenHeight = 2 * MathF.Tan(verticalFOV / 2);
-            var unitZScreenWidth = unitZScreenHeight * aspectRatio;
-            var unitZSpacing = new Vector2(unitZScreenWidth / frustumRayWidth, unitZScreenHeight / frustumRayHeight);
-            var unitZBase = (unitZSpacing - new Vector2(unitZScreenWidth, unitZScreenHeight)) * 0.5f;
-            ref var frustumRays = ref raySources[1];
-            frustumRays = new QuickList<TestRay>(frustumRayWidth * frustumRayHeight, BufferPool);
-            var frustumOrigin = new Vector3(0, 0, -50);
-            for (int i = 0; i < frustumRayWidth; ++i)
-            {
-                for (int j = 0; j < frustumRayHeight; ++j)
-                {
-                    ref var ray = ref frustumRays.AllocateUnsafely();
-                    ray.Direction = new Vector3(unitZBase + new Vector2(i, j) * unitZSpacing, 1);
-                    ray.Origin = frustumOrigin + ray.Direction * 10;
-                    ray.MaximumT = 100;
-                }
-            }
 
-            //Send a wall of rays. Matches an orthographic projection.
-            int wallWidth = 128;
-            int wallHeight = 128;
-            var wallOrigin = new Vector3(0, 0, -50);
-            var wallSpacing = new Vector2(0.1f);
-            var wallBase = 0.5f * (wallSpacing - wallSpacing * new Vector2(wallWidth, wallHeight));
-            ref var wallRays = ref raySources[2];
-            wallRays = new QuickList<TestRay>(wallWidth * wallHeight, BufferPool);
-            for (int i = 0; i < wallWidth; ++i)
+        var sphere = new Sphere(0.5f);
+        var capsule = new Capsule(0, 0.5f);
+        var box = new Box(0.5f, 1.5f, 1f);
+        var cylinder = new Cylinder(0.5f, 1);
+        const int pointCount = 16;
+        var points = new QuickList<Vector3>(pointCount, BufferPool);
+        var random = new Random(5);
+        for (int i = 0; i < pointCount; ++i)
+        {
+            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);
+        var capsuleIndex = Simulation.Shapes.Add(capsule);
+        var boxIndex = Simulation.Shapes.Add(box);
+        var cylinderIndex = Simulation.Shapes.Add(cylinder);
+        var hullIndex = Simulation.Shapes.Add(hullShape);
+        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;
+        for (int i = 0; i < width; ++i)
+        {
+            for (int j = 0; j < height; ++j)
             {
-                for (int j = 0; j < wallHeight; ++j)
+                for (int k = 0; k < length; ++k)
                 {
-                    wallRays.AllocateUnsafely() = new TestRay
+                    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);
+                    var shapeIndex = ((i + j + k) % 5) switch
                     {
-                        Origin = wallOrigin + new Vector3(wallBase + wallSpacing * new Vector2(i, j), 0),
-                        Direction = new Vector3(0, 0, 1),
-                        MaximumT = 100
+                        0 => boxIndex,
+                        1 => capsuleIndex,
+                        2 => sphereIndex,
+                        3 => cylinderIndex,
+                        _ => hullIndex,
                     };
+                    if ((i + j + k) % 2 == 1)
+                    {
+                        Simulation.Bodies.Add(BodyDescription.CreateKinematic((location, orientation), shapeIndex, -0.1f));
+                    }
+                    else
+                    {
+                        Simulation.Statics.Add(new StaticDescription(location, orientation, shapeIndex));
+                    }
                 }
             }
-            var maxRayCount = Math.Max(randomRays.Count, Math.Max(frustumRays.Count, wallRays.Count));
-            testRays = new QuickList<TestRay>(maxRayCount, BufferPool);
-            var timeSampleCount = 16;
-            algorithms = new IntersectionAlgorithm[2];
-            algorithms[0] = new IntersectionAlgorithm("Unbatched", UnbatchedWorker, BufferPool, maxRayCount, timeSampleCount);
-            algorithms[1] = new IntersectionAlgorithm("Batched", BatchedWorker, BufferPool, maxRayCount, timeSampleCount);
+        }
 
-            BufferPool.Take(Environment.ProcessorCount * 2, out jobs);
+        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 / 4f) * MathF.Sin(y / 4f), y - planeHeight / 2);
+            }, 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)));
+
+        int raySourceCount = 3;
+        raySources = new QuickList<QuickList<TestRay>>(raySourceCount, BufferPool);
+        raySources.Count = raySourceCount;
+
+        //Spew rays all over the place, starting inside the shape cube.
+        int randomRayCount = 1 << 14;
+        ref var randomRays = ref raySources[0];
+        randomRays = new QuickList<TestRay>(randomRayCount, BufferPool);
+        for (int i = 0; i < randomRayCount; ++i)
+        {
+            var direction = GetDirection(random);
+            var originScale = (float)Math.Sqrt(random.NextDouble());
+            randomRays.AllocateUnsafely() = new TestRay
+            {
+                Origin = originScale * GetDirection(random) * width * spacing * 0.25f,
+                Direction = GetDirection(random),
+                MaximumT = 50
+            };
         }
 
-        static Vector3 GetDirection(Random random)
+        //Send rays out matching a planar projection.
+        int frustumRayWidth = 128;
+        int frustumRayHeight = 128;
+        float aspectRatio = 1.6f;
+        float verticalFOV = MathHelper.Pi * 0.16f;
+        var unitZScreenHeight = 2 * MathF.Tan(verticalFOV / 2);
+        var unitZScreenWidth = unitZScreenHeight * aspectRatio;
+        var unitZSpacing = new Vector2(unitZScreenWidth / frustumRayWidth, unitZScreenHeight / frustumRayHeight);
+        var unitZBase = (unitZSpacing - new Vector2(unitZScreenWidth, unitZScreenHeight)) * 0.5f;
+        ref var frustumRays = ref raySources[1];
+        frustumRays = new QuickList<TestRay>(frustumRayWidth * frustumRayHeight, BufferPool);
+        var frustumOrigin = new Vector3(0, 0, -50);
+        for (int i = 0; i < frustumRayWidth; ++i)
         {
-            Vector3 direction;
-            float length;
-            do
+            for (int j = 0; j < frustumRayHeight; ++j)
             {
-                direction = 2 * new Vector3((float)random.NextDouble(), (float)random.NextDouble(), (float)random.NextDouble()) - Vector3.One;
-                length = direction.Length();
+                ref var ray = ref frustumRays.AllocateUnsafely();
+                ray.Direction = new Vector3(unitZBase + new Vector2(i, j) * unitZSpacing, 1);
+                ray.Origin = frustumOrigin + ray.Direction * 10;
+                ray.MaximumT = 100;
             }
-            while (length < 1e-7f);
-            direction /= length;
-            return direction;
         }
 
-        struct TestRay
+        //Send a wall of rays. Matches an orthographic projection.
+        int wallWidth = 128;
+        int wallHeight = 128;
+        var wallOrigin = new Vector3(0, 0, -50);
+        var wallSpacing = new Vector2(0.1f);
+        var wallBase = 0.5f * (wallSpacing - wallSpacing * new Vector2(wallWidth, wallHeight));
+        ref var wallRays = ref raySources[2];
+        wallRays = new QuickList<TestRay>(wallWidth * wallHeight, BufferPool);
+        for (int i = 0; i < wallWidth; ++i)
         {
-            public Vector3 Origin;
-            public float MaximumT;
-            public Vector3 Direction;
+            for (int j = 0; j < wallHeight; ++j)
+            {
+                wallRays.AllocateUnsafely() = new TestRay
+                {
+                    Origin = wallOrigin + new Vector3(wallBase + wallSpacing * new Vector2(i, j), 0),
+                    Direction = new Vector3(0, 0, 1),
+                    MaximumT = 100
+                };
+            }
         }
-        QuickList<QuickList<TestRay>> raySources;
-        QuickList<TestRay> testRays;
-
+        var maxRayCount = Math.Max(randomRays.Count, Math.Max(frustumRays.Count, wallRays.Count));
+        testRays = new QuickList<TestRay>(maxRayCount, BufferPool);
+        var timeSampleCount = 16;
+        algorithms = new IntersectionAlgorithm[2];
+        algorithms[0] = new IntersectionAlgorithm("Unbatched", UnbatchedWorker, BufferPool, maxRayCount, timeSampleCount);
+        algorithms[1] = new IntersectionAlgorithm("Batched", BatchedWorker, BufferPool, maxRayCount, timeSampleCount);
+
+        BufferPool.Take(Environment.ProcessorCount * 2, out jobs);
+    }
 
-        struct RayHit
+    static Vector3 GetDirection(Random random)
+    {
+        Vector3 direction;
+        float length;
+        do
         {
-            public Vector3 Normal;
-            public float T;
-            public CollidableReference Collidable;
-            public bool Hit;
+            direction = 2 * new Vector3(random.NextSingle(), random.NextSingle(), random.NextSingle()) - Vector3.One;
+            length = direction.Length();
         }
-        class IntersectionAlgorithm
+        while (length < 1e-7f);
+        direction /= length;
+        return direction;
+    }
+
+    struct TestRay
+    {
+        public Vector3 Origin;
+        public float MaximumT;
+        public Vector3 Direction;
+    }
+    QuickList<QuickList<TestRay>> raySources;
+    QuickList<TestRay> testRays;
+
+
+    struct RayHit
+    {
+        public Vector3 Normal;
+        public float T;
+        public CollidableReference Collidable;
+        public bool Hit;
+    }
+    unsafe class IntersectionAlgorithm
+    {
+        public string Name;
+        public int IntersectionCount;
+        public Buffer<RayHit> Results;
+        public TimingsRingBuffer Timings;
+
+        Func<int, IntersectionAlgorithm, int> worker;
+        Action<int> internalWorker;
+        public int JobIndex;
+
+        public IntersectionAlgorithm(string name, Func<int, IntersectionAlgorithm, int> worker,
+            BufferPool pool, int largestRayCount, int timingSampleCount = 16)
         {
-            public string Name;
-            public int IntersectionCount;
-            public Buffer<RayHit> Results;
-            public TimingsRingBuffer Timings;
+            Name = name;
+            Timings = new TimingsRingBuffer(timingSampleCount, pool);
+            this.worker = worker;
+            internalWorker = ExecuteWorker;
+            pool.Take(largestRayCount, out Results);
+        }
 
-            Func<int, IntersectionAlgorithm, int> worker;
-            Action<int> internalWorker;
-            public int JobIndex;
+        void ExecuteWorker(int workerIndex)
+        {
+            var intersectionCount = worker(workerIndex, this);
+            Interlocked.Add(ref IntersectionCount, intersectionCount);
+        }
 
-            public IntersectionAlgorithm(string name, Func<int, IntersectionAlgorithm, int> worker,
-                BufferPool pool, int largestRayCount, int timingSampleCount = 16)
+        public void Execute(ref QuickList<TestRay> rays, IThreadDispatcher dispatcher)
+        {
+            CacheBlaster.Blast();
+            for (int i = 0; i < rays.Count; ++i)
             {
-                Name = name;
-                Timings = new TimingsRingBuffer(timingSampleCount, pool);
-                this.worker = worker;
-                internalWorker = ExecuteWorker;
-                pool.Take(largestRayCount, out Results);
+                Results[i].T = float.MaxValue;
+                Results[i].Hit = false;
             }
-
-            void ExecuteWorker(int workerIndex)
+            JobIndex = -1;
+            IntersectionCount = 0;
+            var start = Stopwatch.GetTimestamp();
+            if (dispatcher != null)
             {
-                var intersectionCount = worker(workerIndex, this);
-                Interlocked.Add(ref IntersectionCount, intersectionCount);
+                dispatcher.DispatchWorkers(internalWorker);
             }
-
-            public void Execute(ref QuickList<TestRay> rays, SimpleThreadDispatcher dispatcher)
+            else
             {
-                CacheBlaster.Blast();
-                for (int i = 0; i < rays.Count; ++i)
-                {
-                    Results[i].T = float.MaxValue;
-                    Results[i].Hit = false;
-                }
-                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 BatchedWorker(int workerIndex, IntersectionAlgorithm algorithm)
+    unsafe int BatchedWorker(int workerIndex, IntersectionAlgorithm algorithm)
+    {
+        int intersectionCount = 0;
+        var hitHandler = new HitHandler { Hits = algorithm.Results, IntersectionCount = &intersectionCount };
+        var batcher = new SimulationRayBatcher<HitHandler>(ThreadDispatcher.WorkerPools[workerIndex], Simulation, hitHandler, 2048);
+        int claimedIndex;
+        while ((claimedIndex = Interlocked.Increment(ref algorithm.JobIndex)) < jobs.Length)
         {
-            int intersectionCount = 0;
-            var hitHandler = new HitHandler { Hits = algorithm.Results, IntersectionCount = &intersectionCount };
-            var batcher = new SimulationRayBatcher<HitHandler>(ThreadDispatcher.GetThreadMemoryPool(workerIndex), Simulation, hitHandler, 2048);
-            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 ray = ref testRays[i];
-                    batcher.Add(ref ray.Origin, ref ray.Direction, ray.MaximumT, i);
-                }
+                ref var ray = ref testRays[i];
+                batcher.Add(ref ray.Origin, ref ray.Direction, ray.MaximumT, i);
             }
-            batcher.Flush();
-            batcher.Dispose();
-            return intersectionCount;
         }
+        batcher.Flush();
+        batcher.Dispose();
+        return intersectionCount;
+    }
 
-        unsafe int UnbatchedWorker(int workerIndex, IntersectionAlgorithm algorithm)
+    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)
         {
-            int intersectionCount = 0;
-            var hitHandler = new HitHandler { Hits = algorithm.Results, 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 ray = ref testRays[i];
-                    Simulation.RayCast(ray.Origin, ray.Direction, ray.MaximumT, ref hitHandler, i);
-                }
+                ref var ray = ref testRays[i];
+                Simulation.RayCast(ray.Origin, ray.Direction, ray.MaximumT, pool, ref hitHandler, i);
             }
-            return intersectionCount;
         }
+        return intersectionCount;
+    }
 
-        IntersectionAlgorithm[] algorithms;
+    IntersectionAlgorithm[] algorithms;
 
-        struct RayJob
+    struct RayJob
+    {
+        public int Start;
+        public int End;
+    }
+    Buffer<RayJob> jobs;
+
+
+    unsafe struct HitHandler : IRayHitHandler
+    {
+        public Buffer<RayHit> Hits;
+        public int* IntersectionCount;
+        [MethodImpl(MethodImplOptions.AggressiveInlining)]
+        public bool AllowTest(CollidableReference collidable)
         {
-            public int Start;
-            public int End;
+            return true;
         }
-        Buffer<RayJob> jobs;
 
+        [MethodImpl(MethodImplOptions.AggressiveInlining)]
+        public bool AllowTest(CollidableReference collidable, int childIndex)
+        {
+            return true;
+        }
 
-        unsafe struct HitHandler : IRayHitHandler
+        [MethodImpl(MethodImplOptions.AggressiveInlining)]
+        public void OnRayHit(in RayData ray, ref float maximumT, float t, Vector3 normal, CollidableReference collidable, int childIndex)
         {
-            public Buffer<RayHit> Hits;
-            public int* IntersectionCount;
-            [MethodImpl(MethodImplOptions.AggressiveInlining)]
-            public bool AllowTest(CollidableReference collidable)
+            maximumT = t;
+            ref var hit = ref Hits[ray.Id];
+            if (t < hit.T)
             {
-                return true;
+                if (hit.T == float.MaxValue)
+                    ++*IntersectionCount;
+                hit.Normal = normal;
+                hit.T = t;
+                hit.Collidable = collidable;
+                hit.Hit = true;
             }
+        }
+    }
 
-            [MethodImpl(MethodImplOptions.AggressiveInlining)]
-            public bool AllowTest(CollidableReference collidable, int childIndex)
-            {
-                return true;
-            }
+    bool shouldCycle = true;
+    bool shouldRotate = true;
+    bool shouldUseMultithreading = true;
+    int raySourceIndex;
+    int frameCount;
+    float rotation;
+    void CopyAndRotate(ref QuickList<TestRay> source)
+    {
+        testRays.Count = source.Count;
+        var transform = Matrix3x3.CreateFromAxisAngle(new Vector3(0, 1, 0), rotation);
 
-            [MethodImpl(MethodImplOptions.AggressiveInlining)]
-            public void OnRayHit(in RayData ray, ref float maximumT, float t, in Vector3 normal, CollidableReference collidable, int childIndex)
-            {
-                maximumT = t;
-                ref var hit = ref Hits[ray.Id];
-                if (t < hit.T)
-                {
-                    if (hit.T == float.MaxValue)
-                        ++*IntersectionCount;
-                    hit.Normal = normal;
-                    hit.T = t;
-                    hit.Collidable = collidable;
-                    hit.Hit = true;
-                }
-            }
+        for (int i = 0; i < source.Count; ++i)
+        {
+            ref var targetRay = ref testRays[i];
+            ref var sourceRay = ref source[i];
+            Matrix3x3.Transform(sourceRay.Origin, transform, out targetRay.Origin);
+            Matrix3x3.Transform(sourceRay.Direction, transform, out targetRay.Direction);
+            targetRay.MaximumT = sourceRay.MaximumT;
         }
+    }
 
-        bool shouldCycle = true;
-        bool shouldRotate = true;
-        bool shouldUseMultithreading = true;
-        int raySourceIndex;
-        int frameCount;
-        float rotation;
-        void CopyAndRotate(ref QuickList<TestRay> source)
-        {
-            testRays.Count = source.Count;
-            var transform = Matrix3x3.CreateFromAxisAngle(new Vector3(0, 1, 0), rotation);
+    public override void Update(Window window, Camera camera, Input input, float dt)
+    {
+        base.Update(window, camera, input, dt);
 
-            for (int i = 0; i < source.Count; ++i)
+        char one = '1';
+        for (int i = 0; i < raySources.Count; ++i)
+        {
+            if (input.TypedCharacters.Contains((char)(one + i)))
             {
-                ref var targetRay = ref testRays[i];
-                ref var sourceRay = ref source[i];
-                Matrix3x3.Transform(sourceRay.Origin, transform, out targetRay.Origin);
-                Matrix3x3.Transform(sourceRay.Direction, transform, out targetRay.Direction);
-                targetRay.MaximumT = sourceRay.MaximumT;
+                shouldCycle = false;
+                raySourceIndex = i + 1;
             }
         }
+        if (input.WasPushed(OpenTK.Input.Key.C))
+        {
+            frameCount = 0;
+            shouldCycle = !shouldCycle;
+        }
+        if (input.WasPushed(OpenTK.Input.Key.R))
+        {
+            shouldRotate = !shouldRotate;
+        }
+        if (input.WasPushed(OpenTK.Input.Key.F))
+        {
+            rotation = 0;
+        }
+        if (input.WasPushed(OpenTK.Input.Key.T))
+        {
+            shouldUseMultithreading = !shouldUseMultithreading;
+        }
 
-        public unsafe override void Update(Window window, Camera camera, Input input, float dt)
+        ++frameCount;
+        if (frameCount > 1 << 20)
+            frameCount = 0;
+        if (shouldRotate)
+        {
+            rotation += (MathF.PI * 1e-2f * (TimestepDuration)) % (2 * MathF.PI);
+        }
+        if (shouldCycle)
         {
-            base.Update(window, camera, input, dt);
+            raySourceIndex = 1 + (frameCount / 256) % raySources.Count;
+        }
+        CopyAndRotate(ref raySources[raySourceIndex - 1]);
 
-            char one = '1';
-            for (int i = 0; i < raySources.Count; ++i)
-            {
-                if (input.TypedCharacters.Contains((char)(one + i)))
-                {
-                    shouldCycle = false;
-                    raySourceIndex = i + 1;
-                }
-            }
-            if (input.WasPushed(OpenTK.Input.Key.C))
-            {
-                frameCount = 0;
-                shouldCycle = !shouldCycle;
-            }
-            if (input.WasPushed(OpenTK.Input.Key.R))
-            {
-                shouldRotate = !shouldRotate;
-            }
-            if (input.WasPushed(OpenTK.Input.Key.F))
-            {
-                rotation = 0;
-            }
-            if (input.WasPushed(OpenTK.Input.Key.T))
-            {
-                shouldUseMultithreading = !shouldUseMultithreading;
-            }
+        var raysPerJobBase = testRays.Count / jobs.Length;
+        var remainder = testRays.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;
+        }
 
-            ++frameCount;
-            if (frameCount > 1 << 20)
-                frameCount = 0;
-            if (shouldRotate)
-            {
-                rotation += (MathF.PI * 1e-2f * (1 / 60f)) % (2 * MathF.PI);
-            }
-            if (shouldCycle)
-            {
-                raySourceIndex = 1 + (frameCount / 256) % raySources.Count;
-            }
-            CopyAndRotate(ref raySources[raySourceIndex - 1]);
 
-            var raysPerJobBase = testRays.Count / jobs.Length;
-            var remainder = testRays.Count - raysPerJobBase * jobs.Length;
-            var previousJobEnd = 0;
-            for (int i = 0; i < jobs.Length; ++i)
+        for (int i = 0; i < algorithms.Length; ++i)
+        {
+            algorithms[i].Execute(ref testRays, shouldUseMultithreading ? ThreadDispatcher : null);
+        }
+        for (int i = 1; i < algorithms.Length; ++i)
+        {
+            Debug.Assert(algorithms[i].IntersectionCount == algorithms[0].IntersectionCount);
+            var current = algorithms[i];
+            var earlier = algorithms[0];
+            for (int j = 0; j < testRays.Count; ++j)
             {
-                int raysInJob = i < remainder ? raysPerJobBase + 1 : raysPerJobBase;
-                ref var job = ref jobs[i];
-                job.Start = previousJobEnd;
-                job.End = previousJobEnd = previousJobEnd + raysInJob;
+                ref var currentResult = ref current.Results[j];
+                ref var earlierResult = ref earlier.Results[j];
+                Debug.Assert(currentResult.Hit == earlierResult.Hit && (!earlierResult.Hit || Math.Abs(earlierResult.T - currentResult.T) < 1e-6f));
             }
+        }
 
+    }
 
-            for (int i = 0; i < algorithms.Length; ++i)
-            {
-                algorithms[i].Execute(ref testRays, shouldUseMultithreading ? ThreadDispatcher : null);
-            }
-            for (int i = 1; i < algorithms.Length; ++i)
+    void DrawRays(ref Buffer<RayHit> results, Renderer renderer, Vector3 foregroundMissColor, Vector3 foregroundHitColor, Vector3 foregroundNormalColor, Vector3 backgroundColor)
+    {
+        var packedForegroundMiss = Helpers.PackColor(foregroundMissColor);
+        var packedForegroundHit = Helpers.PackColor(foregroundHitColor);
+        var packedForegroundNormal = Helpers.PackColor(foregroundNormalColor);
+        var packedBackground = Helpers.PackColor(backgroundColor);
+        for (int i = 0; i < testRays.Count; ++i)
+        {
+            ref var result = ref results[i];
+            ref var ray = ref testRays[i];
+            if (result.Hit)
             {
-                Debug.Assert(algorithms[i].IntersectionCount == algorithms[0].IntersectionCount);
-                var current = algorithms[i];
-                var earlier = algorithms[0];
-                for (int j = 0; j < testRays.Count; ++j)
+                var end = ray.Origin + ray.Direction * result.T;
+                var diffuseLight = Vector3.Dot(result.Normal, new Vector3(0.57735f));
+                if (diffuseLight < 0)
                 {
-                    ref var currentResult = ref current.Results[j];
-                    ref var earlierResult = ref earlier.Results[j];
-                    Debug.Assert(currentResult.Hit == earlierResult.Hit && (!earlierResult.Hit || Math.Abs(earlierResult.T - currentResult.T) < 1e-6f));
+                    diffuseLight = -0.5f * diffuseLight;
                 }
+                renderer.Lines.Allocate() = new LineInstance(ray.Origin, end, Helpers.PackColor(foregroundHitColor * (0.2f + 0.8f * diffuseLight)), packedBackground);
+                renderer.Lines.Allocate() = new LineInstance(end, end + result.Normal, packedForegroundNormal, packedBackground);
             }
-
-        }
-
-        void DrawRays(ref Buffer<RayHit> results, Renderer renderer, Vector3 foregroundMissColor, Vector3 foregroundHitColor, Vector3 foregroundNormalColor, Vector3 backgroundColor)
-        {
-            var packedForegroundMiss = Helpers.PackColor(foregroundMissColor);
-            var packedForegroundHit = Helpers.PackColor(foregroundHitColor);
-            var packedForegroundNormal = Helpers.PackColor(foregroundNormalColor);
-            var packedBackground = Helpers.PackColor(backgroundColor);
-            for (int i = 0; i < testRays.Count; ++i)
+            else
             {
-                ref var result = ref results[i];
-                ref var ray = ref testRays[i];
-                if (result.Hit)
-                {
-                    var end = ray.Origin + ray.Direction * result.T;
-                    var diffuseLight = Vector3.Dot(result.Normal, new Vector3(0.57735f));
-                    if (diffuseLight < 0)
-                    {
-                        diffuseLight = -0.5f * diffuseLight;
-                    }
-                    renderer.Lines.Allocate() = new LineInstance(ray.Origin, end, Helpers.PackColor(foregroundHitColor * (0.2f + 0.8f * diffuseLight)), packedBackground);
-                    renderer.Lines.Allocate() = new LineInstance(end, end + result.Normal, packedForegroundNormal, packedBackground);
-                }
-                else
-                {
-                    var end = ray.Origin + ray.Direction * ray.MaximumT;
-                    renderer.Lines.Allocate() = new LineInstance(ray.Origin, end, packedForegroundMiss, packedBackground);
-                }
+                var end = ray.Origin + ray.Direction * ray.MaximumT;
+                renderer.Lines.Allocate() = new LineInstance(ray.Origin, end, packedForegroundMiss, packedBackground);
             }
         }
+    }
 
-        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(testRays.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);
-        }
-
-        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)
-        {
-            var batchedPackedColor = Helpers.PackColor(new Vector3(0.75f, 0.75f, 0));
-            var batchedPackedNormalColor = Helpers.PackColor(new Vector3(1f, 1f, 0));
-            var batchedPackedBackgroundColor = Helpers.PackColor(new Vector3());
-
-            DrawRays(ref algorithms[0].Results, renderer, new Vector3(0.25f, 0, 0), new Vector3(0, 1, 0), new Vector3(1, 1, 0), new Vector3());
-
-            text.Clear().Append("Active ray source index: ").Append(raySourceIndex);
-            if (shouldCycle)
-                text.Append(" (cycling)");
-            if (shouldRotate)
-                text.Append(" (rotating)");
-            if (shouldUseMultithreading)
-                text.Append(" (multithreaded)");
-            renderer.TextBatcher.Write(text, new Vector2(32, renderer.Surface.Resolution.Y - 192), 16, new Vector3(1), font);
-            renderer.TextBatcher.Write(text.Clear().Append("Demo specific controls:"), new Vector2(32, renderer.Surface.Resolution.Y - 176), 16, new Vector3(1), font);
-            WriteControl("Change ray source", text.Clear().Append("1 through ").Append(raySources.Count), renderer.Surface.Resolution.Y - 160, renderer.TextBatcher, font);
-            WriteControl("Toggle cycling", text.Clear().Append("C"), renderer.Surface.Resolution.Y - 144, renderer.TextBatcher, font);
-            WriteControl("Toggle rotation", text.Clear().Append("R"), renderer.Surface.Resolution.Y - 128, renderer.TextBatcher, font);
-            WriteControl("Reset rotation", text.Clear().Append("F"), renderer.Surface.Resolution.Y - 112, renderer.TextBatcher, font);
-            WriteControl("Toggle threading", text.Clear().Append("T"), renderer.Surface.Resolution.Y - 96, renderer.TextBatcher, font);
-
-            renderer.TextBatcher.Write(text.Clear().Append("Ray count: ").Append(testRays.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("Rays 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(testRays.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);
+    }
+
+    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);
+    }
 
-            base.Render(renderer, camera, input, text, font);
+    public override void Render(Renderer renderer, Camera camera, Input input, TextBuilder text, Font font)
+    {
+        var batchedPackedColor = Helpers.PackColor(new Vector3(0.75f, 0.75f, 0));
+        var batchedPackedNormalColor = Helpers.PackColor(new Vector3(1f, 1f, 0));
+        var batchedPackedBackgroundColor = Helpers.PackColor(new Vector3());
+
+        DrawRays(ref algorithms[0].Results, renderer, new Vector3(0.25f, 0, 0), new Vector3(0, 1, 0), new Vector3(1, 1, 0), new Vector3());
+
+        text.Clear().Append("Active ray source index: ").Append(raySourceIndex);
+        if (shouldCycle)
+            text.Append(" (cycling)");
+        if (shouldRotate)
+            text.Append(" (rotating)");
+        if (shouldUseMultithreading)
+            text.Append(" (multithreaded)");
+        renderer.TextBatcher.Write(text, new Vector2(32, renderer.Surface.Resolution.Y - 192), 16, new Vector3(1), font);
+        renderer.TextBatcher.Write(text.Clear().Append("Demo specific controls:"), new Vector2(32, renderer.Surface.Resolution.Y - 176), 16, new Vector3(1), font);
+        WriteControl("Change ray source", text.Clear().Append("1 through ").Append(raySources.Count), renderer.Surface.Resolution.Y - 160, renderer.TextBatcher, font);
+        WriteControl("Toggle cycling", text.Clear().Append("C"), renderer.Surface.Resolution.Y - 144, renderer.TextBatcher, font);
+        WriteControl("Toggle rotation", text.Clear().Append("R"), renderer.Surface.Resolution.Y - 128, renderer.TextBatcher, font);
+        WriteControl("Reset rotation", text.Clear().Append("F"), renderer.Surface.Resolution.Y - 112, renderer.TextBatcher, font);
+        WriteControl("Toggle threading", text.Clear().Append("T"), renderer.Surface.Resolution.Y - 96, renderer.TextBatcher, font);
+
+        renderer.TextBatcher.Write(text.Clear().Append("Ray count: ").Append(testRays.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("Rays 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/Demos/RopeStabilityDemo.cs b/Demos/Demos/RopeStabilityDemo.cs
index fd517de87..d6b0b9dcc 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,264 +7,266 @@
 using DemoRenderer.UI;
 using DemoUtilities;
 using System;
-using System.Collections.Generic;
 using System.Numerics;
-using System.Text;
 
-namespace Demos.Demos
+namespace Demos.Demos;
+
+/// <summary>
+/// Shows a few different ways of making a rope with a heavy thing attached not freak out.
+/// </summary>
+public class RopeStabilityDemo : Demo
 {
-    /// <summary>
-    /// Shows a few different ways of making a rope with a heavy thing attached not freak out.
-    /// </summary>
-    public class RopeStabilityDemo : Demo
+    public static BodyHandle[] BuildRopeBodies(Simulation simulation, Vector3 start, int bodyCount, float bodySize, float bodySpacing, float massPerBody, float inverseInertiaScale)
     {
-        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);
+        var ropeInertia = ropeShape.ComputeInertia(massPerBody);
+        Symmetric3x3.Scale(ropeInertia.InverseInertiaTensor, inverseInertiaScale, out ropeInertia.InverseInertiaTensor);
+        var ropeShapeIndex = simulation.Shapes.Add(ropeShape);
+        //Build the links.
+        var bodyDescription = new BodyDescription
         {
-            BodyHandle[] handles = new BodyHandle[bodyCount + 1];
-            var ropeShape = new Sphere(bodySize);
-            ropeShape.ComputeInertia(massPerBody, out var ropeInertia);
-            Symmetric3x3.Scale(ropeInertia.InverseInertiaTensor, inverseInertiaScale, out ropeInertia.InverseInertiaTensor);
-            var ropeShapeIndex = simulation.Shapes.Add(ropeShape);
-            //Build the links.
-            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, 0.1f),
-            };
-            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));
-                handles[linkIndex] = simulation.Bodies.Add(bodyDescription);
-            }
-
-            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)
+            Activity = .01f,
+            Collidable = ropeShapeIndex
+        };
+        for (int linkIndex = 0; linkIndex < bodyCount + 1; ++linkIndex)
         {
-            var handles = BuildRopeBodies(simulation, start, bodyCount, bodySize, bodySpacing, massPerBody, inverseInertiaScale);
-            var maximumDistance = 2 * bodySize + bodySpacing - 2 * constraintOffsetLength;
-            for (int i = 0; i < handles.Length - 1; ++i)
-            {
-                simulation.Solver.Add(handles[i], handles[i + 1],
-                    new DistanceLimit(new Vector3(0, -constraintOffsetLength, 0), new Vector3(0, constraintOffsetLength, 0), maximumDistance * 0.1f, maximumDistance, springSettings));
-            }
-            return handles;
+            //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);
         }
 
-        static BodyHandle CreateWreckingBall(Simulation simulation, BodyHandle[] bodyHandles, float ropeBodyRadius, float bodySpacing, float wreckingBallRadius, BodyInertia wreckingBallInertia, TypedIndex wreckingBallShapeIndex)
+        return handles;
+    }
+    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;
+        for (int i = 0; i < handles.Length - 1; ++i)
         {
-            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));
-            //Give it a little bump.
-            description.Velocity = new BodyVelocity(new Vector3(-10, 0, 0), default);
-            var wreckingBallBodyHandle = simulation.Bodies.Add(description);
-            return wreckingBallBodyHandle;
+            simulation.Solver.Add(handles[i], handles[i + 1],
+                new DistanceLimit(new Vector3(0, -constraintOffsetLength, 0), new Vector3(0, constraintOffsetLength, 0), maximumDistance * 0.1f, maximumDistance, springSettings));
         }
+        return handles;
+    }
 
-        public static BodyHandle AttachWreckingBall(Simulation simulation, BodyHandle[] bodyHandles, float ropeBodyRadius, float bodySpacing, float constraintOffsetLength, float wreckingBallRadius, BodyInertia wreckingBallInertia, TypedIndex wreckingBallShapeIndex, SpringSettings springSettings)
-        {
-            BodyHandle wreckingBallBodyHandle = CreateWreckingBall(simulation, bodyHandles, ropeBodyRadius, bodySpacing, wreckingBallRadius, wreckingBallInertia, wreckingBallShapeIndex);
-            var maximumDistance = bodySpacing + ropeBodyRadius - constraintOffsetLength;
-            simulation.Solver.Add(bodyHandles[bodyHandles.Length - 1], wreckingBallBodyHandle,
-                new DistanceLimit(new Vector3(0, -constraintOffsetLength, 0), new Vector3(0, wreckingBallRadius, 0), maximumDistance * 0.1f, maximumDistance, springSettings));
-            return wreckingBallBodyHandle;
-        }
+    static BodyHandle CreateWreckingBall(Simulation simulation, BodyHandle[] bodyHandles, float ropeBodyRadius, float bodySpacing, float wreckingBallRadius, BodyInertia wreckingBallInertia, TypedIndex wreckingBallShapeIndex)
+    {
+        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, wreckingBallShapeIndex, 0.01f);
+        //Give it a little bump.
+        description.Velocity = new Vector3(-10, 0, 0);
+        var wreckingBallBodyHandle = simulation.Bodies.Add(description);
+        return wreckingBallBodyHandle;
+    }
 
-        RolloverInfo rolloverInfo;
+    public static BodyHandle AttachWreckingBall(Simulation simulation, BodyHandle[] bodyHandles, float ropeBodyRadius, float bodySpacing, float constraintOffsetLength, float wreckingBallRadius, BodyInertia wreckingBallInertia, TypedIndex wreckingBallShapeIndex, SpringSettings springSettings)
+    {
+        BodyHandle wreckingBallBodyHandle = CreateWreckingBall(simulation, bodyHandles, ropeBodyRadius, bodySpacing, wreckingBallRadius, wreckingBallInertia, wreckingBallShapeIndex);
+        var maximumDistance = bodySpacing + ropeBodyRadius - constraintOffsetLength;
+        simulation.Solver.Add(bodyHandles[bodyHandles.Length - 1], wreckingBallBodyHandle,
+            new DistanceLimit(new Vector3(0, -constraintOffsetLength, 0), new Vector3(0, wreckingBallRadius, 0), maximumDistance * 0.1f, maximumDistance, springSettings));
+        return wreckingBallBodyHandle;
+    }
 
+    RolloverInfo rolloverInfo;
 
-        public unsafe override void Initialize(ContentArchive content, Camera camera)
-        {
-            camera.Position = new Vector3(0, 25, 80);
-            camera.Yaw = 0;
-            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.
-            //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 SubsteppingTimestepper(4), 1);
+    public override void Initialize(ContentArchive content, Camera camera)
+    {
+        camera.Position = new Vector3(0, 25, 80);
+        camera.Yaw = 0;
+        camera.Pitch = 0;
 
-            //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());
+        //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 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.
+        //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.
 
-            rolloverInfo = new RolloverInfo();
-            var smallWreckingBall = new Sphere(1);
-            smallWreckingBall.ComputeInertia(5, out var smallWreckingBallInertia);
-            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.
-                var startLocation = new Vector3(-55, 35, 0);
-                const float bodySpacing = 0.3f;
-                const float bodyRadius = 0.5f;
-                var springSettings = new SpringSettings(30, 1);
-                var bodyHandles = BuildRope(Simulation, startLocation, 12, bodyRadius, bodySpacing, bodyRadius, 1, 1, springSettings);
+        //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));
 
-                //With a small wrecking ball, this actually works fine with reasonable spring settings.
-                AttachWreckingBall(Simulation, bodyHandles, bodyRadius, bodySpacing, bodyRadius, smallWreckingBall.Radius, smallWreckingBallInertia, smallWreckingBallIndex, springSettings);
-                rolloverInfo.Add(startLocation + new Vector3(0, 2, 0), "Naive, 5:1 mass ratio");
-            }
-            var bigWreckingBall = new Sphere(3);
-            //This wrecking ball is much, much heavier.
-            bigWreckingBall.ComputeInertia(100, out var bigWreckingBallInertia);
-            var bigWreckingBallIndex = Simulation.Shapes.Add(bigWreckingBall);
-            {
-                //Everything identical to the first rope, but now with a heavier wrecking ball.
-                //This is going to behave very poorly. The extremely heavy wrecking ball depends on the much lighter rope bodies.
-                //The force required to keep the wrecking ball in place needs to propagate through the rope to the kinematic at the top, but the low mass of the rope 
-                //makes that a very time consuming process. 
-                //Stabilizing this without changing the constraint configuration requires either an absurd number of solver iterations or increasing the update rate.
-                var startLocation = new Vector3(-35, 35, 0);
-                const float bodySpacing = 0.3f;
-                const float bodyRadius = 0.5f;
-                var springSettings = new SpringSettings(30, 1);
-                var bodyHandles = BuildRope(Simulation, startLocation, 12, bodyRadius, bodySpacing, bodyRadius, 1, 1, springSettings);
+        rolloverInfo = new RolloverInfo();
+        var smallWreckingBall = new Sphere(1);
+        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.
+            var startLocation = new Vector3(-55, 35, 0);
+            const float bodySpacing = 0.3f;
+            const float bodyRadius = 0.5f;
+            var springSettings = new SpringSettings(30, 1);
+            var bodyHandles = BuildRope(Simulation, startLocation, 12, bodyRadius, bodySpacing, bodyRadius, 1, 1, springSettings);
 
-                AttachWreckingBall(Simulation, bodyHandles, bodyRadius, bodySpacing, bodyRadius, bigWreckingBall.Radius, bigWreckingBallInertia, bigWreckingBallIndex, springSettings);
-                rolloverInfo.Add(startLocation + new Vector3(0, 2, 0), "Naive, 100:1 mass ratio");
-            }
-            {
-                //If stiffness is causing a problem, how about we reduce the stiffness by adjusting the spring settings frequency? 
-                //This certainly makes it more stable, but it behaves more like loose elastic than a rope.
-                //If you have a simulation where softness is actually okay, this is often the quickest and easiest fix.
-                var startLocation = new Vector3(-15, 35, 0);
-                const float bodySpacing = 0.3f;
-                const float bodyRadius = 0.5f;
-                var springSettings = new SpringSettings(3, 1);
-                var bodyHandles = BuildRope(Simulation, startLocation, 12, bodyRadius, bodySpacing, bodyRadius, 1, 1, springSettings);
+            //With a small wrecking ball, this actually works fine with reasonable spring settings.
+            AttachWreckingBall(Simulation, bodyHandles, bodyRadius, bodySpacing, bodyRadius, smallWreckingBall.Radius, smallWreckingBallInertia, smallWreckingBallIndex, springSettings);
+            rolloverInfo.Add(startLocation + new Vector3(0, 2, 0), "Naive, 5:1 mass ratio");
+        }
+        var bigWreckingBall = new Sphere(3);
+        //This wrecking ball is much, much heavier.
+        var bigWreckingBallInertia = bigWreckingBall.ComputeInertia(100);
+        var bigWreckingBallIndex = Simulation.Shapes.Add(bigWreckingBall);
+        {
+            //Everything identical to the first rope, but now with a heavier wrecking ball.
+            //This is going to behave very poorly. The extremely heavy wrecking ball depends on the much lighter rope bodies.
+            //The force required to keep the wrecking ball in place needs to propagate through the rope to the kinematic at the top, but the low mass of the rope 
+            //makes that a very time consuming process. 
+            //Stabilizing this without changing the constraint configuration requires either an absurd number of solver iterations or increasing the update rate.
+            var startLocation = new Vector3(-35, 35, 0);
+            const float bodySpacing = 0.3f;
+            const float bodyRadius = 0.5f;
+            var springSettings = new SpringSettings(30, 1);
+            var bodyHandles = BuildRope(Simulation, startLocation, 12, bodyRadius, bodySpacing, bodyRadius, 1, 1, springSettings);
 
-                AttachWreckingBall(Simulation, bodyHandles, bodyRadius, bodySpacing, bodyRadius, bigWreckingBall.Radius, bigWreckingBallInertia, bigWreckingBallIndex, springSettings);
-                rolloverInfo.Add(startLocation + new Vector3(0, 2, 0), "Softer constraints");
-            }
-            {
-                //If the mass ratio between the wrecking ball and rope bodies make it hard to propagate impulses, how about increasing the mass of the rope?
-                //It does help, but now the rope is really heavy. That's not really the behavior we want, and it's still not perfect.
-                var startLocation = new Vector3(-5, 35, 0);
-                const float bodySpacing = 0.3f;
-                const float bodyRadius = 0.5f;
-                var springSettings = new SpringSettings(30, 1);
-                var bodyHandles = BuildRope(Simulation, startLocation, 12, bodyRadius, bodySpacing, bodyRadius, 20, 1, springSettings);
+            AttachWreckingBall(Simulation, bodyHandles, bodyRadius, bodySpacing, bodyRadius, bigWreckingBall.Radius, bigWreckingBallInertia, bigWreckingBallIndex, springSettings);
+            rolloverInfo.Add(startLocation + new Vector3(0, 2, 0), "Naive, 100:1 mass ratio");
+        }
+        {
+            //If stiffness is causing a problem, how about we reduce the stiffness by adjusting the spring settings frequency? 
+            //This certainly makes it more stable, but it behaves more like loose elastic than a rope.
+            //If you have a simulation where softness is actually okay, this is often the quickest and easiest fix.
+            var startLocation = new Vector3(-15, 35, 0);
+            const float bodySpacing = 0.3f;
+            const float bodyRadius = 0.5f;
+            var springSettings = new SpringSettings(3, 1);
+            var bodyHandles = BuildRope(Simulation, startLocation, 12, bodyRadius, bodySpacing, bodyRadius, 1, 1, springSettings);
 
-                AttachWreckingBall(Simulation, bodyHandles, bodyRadius, bodySpacing, bodyRadius, bigWreckingBall.Radius, bigWreckingBallInertia, bigWreckingBallIndex, springSettings);
-                rolloverInfo.Add(startLocation + new Vector3(0, 2, 0), "20x rope mass boost");
-            }
-            {
-                //The difficulty of propagating impulses isn't just a matter of mass alone. The constraint lever arms and body inertia tensors are also very important.
-                //How about we treat the bodies as having way more rotational inertia than their shape would imply, and don't increase the mass as much?
-                //This helps about as much as using the higher mass without quite as much negative impact on behavior.
-                var startLocation = new Vector3(5, 35, 0);
-                const float bodySpacing = 0.3f;
-                const float bodyRadius = 0.5f;
-                var springSettings = new SpringSettings(30, 1);
-                var bodyHandles = BuildRope(Simulation, startLocation, 12, bodyRadius, bodySpacing, bodyRadius, 5, 0.2f, springSettings);
+            AttachWreckingBall(Simulation, bodyHandles, bodyRadius, bodySpacing, bodyRadius, bigWreckingBall.Radius, bigWreckingBallInertia, bigWreckingBallIndex, springSettings);
+            rolloverInfo.Add(startLocation + new Vector3(0, 2, 0), "Softer constraints");
+        }
+        {
+            //If the mass ratio between the wrecking ball and rope bodies make it hard to propagate impulses, how about increasing the mass of the rope?
+            //It does help, but now the rope is really heavy. That's not really the behavior we want, and it's still not perfect.
+            var startLocation = new Vector3(-5, 35, 0);
+            const float bodySpacing = 0.3f;
+            const float bodyRadius = 0.5f;
+            var springSettings = new SpringSettings(30, 1);
+            var bodyHandles = BuildRope(Simulation, startLocation, 12, bodyRadius, bodySpacing, bodyRadius, 20, 1, springSettings);
 
-                AttachWreckingBall(Simulation, bodyHandles, bodyRadius, bodySpacing, bodyRadius, bigWreckingBall.Radius, bigWreckingBallInertia, bigWreckingBallIndex, springSettings);
-                rolloverInfo.Add(startLocation + new Vector3(0, 2, 0), "5x rope mass boost, 25x rope inertia boost");
-            }
-            {
-                //If increasing the inertia helped, and longer lever arms can make things trickier to solve, how about reducing the constraint lever arms to zero 
-                //so that rope body angular motion doesn't matter at all?
-                //(We'll also lock the rope body orientation by setting their rotational inertia to infinity, but since the constraint lever arms are 0 length, that doesn't actually
-                //change anything.)
-                //This helps a lot. The angular oscillation is completely eliminated, and it takes quite a bit to force linear oscillation.
-                var startLocation = new Vector3(15, 35, 0);
-                const float bodySpacing = 0.3f;
-                const float bodyRadius = 0.5f;
-                var springSettings = new SpringSettings(30, 1);
-                var bodyHandles = BuildRope(Simulation, startLocation, 12, bodyRadius, bodySpacing, 0, 1, 0, springSettings);
+            AttachWreckingBall(Simulation, bodyHandles, bodyRadius, bodySpacing, bodyRadius, bigWreckingBall.Radius, bigWreckingBallInertia, bigWreckingBallIndex, springSettings);
+            rolloverInfo.Add(startLocation + new Vector3(0, 2, 0), "20x rope mass boost");
+        }
+        {
+            //The difficulty of propagating impulses isn't just a matter of mass alone. The constraint lever arms and body inertia tensors are also very important.
+            //How about we treat the bodies as having way more rotational inertia than their shape would imply, and don't increase the mass as much?
+            //This helps about as much as using the higher mass without quite as much negative impact on behavior.
+            var startLocation = new Vector3(5, 35, 0);
+            const float bodySpacing = 0.3f;
+            const float bodyRadius = 0.5f;
+            var springSettings = new SpringSettings(30, 1);
+            var bodyHandles = BuildRope(Simulation, startLocation, 12, bodyRadius, bodySpacing, bodyRadius, 5, 0.2f, springSettings);
 
-                AttachWreckingBall(Simulation, bodyHandles, bodyRadius, bodySpacing, 0, bigWreckingBall.Radius, bigWreckingBallInertia, bigWreckingBallIndex, springSettings);
-                rolloverInfo.Add(startLocation + new Vector3(0, 2, 0), "0 lever arm");
-            }
-            {
-                //But what if we don't want to change the mass of the rope bodies, but we want to better than zero lever arm?
-                //There's still an option! Attach the wrecking ball directly to the kinematic. No more concern about impulse propagation.
-                //This makes the rest of the rope effectively cosmetic when the wrecking ball is freely swinging.
-                //The problem is that you can *tell* that the rope is cosmetic. It's not loaded, so it flops around too much.
-                //Further, if the rope wraps around something such that the cheat constraint isn't holding the weight anymore, the rope will freak out just as bad as it did in the first attempt.
-                var startLocation = new Vector3(25, 35, 0);
-                const float bodySpacing = 0.3f;
-                const float bodyRadius = 0.5f;
-                var springSettings = new SpringSettings(30, 1);
-                var bodyHandles = BuildRope(Simulation, startLocation, 12, bodyRadius, bodySpacing, 0, 1f, 0, springSettings);
+            AttachWreckingBall(Simulation, bodyHandles, bodyRadius, bodySpacing, bodyRadius, bigWreckingBall.Radius, bigWreckingBallInertia, bigWreckingBallIndex, springSettings);
+            rolloverInfo.Add(startLocation + new Vector3(0, 2, 0), "5x rope mass boost, 25x rope inertia boost");
+        }
+        {
+            //If increasing the inertia helped, and longer lever arms can make things trickier to solve, how about reducing the constraint lever arms to zero 
+            //so that rope body angular motion doesn't matter at all?
+            //(We'll also lock the rope body orientation by setting their rotational inertia to infinity, but since the constraint lever arms are 0 length, that doesn't actually
+            //change anything.)
+            //This helps a lot. The angular oscillation is completely eliminated, and it takes quite a bit to force linear oscillation.
+            var startLocation = new Vector3(15, 35, 0);
+            const float bodySpacing = 0.3f;
+            const float bodyRadius = 0.5f;
+            var springSettings = new SpringSettings(30, 1);
+            var bodyHandles = BuildRope(Simulation, startLocation, 12, bodyRadius, bodySpacing, 0, 1, 0, springSettings);
+
+            AttachWreckingBall(Simulation, bodyHandles, bodyRadius, bodySpacing, 0, bigWreckingBall.Radius, bigWreckingBallInertia, bigWreckingBallIndex, springSettings);
+            rolloverInfo.Add(startLocation + new Vector3(0, 2, 0), "0 lever arm");
+        }
+        {
+            //But what if we don't want to change the mass of the rope bodies, but we want to better than zero lever arm?
+            //There's still an option! Attach the wrecking ball directly to the kinematic. No more concern about impulse propagation.
+            //This makes the rest of the rope effectively cosmetic when the wrecking ball is freely swinging.
+            //The problem is that you can *tell* that the rope is cosmetic. It's not loaded, so it flops around too much.
+            //Further, if the rope wraps around something such that the cheat constraint isn't holding the weight anymore, the rope will freak out just as bad as it did in the first attempt.
+            var startLocation = new Vector3(25, 35, 0);
+            const float bodySpacing = 0.3f;
+            const float bodyRadius = 0.5f;
+            var springSettings = new SpringSettings(30, 1);
+            var bodyHandles = BuildRope(Simulation, startLocation, 12, bodyRadius, bodySpacing, 0, 1f, 0, springSettings);
+
+            var wreckingBallHandle = AttachWreckingBall(Simulation, bodyHandles, bodyRadius, bodySpacing, 0, bigWreckingBall.Radius, bigWreckingBallInertia, bigWreckingBallIndex, springSettings);
+            var wreckingBallConnectionOffset = new Vector3(0, bigWreckingBall.Radius, 0);
+            var maximumDistance = Vector3.Distance(
+                new BodyReference(bodyHandles[0], Simulation.Bodies).Pose.Position,
+                new BodyReference(wreckingBallHandle, Simulation.Bodies).Pose.Position + wreckingBallConnectionOffset);
+            Simulation.Solver.Add(bodyHandles[0], wreckingBallHandle, new DistanceLimit(default, wreckingBallConnectionOffset, 0.01f, maximumDistance, springSettings));
+            rolloverInfo.Add(startLocation + new Vector3(0, 2, 0), "0 lever arm, direct cheat constraint");
+        }
+        {
+            //The last attempt was pretty stable, but how do we address its problems? Instead of having a single constraint from the source to the target, we can
+            //create a bunch of 'skip constraints' between nearby rope bodies. Rather than having to traverse the constraint graph one body a time,
+            //these skip constraints allow impulses to propagate through the graph along multiple 'shortcut' paths at the same time. Any one body acting as a bottleneck
+            //has its load propagated quickly to all its neighbors.
+            //As a result, this is extremely stable. You can choose to increase the number of skip constraints for additional stability. Reducing the rope mass further is very possible.
+            var startLocation = new Vector3(35, 140, 0);
+            const float bodySpacing = 0.3f;
+            const float bodyRadius = 0.5f;
+            var springSettings = new SpringSettings(30, 1);
+            var bodyHandles = BuildRopeBodies(Simulation, startLocation, 100, bodyRadius, bodySpacing, 1f, 0);
 
-                var wreckingBallHandle = AttachWreckingBall(Simulation, bodyHandles, bodyRadius, bodySpacing, 0, bigWreckingBall.Radius, bigWreckingBallInertia, bigWreckingBallIndex, springSettings);
-                var wreckingBallConnectionOffset = new Vector3(0, bigWreckingBall.Radius, 0);
+            bool TryCreateConstraint(int handleIndexA, int handleIndexB)
+            {
+                if (handleIndexA >= bodyHandles.Length || handleIndexB >= bodyHandles.Length)
+                    return false;
                 var maximumDistance = Vector3.Distance(
-                    new BodyReference(bodyHandles[0], Simulation.Bodies).Pose.Position,
-                    new BodyReference(wreckingBallHandle, Simulation.Bodies).Pose.Position + wreckingBallConnectionOffset);
-                Simulation.Solver.Add(bodyHandles[0], wreckingBallHandle, new DistanceLimit(default, wreckingBallConnectionOffset, 0.01f, maximumDistance, springSettings));
-                rolloverInfo.Add(startLocation + new Vector3(0, 2, 0), "0 lever arm, direct cheat constraint");
+                    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 = 4;
+            for (int i = 0; i < bodyHandles.Length - 1; ++i)
             {
-                //The last attempt was pretty stable, but how do we address its problems? Instead of having a single constraint from the source to the target, we can
-                //create a bunch of 'skip constraints' between nearby rope bodies. Rather than having to traverse the constraint graph one body a time,
-                //these skip constraints allow impulses to propagate through the graph along multiple 'shortcut' paths at the same time. Any one body acting as a bottleneck
-                //has its load propagated quickly to all its neighbors.
-                //As a result, this is extremely stable. You can choose to increase the number of skip constraints for additional stability. Reducing the rope mass further is very possible.
-                var startLocation = new Vector3(35, 140, 0);
-                const float bodySpacing = 0.3f;
-                const float bodyRadius = 0.5f;
-                var springSettings = new SpringSettings(30, 1);
-                var bodyHandles = BuildRopeBodies(Simulation, startLocation, 100, bodyRadius, bodySpacing, 1f, 0);
-
-                bool TryCreateConstraint(int handleIndexA, int handleIndexB)
+                //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 (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 = 4;
-                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 wreckingBallHandle = CreateWreckingBall(Simulation, bodyHandles, bodyRadius, bodySpacing, bigWreckingBall.Radius, bigWreckingBallInertia, bigWreckingBallIndex);
-                var wreckingBallConnectionOffset = new Vector3(0, bigWreckingBall.Radius, 0);
-                for (int i = 1; i <= constraintsPerBody; ++i)
-                {
-                    var targetBodyHandleIndex = bodyHandles.Length - i;
-                    if (targetBodyHandleIndex < 0)
+                    if (!TryCreateConstraint(i, i + j))
                         break;
-                    var maximumDistance = Vector3.Distance(
-                        new BodyReference(bodyHandles[targetBodyHandleIndex], Simulation.Bodies).Pose.Position,
-                        new BodyReference(wreckingBallHandle, Simulation.Bodies).Pose.Position + wreckingBallConnectionOffset);
-                    Simulation.Solver.Add(bodyHandles[targetBodyHandleIndex], wreckingBallHandle, new DistanceLimit(default, wreckingBallConnectionOffset, 0.01f, maximumDistance, springSettings));
                 }
-                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(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)));
-
+            var wreckingBallHandle = CreateWreckingBall(Simulation, bodyHandles, bodyRadius, bodySpacing, bigWreckingBall.Radius, bigWreckingBallInertia, bigWreckingBallIndex);
+            var wreckingBallConnectionOffset = new Vector3(0, bigWreckingBall.Radius, 0);
+            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,
+                    new BodyReference(wreckingBallHandle, Simulation.Bodies).Pose.Position + wreckingBallConnectionOffset);
+                Simulation.Solver.Add(bodyHandles[targetBodyHandleIndex], wreckingBallHandle, new DistanceLimit(default, wreckingBallConnectionOffset, 0.01f, maximumDistance, springSettings));
+            }
+            rolloverInfo.Add(startLocation, $"100:1 mass ratio, {constraintsPerBody - 1}x extra skip constraints");
         }
 
-        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);
-        }
+        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),
+            Simulation.Shapes.Add(new Capsule(8, 64))));
+
+    }
 
+    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);
     }
+
 }
diff --git a/Demos/Demos/RopeTwistDemo.cs b/Demos/Demos/RopeTwistDemo.cs
new file mode 100644
index 000000000..e8475b39b
--- /dev/null
+++ b/Demos/Demos/RopeTwistDemo.cs
@@ -0,0 +1,183 @@
+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;
+
+/// <summary>
+/// Filter for a body in a rope, used by the <see cref="RopeNarrowPhaseCallbacks"/>.
+/// </summary>
+struct RopeFilter
+{
+    public short RopeIndex;
+    public short IndexInRope;
+}
+
+/// <summary>
+/// Narrow phase callbacks that include collision filters designed for ropes. Adjacent bodies in a rope do not collide with each other.
+/// </summary>
+struct RopeNarrowPhaseCallbacks : INarrowPhaseCallbacks
+{
+    public CollidableProperty<RopeFilter> Filters;
+    public PairMaterialProperties Material;
+    public int MinimumDistanceForCollisions;
+
+    public RopeNarrowPhaseCallbacks(CollidableProperty<RopeFilter> filters, PairMaterialProperties contactMaterial, int minimumDistanceForCollisions = 3)
+    {
+        Filters = filters;
+        Material = contactMaterial;
+        MinimumDistanceForCollisions = minimumDistanceForCollisions;
+    }
+    public RopeNarrowPhaseCallbacks(CollidableProperty<RopeFilter> filters, int minimumDistanceForCollisions = 3) : this(filters, new PairMaterialProperties(1, 2, new SpringSettings(30, 1)), minimumDistanceForCollisions)
+    {
+    }
+
+    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) > 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 ConfigureContactManifold<TManifold>(int workerIndex, CollidablePair pair, ref TManifold manifold, out PairMaterialProperties pairMaterial) where TManifold : unmanaged, IContactManifold<TManifold>
+    {
+        pairMaterial = Material;
+        return true;
+    }
+
+    [MethodImpl(MethodImplOptions.AggressiveInlining)]
+    public bool ConfigureContactManifold(int workerIndex, CollidablePair pair, int childIndexA, int childIndexB, ref ConvexContactManifold manifold)
+    {
+        return true;
+    }
+
+    public void Dispose()
+    {
+    }
+}
+
+/// <summary>
+/// Shows a bundle of ropes being tangled up by spinning weights.
+/// </summary>
+public class RopeTwistDemo : Demo
+{
+    public override void Initialize(ContentArchive content, Camera camera)
+    {
+        camera.Position = new Vector3(0, 20, 20);
+        camera.Yaw = 0;
+        camera.Pitch = 0;
+
+        var filters = new CollidableProperty<RopeFilter>();
+        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));
+
+        for (int twistIndex = 0; twistIndex < 1; ++twistIndex)
+        {
+            const int ropeCount = 4;
+            var startLocation = new Vector3(0 + twistIndex * 15, 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));
+                }
+
+            }
+        }
+
+
+        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);
+    }
+}
diff --git a/Demos/Demos/SimpleSelfContainedDemo.cs b/Demos/Demos/SimpleSelfContainedDemo.cs
index a64d3844a..d5a13f377 100644
--- a/Demos/Demos/SimpleSelfContainedDemo.cs
+++ b/Demos/Demos/SimpleSelfContainedDemo.cs
@@ -5,213 +5,255 @@
 using BepuUtilities;
 using BepuUtilities.Memory;
 using System;
-using System.Collections.Generic;
 using System.Numerics;
 using System.Runtime.CompilerServices;
-using System.Text;
 
-namespace Demos.Demos
+namespace Demos.Demos;
+
+/// <summary>
+/// Shows a completely isolated usage of the engine without using any of the other demo physics-related types.
+/// </summary>
+public static class SimpleSelfContainedDemo
 {
-    /// <summary>
-    /// Shows a completely isolated usage of the engine without using any of the other demo types.
-    /// </summary>
-    public static class SimpleSelfContainedDemo
+    //The simulation has a variety of extension points that must be defined. 
+    //The demos tend to reuse a few types like the DemoNarrowPhaseCallbacks, but this demo will provide its own (super simple) versions.
+    //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.
+    struct NarrowPhaseCallbacks : INarrowPhaseCallbacks
     {
-        //The simulation has a variety of extension points that must be defined. 
-        //The demos tend to reuse a few types like the DemoNarrowPhaseCallbacks, but this demo will provide its own (super simple) versions.
-        //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
+        /// <summary>
+        /// Performs any required initialization logic after the Simulation instance has been constructed.
+        /// </summary>
+        /// <param name="simulation">Simulation that owns these callbacks.</param>
+        public void Initialize(Simulation simulation)
+        {
+            //Often, the callbacks type is created before the simulation instance is fully constructed, so the simulation will call this function when it's ready.
+            //Any logic which depends on the simulation existing can be put here.
+        }
+
+        /// <summary>
+        /// Chooses whether to allow contact generation to proceed for two overlapping collidables.
+        /// </summary>
+        /// <param name="workerIndex">Index of the worker that identified the overlap.</param>
+        /// <param name="a">Reference to the first collidable in the pair.</param>
+        /// <param name="b">Reference to the second collidable in the pair.</param>
+        /// <param name="speculativeMargin">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.</param>
+        /// <returns>True if collision detection should proceed, false otherwise.</returns>
+        [MethodImpl(MethodImplOptions.AggressiveInlining)]
+        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.
+            //Here, we'll make sure at least one of the two bodies is dynamic.
+            //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;
+        }
+
+        /// <summary>
+        /// Chooses whether to allow contact generation to proceed for the children of two overlapping collidables in a compound-including pair.
+        /// </summary>
+        /// <param name="workerIndex">Index of the worker thread processing this pair.</param>
+        /// <param name="pair">Parent pair of the two child collidables.</param>
+        /// <param name="childIndexA">Index of the child of collidable A in the pair. If collidable A is not compound, then this is always 0.</param>
+        /// <param name="childIndexB">Index of the child of collidable B in the pair. If collidable B is not compound, then this is always 0.</param>
+        /// <returns>True if collision detection should proceed, false otherwise.</returns>
+        /// <remarks>This is called for each sub-overlap in a collidable pair involving compound collidables. If neither collidable in a pair is compound, this will not be called.
+        /// For compound-including pairs, if the earlier call to AllowContactGeneration returns false for owning pair, this will not be called. Note that it is possible
+        /// for this function to be called twice for the same subpair if the pair has continuous collision detection enabled; 
+        /// the CCD sweep test that runs before the contact generation test also asks before performing child pair tests.</remarks>
+        [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 only gets called in pairs that involve at least one shape type that can contain multiple children, like a Compound.
+            return true;
+        }
+
+        /// <summary>
+        /// Provides a notification that a manifold has been created for a pair. Offers an opportunity to change the manifold's details. 
+        /// </summary>
+        /// <param name="workerIndex">Index of the worker thread that created this manifold.</param>
+        /// <param name="pair">Pair of collidables that the manifold was detected between.</param>
+        /// <param name="manifold">Set of contacts detected between the collidables.</param>
+        /// <param name="pairMaterial">Material properties of the manifold.</param>
+        /// <returns>True if a constraint should be created for the manifold, false otherwise.</returns>
+        [MethodImpl(MethodImplOptions.AggressiveInlining)]
+        public bool ConfigureContactManifold<TManifold>(int workerIndex, CollidablePair pair, ref TManifold manifold, out PairMaterialProperties pairMaterial) where TManifold : unmanaged, IContactManifold<TManifold>
         {
-            /// <summary>
-            /// Performs any required initialization logic after the Simulation instance has been constructed.
-            /// </summary>
-            /// <param name="simulation">Simulation that owns these callbacks.</param>
-            public void Initialize(Simulation simulation)
-            {
-                //Often, the callbacks type is created before the simulation instance is fully constructed, so the simulation will call this function when it's ready.
-                //Any logic which depends on the simulation existing can be put here.
-            }
-
-            /// <summary>
-            /// Chooses whether to allow contact generation to proceed for two overlapping collidables.
-            /// </summary>
-            /// <param name="workerIndex">Index of the worker that identified the overlap.</param>
-            /// <param name="a">Reference to the first collidable in the pair.</param>
-            /// <param name="b">Reference to the second collidable in the pair.</param>
-            /// <returns>True if collision detection should proceed, false otherwise.</returns>
-            [MethodImpl(MethodImplOptions.AggressiveInlining)]
-            public bool AllowContactGeneration(int workerIndex, CollidableReference a, CollidableReference b)
-            {
-                //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.
-                //Here, we'll make sure at least one of the two bodies is dynamic.
-                //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.
-                return a.Mobility == CollidableMobility.Dynamic || b.Mobility == CollidableMobility.Dynamic;
-            }
-
-            /// <summary>
-            /// Chooses whether to allow contact generation to proceed for the children of two overlapping collidables in a compound-including pair.
-            /// </summary>
-            /// <param name="pair">Parent pair of the two child collidables.</param>
-            /// <param name="childIndexA">Index of the child of collidable A in the pair. If collidable A is not compound, then this is always 0.</param>
-            /// <param name="childIndexB">Index of the child of collidable B in the pair. If collidable B is not compound, then this is always 0.</param>
-            /// <returns>True if collision detection should proceed, false otherwise.</returns>
-            /// <remarks>This is called for each sub-overlap in a collidable pair involving compound collidables. If neither collidable in a pair is compound, this will not be called.
-            /// For compound-including pairs, if the earlier call to AllowContactGeneration returns false for owning pair, this will not be called. Note that it is possible
-            /// for this function to be called twice for the same subpair if the pair has continuous collision detection enabled; 
-            /// the CCD sweep test that runs before the contact generation test also asks before performing child pair tests.</remarks>
-            [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 only gets called in pairs that involve at least one shape type that can contain multiple children, like a Compound.
-                return true;
-            }
-
-            /// <summary>
-            /// Provides a notification that a manifold has been created for a pair. Offers an opportunity to change the manifold's details. 
-            /// </summary>
-            /// <param name="workerIndex">Index of the worker thread that created this manifold.</param>
-            /// <param name="pair">Pair of collidables that the manifold was detected between.</param>
-            /// <param name="manifold">Set of contacts detected between the collidables.</param>
-            /// <param name="pairMaterial">Material properties of the manifold.</param>
-            /// <returns>True if a constraint should be created for the manifold, false otherwise.</returns>
-            [MethodImpl(MethodImplOptions.AggressiveInlining)]
-            public unsafe bool ConfigureContactManifold<TManifold>(int workerIndex, CollidablePair pair, ref TManifold manifold, out PairMaterialProperties pairMaterial) where TManifold : unmanaged, IContactManifold<TManifold>
-            {
-                //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<TManifold, ConvexContactManifold or NonconvexContactManifold>(ref manifold).
-
-                //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.)
-                pairMaterial.FrictionCoefficient = 1f;
-                pairMaterial.MaximumRecoveryVelocity = 2f;
-                pairMaterial.SpringSettings = new SpringSettings(30, 1);
-                //For the purposes of the demo, contact constraints are always generated.
-                return true;
-            }
-
-            /// <summary>
-            /// Provides a notification that a manifold has been created between the children of two collidables in a compound-including pair.
-            /// Offers an opportunity to change the manifold's details. 
-            /// </summary>
-            /// <param name="workerIndex">Index of the worker thread that created this manifold.</param>
-            /// <param name="pair">Pair of collidables that the manifold was detected between.</param>
-            /// <param name="childIndexA">Index of the child of collidable A in the pair. If collidable A is not compound, then this is always 0.</param>
-            /// <param name="childIndexB">Index of the child of collidable B in the pair. If collidable B is not compound, then this is always 0.</param>
-            /// <param name="manifold">Set of contacts detected between the collidables.</param>
-            /// <returns>True if this manifold should be considered for constraint generation, false otherwise.</returns>
-            [MethodImpl(MethodImplOptions.AggressiveInlining)]
-            public bool ConfigureContactManifold(int workerIndex, CollidablePair pair, int childIndexA, int childIndexB, ref ConvexContactManifold manifold)
-            {
-                return true;
-            }
-
-            /// <summary>
-            /// Releases any resources held by the callbacks. Called by the owning narrow phase when it is being disposed.
-            /// </summary>
-            public void Dispose()
-            {
-            }
+            //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<TManifold, ConvexContactManifold or NonconvexContactManifold>(ref manifold).
+
+            //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' 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);
+            //For the purposes of the demo, contact constraints are always generated.
+            return true;
         }
 
-        //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
+        /// <summary>
+        /// Provides a notification that a manifold has been created between the children of two collidables in a compound-including pair.
+        /// Offers an opportunity to change the manifold's details. 
+        /// </summary>
+        /// <param name="workerIndex">Index of the worker thread that created this manifold.</param>
+        /// <param name="pair">Pair of collidables that the manifold was detected between.</param>
+        /// <param name="childIndexA">Index of the child of collidable A in the pair. If collidable A is not compound, then this is always 0.</param>
+        /// <param name="childIndexB">Index of the child of collidable B in the pair. If collidable B is not compound, then this is always 0.</param>
+        /// <param name="manifold">Set of contacts detected between the collidables.</param>
+        /// <returns>True if this manifold should be considered for constraint generation, false otherwise.</returns>
+        [MethodImpl(MethodImplOptions.AggressiveInlining)]
+        public bool ConfigureContactManifold(int workerIndex, CollidablePair pair, int childIndexA, int childIndexB, ref ConvexContactManifold manifold)
         {
-            public Vector3 Gravity;
-            Vector3 gravityDt;
-
-            /// <summary>
-            /// Performs any required initialization logic after the Simulation instance has been constructed.
-            /// </summary>
-            /// <param name="simulation">Simulation that owns these callbacks.</param>
-            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<T> or something similar, having the simulation provided in a callback can be helpful.
-            }
-
-            /// <summary>
-            /// Gets how the pose integrator should handle angular velocity integration.
-            /// </summary>
-            public AngularIntegrationMode AngularIntegrationMode => AngularIntegrationMode.Nonconserving; //Don't care about fidelity in this demo!
-
-            public PoseIntegratorCallbacks(Vector3 gravity) : this()
-            {
-                Gravity = gravity;
-            }
-
-            /// <summary>
-            /// 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.
-            /// </summary>
-            /// <param name="dt">Current time step duration.</param>
-            public void PrepareForIntegration(float dt)
-            {
-                //No reason to recalculate gravity * dt for every body; just cache it ahead of time.
-                gravityDt = Gravity * dt;
-            }
-
-            /// <summary>
-            /// Callback called for each active body within the simulation during body integration.
-            /// </summary>
-            /// <param name="bodyIndex">Index of the body being visited.</param>
-            /// <param name="pose">Body's current pose.</param>
-            /// <param name="localInertia">Body's current local inertia.</param>
-            /// <param name="workerIndex">Index of the worker thread processing this body.</param>
-            /// <param name="velocity">Reference to the body's current velocity to integrate.</param>
-            [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;
-                }
-            }
+            return true;
+        }
 
+        /// <summary>
+        /// Releases any resources held by the callbacks. Called by the owning narrow phase when it is being disposed.
+        /// </summary>
+        public void Dispose()
+        {
         }
+    }
 
-        public static void Run()
+    //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
+    {
+        /// <summary>
+        /// Performs any required initialization logic after the Simulation instance has been constructed.
+        /// </summary>
+        /// <param name="simulation">Simulation that owns these callbacks.</param>
+        public void Initialize(Simulation simulation)
         {
-            //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.)
-            var simulation = Simulation.Create(bufferPool, new NarrowPhaseCallbacks(), new PoseIntegratorCallbacks(new Vector3(0, -10, 0)), new PositionLastTimestepper());
-
-            //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.Statics.Add(new StaticDescription(new Vector3(0, 0, 0), new CollidableDescription(simulation.Shapes.Add(new Box(500, 1, 500)), 0.1f)));
-
-            var threadDispatcher = new SimpleThreadDispatcher(Environment.ProcessorCount);
-
-            //Now take 100 time steps!
-            for (int i = 0; i < 100; ++i)
-            {
-                //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.
-                simulation.Timestep(0.01f, threadDispatcher);
-            }
-
-            //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.
-            simulation.Dispose();
-            threadDispatcher.Dispose();
-            bufferPool.Clear();
+            //In this demo, we don't need to initialize anything.
+            //If you had a simulation with per body gravity stored in a CollidableProperty<T> or something similar, having the simulation provided in a callback can be helpful.
         }
+
+        /// <summary>
+        /// Gets how the pose integrator should handle angular velocity integration.
+        /// </summary>
+        public readonly AngularIntegrationMode AngularIntegrationMode => AngularIntegrationMode.Nonconserving;
+
+        /// <summary>
+        /// 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. 
+        /// </summary>
+        public readonly bool AllowSubstepsForUnconstrainedBodies => false;
+
+        /// <summary>
+        /// 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.
+        /// </summary>
+        public readonly bool IntegrateVelocityForKinematics => false;
+
+        public Vector3 Gravity;
+
+        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<float>.Count bodies simultaneously.
+        Vector3Wide gravityWideDt;
+
+        /// <summary>
+        /// Callback invoked ahead of dispatches that may call into <see cref="IntegrateVelocity"/>.
+        /// 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. 
+        /// </summary>
+        /// <param name="dt">Current integration time step duration.</param>
+        /// <remarks>This is typically used for precomputing anything expensive that will be used across velocity integration.</remarks>
+        public void PrepareForIntegration(float dt)
+        {
+            //No reason to recalculate gravity * dt for every body; just cache it ahead of time.
+            gravityWideDt = Vector3Wide.Broadcast(Gravity * dt);
+        }
+
+        /// <summary>
+        /// Callback for a bundle of bodies being integrated.
+        /// </summary>
+        /// <param name="bodyIndices">Indices of the bodies being integrated in this bundle.</param>
+        /// <param name="position">Current body positions.</param>
+        /// <param name="orientation">Current body orientations.</param>
+        /// <param name="localInertia">Body's current local inertia.</param>
+        /// <param name="integrationMask">Mask indicating which lanes are active in the bundle. Active lanes will contain 0xFFFFFFFF, inactive lanes will contain 0.</param>
+        /// <param name="workerIndex">Index of the worker thread processing this bundle.</param>
+        /// <param name="dt">Durations to integrate the velocity over. Can vary over lanes.</param>
+        /// <param name="velocity">Velocity of bodies in the bundle. Any changes to lanes which are not active by the integrationMask will be discarded.</param>
+        public void IntegrateVelocity(Vector<int> bodyIndices, Vector3Wide position, QuaternionWide orientation, BodyInertiaWide localInertia, Vector<int> integrationMask, int workerIndex, Vector<float> 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<float>.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;
+        }
+
+    }
+
+    public static void Run()
+    {
+        //The buffer pool is a source of raw memory blobs for the engine to use.
+        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.
+        var sphere = new Sphere(1);
+        var sphereInertia = sphere.ComputeInertia(1);
+        var bodyHandle = simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(0, 5, 0), sphereInertia, simulation.Shapes.Add(sphere), 0.01f));
+
+        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);
+
+        //Now take 100 time steps!
+        for (int i = 0; i < 100; ++i)
+        {
+            //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);
+
+            //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.
+        simulation.Dispose();
+        threadDispatcher.Dispose();
+        bufferPool.Clear();
     }
 }
diff --git a/Demos/Demos/SolverContactEnumerationDemo.cs b/Demos/Demos/SolverContactEnumerationDemo.cs
index 371c35d41..9c37e23e4 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;
@@ -12,257 +13,276 @@
 using System;
 using System.Numerics;
 
-namespace Demos.Demos
+namespace Demos.Demos;
+
+
+/// <summary>
+/// Shows how to enumerate contact constraints in the solver using contact accessors and <see cref="ISolverContactDataExtractor"/>.
+/// </summary>
+public class SolverContactEnumerationDemo : Demo
 {
     /// <summary>
-    /// Shows how to enumerate contact constraints in the solver using contact accessors and ISolverContactDataExtractor.
+    /// Stores the easy to read data associated with a single contact extracted from solver contact constraints.
     /// </summary>
-    public class SolverContactEnumerationDemo : Demo
+    struct ExtractedContact
     {
-        BodyHandle sensorBodyHandle;
-        public override void Initialize(ContentArchive content, Camera camera)
-        {
-            camera.Position = new Vector3(0, 5, 25);
-            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());
-
-            //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 boxIndex = Simulation.Shapes.Add(boxShape);
+        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;
+    }
 
-            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 + 10, 0),
-                        boxInertia,
-                        new CollidableDescription(boxIndex, 0.1f),
-                        new BodyActivityDescription(0.01f)));
-                }
-            }
+    /// <summary>
+    /// Stores the connected bodies and contacts for a contact manifold constraint extracted from the solver.
+    /// </summary>
+    struct ExtractedManifold
+    {
+        public QuickList<ExtractedContact> 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<ExtractedContact>(NonconvexContactManifold.MaximumContactCount, pool);
+            BodyA = a;
+            BodyB = b;
+        }
 
-            sensorBodyHandle = Simulation.Bodies.Add(BodyDescription.CreateConvexDynamic(new Vector3(0, 0, 1), 10, Simulation.Shapes, new Box(4, 2, 6)));
+        public ExtractedManifold(BufferPool pool, BodyHandle a) : this(pool, a, new BodyHandle(-1)) { }
 
-            //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),
-                new CollidableDescription(Simulation.Shapes.Add(planeMesh), 0.1f)));
+        public void Dispose(BufferPool pool)
+        {
+            Contacts.Dispose(pool);
         }
+    }
 
-        struct Contact
+    /// <summary>
+    /// Example implementation of a <see cref="ISolverContactDataExtractor"/> that pulls contact data into an easier to read format.
+    /// </summary>
+    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<ExtractedManifold> Constraints;
+        public BufferPool Pool;
+
+        public SolverContactDataExtractor(BufferPool pool, int initialCapacity)
         {
-            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;
+            Pool = pool;
+            Constraints = new QuickList<ExtractedManifold>(initialCapacity, pool);
         }
 
-        struct ConstraintContacts
+        //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<TPrestep, TAccumulatedImpulses>(ref ExtractedManifold constraintContacts, ref TPrestep prestep, ref TAccumulatedImpulses impulses)
+            where TPrestep : struct, IConvexContactPrestep<TPrestep>
+            where TAccumulatedImpulses : struct, IConvexContactAccumulatedImpulses<TAccumulatedImpulses>
         {
-            public QuickList<Contact> Contacts;
-            public BodyHandle BodyA;
-            //For one body constraints, this will be -1.
-            public BodyHandle BodyB;
+            //Note that we narrow from the raw vectorized reference into a more application-convenient AOS representation.
+            Vector3Wide.ReadFirst(TPrestep.GetNormal(ref prestep), out var normal);
 
-            public ConstraintContacts(BufferPool pool, BodyHandle a, BodyHandle b)
+            //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 < TPrestep.ContactCount; ++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 = TAccumulatedImpulses.GetPenetrationImpulseForContact(ref impulses, i)[0];
+                totalPenetrationImpulse += targetContact.PenetrationImpulse;
+            }
+            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 < TPrestep.ContactCount; ++i)
             {
-                //Nonconvex manifolds will never have less than the convex count, so we'll preallocate enough space for a nonconvex manifold.
-                Contacts = new QuickList<Contact>(NonconvexContactManifold.MaximumContactCount, pool);
-                BodyA = a;
-                BodyB = b;
+                ref var contact = ref constraintContacts.Contacts[i];
+                contact.FrictionImpulseMagnitude = contact.PenetrationImpulse * impulseScale;
             }
+        }
 
-            public ConstraintContacts(BufferPool pool, BodyHandle a) : this(pool, a, new BodyHandle(-1)) { }
+        public void ConvexOneBody<TPrestep, TAccumulatedImpulses>(BodyHandle bodyHandle, ref TPrestep prestep, ref TAccumulatedImpulses impulses)
+            where TPrestep : struct, IConvexContactPrestep<TPrestep>
+            where TAccumulatedImpulses : struct, IConvexContactAccumulatedImpulses<TAccumulatedImpulses>
+        {
+            ref var constraintContacts = ref Constraints.Allocate(Pool);
+            constraintContacts = new ExtractedManifold(Pool, bodyHandle);
+            ExtractConvexData(ref constraintContacts, ref prestep, ref impulses);
         }
 
-        struct Extractor : ISolverContactDataExtractor
+        public void ConvexTwoBody<TPrestep, TAccumulatedImpulses>(BodyHandle bodyHandleA, BodyHandle bodyHandleB, ref TPrestep prestep, ref TAccumulatedImpulses impulses)
+            where TPrestep : struct, ITwoBodyConvexContactPrestep<TPrestep>
+            where TAccumulatedImpulses : struct, IConvexContactAccumulatedImpulses<TAccumulatedImpulses>
         {
-            //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> ConstraintContacts;
-            public BufferPool Pool;
+            ref var constraintContacts = ref Constraints.Allocate(Pool);
+            constraintContacts = new ExtractedManifold(Pool, bodyHandleA, bodyHandleB);
+            ExtractConvexData(ref constraintContacts, ref prestep, ref impulses);
+        }
 
-            public Extractor(BufferPool pool, int initialCapacity)
+        void ExtractNonconvexData<TPrestep, TAccumulatedImpulses>(ref ExtractedManifold constraintContacts, ref TPrestep prestep, ref TAccumulatedImpulses impulses)
+            where TPrestep : struct, INonconvexContactPrestep<TPrestep>
+            where TAccumulatedImpulses : struct, INonconvexContactAccumulatedImpulses<TAccumulatedImpulses>
+        {
+            //Nonconvex types require no approximation of friction; we can pull it directly from the solved results.
+            for (int i = 0; i < TPrestep.ContactCount; ++i)
             {
-                Pool = pool;
-                ConstraintContacts = new QuickList<ConstraintContacts>(initialCapacity, pool);
+                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 TAccumulatedImpulses.GetImpulsesForContact(ref impulses, i);
+                targetContact.PenetrationImpulse = contactImpulses.Penetration[0];
+                Vector2Wide.ReadFirst(contactImpulses.Tangent, out var tangentImpulses);
+                targetContact.FrictionImpulseMagnitude = tangentImpulses.Length();
             }
+        }
 
-            //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.
+        public void NonconvexOneBody<TPrestep, TAccumulatedImpulses>(BodyHandle bodyHandle, ref TPrestep prestep, ref TAccumulatedImpulses impulses)
+            where TPrestep : struct, INonconvexContactPrestep<TPrestep>
+            where TAccumulatedImpulses : struct, INonconvexContactAccumulatedImpulses<TAccumulatedImpulses>
+        {
+            ref var constraintContacts = ref Constraints.Allocate(Pool);
+            constraintContacts = new ExtractedManifold(Pool, bodyHandle);
+            ExtractNonconvexData(ref constraintContacts, ref prestep, ref impulses);
+        }
 
-            //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.
+        public void NonconvexTwoBody<TPrestep, TAccumulatedImpulses>(BodyHandle bodyHandleA, BodyHandle bodyHandleB, ref TPrestep prestep, ref TAccumulatedImpulses impulses)
+            where TPrestep : struct, ITwoBodyNonconvexContactPrestep<TPrestep>
+            where TAccumulatedImpulses : struct, INonconvexContactAccumulatedImpulses<TAccumulatedImpulses>
+        {
+            ref var constraintContacts = ref Constraints.Allocate(Pool);
+            constraintContacts = new ExtractedManifold(Pool, bodyHandleA, bodyHandleB);
+            ExtractNonconvexData(ref constraintContacts, ref prestep, ref impulses);
+        }
 
-            void ExtractConvexData<TPrestep, TAccumulatedImpulses>(ref ConstraintContacts constraintContacts, ref TPrestep prestep, ref TAccumulatedImpulses impulses)
-                where TPrestep : struct, IConvexContactPrestep<TPrestep>
-                where TAccumulatedImpulses : struct, IConvexContactAccumulatedImpulses<TAccumulatedImpulses>
+        public void Reset()
+        {
+            for (int i = 0; i < Constraints.Count; ++i)
             {
-                //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;
-                }
+                Constraints[i].Dispose(Pool);
             }
+            Constraints.Count = 0;
+        }
 
-            public void ConvexOneBody<TPrestep, TAccumulatedImpulses>(BodyHandle bodyHandle, ref TPrestep prestep, ref TAccumulatedImpulses impulses)
-                where TPrestep : struct, IConvexContactPrestep<TPrestep>
-                where TAccumulatedImpulses : struct, IConvexContactAccumulatedImpulses<TAccumulatedImpulses>
-            {
-                ref var constraintContacts = ref ConstraintContacts.Allocate(Pool);
-                constraintContacts = new ConstraintContacts(Pool, bodyHandle);
-                ExtractConvexData(ref constraintContacts, ref prestep, ref impulses);
-            }
+        public void Dispose()
+        {
+            Reset();
+            Constraints.Dispose(Pool);
+        }
+    }
 
-            public void ConvexTwoBody<TPrestep, TAccumulatedImpulses>(BodyHandle bodyHandleA, BodyHandle bodyHandleB, ref TPrestep prestep, ref TAccumulatedImpulses impulses)
-                where TPrestep : struct, ITwoBodyConvexContactPrestep<TPrestep>
-                where TAccumulatedImpulses : struct, IConvexContactAccumulatedImpulses<TAccumulatedImpulses>
-            {
-                ref var constraintContacts = ref ConstraintContacts.Allocate(Pool);
-                constraintContacts = new ConstraintContacts(Pool, bodyHandleA, bodyHandleB);
-                ExtractConvexData(ref constraintContacts, ref prestep, ref impulses);
-            }
+    BodyHandle sensorBodyHandle;
+    public override void Initialize(ContentArchive content, Camera camera)
+    {
+        camera.Position = new Vector3(0, 5, 25);
+        camera.Yaw = 0;
+        camera.Pitch = 0;
 
-            void ExtractNonconvexData<TPrestep, TAccumulatedImpulses>(ref ConstraintContacts constraintContacts, ref TPrestep prestep, ref TAccumulatedImpulses impulses)
-                where TPrestep : struct, INonconvexContactPrestep<TPrestep>
-                where TAccumulatedImpulses : struct, INonconvexContactAccumulatedImpulses<TAccumulatedImpulses>
-            {
-                //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);
+        Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(new SpringSettings(30, 1)), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(8, 1));
 
-                    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();
-                }
-            }
+        //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);
 
-            public void NonconvexOneBody<TPrestep, TAccumulatedImpulses>(BodyHandle bodyHandle, ref TPrestep prestep, ref TAccumulatedImpulses impulses)
-                where TPrestep : struct, INonconvexContactPrestep<TPrestep>
-                where TAccumulatedImpulses : struct, INonconvexContactAccumulatedImpulses<TAccumulatedImpulses>
+        const int rowCount = 20;
+        for (int rowIndex = 0; rowIndex < rowCount; ++rowIndex)
+        {
+            int columnCount = rowCount - rowIndex;
+            for (int columnIndex = 0; columnIndex < columnCount; ++columnIndex)
             {
-                ref var constraintContacts = ref ConstraintContacts.Allocate(Pool);
-                constraintContacts = new ConstraintContacts(Pool, bodyHandle);
-                ExtractNonconvexData(ref constraintContacts, ref prestep, ref impulses);
+                Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(
+                    (-columnCount * 0.5f + columnIndex) * boxShape.Width,
+                    (rowIndex + 0.5f) * boxShape.Height + 10, 0),
+                    boxInertia, boxIndex, 0.01f));
             }
+        }
 
-            public void NonconvexTwoBody<TPrestep, TAccumulatedImpulses>(BodyHandle bodyHandleA, BodyHandle bodyHandleB, ref TPrestep prestep, ref TAccumulatedImpulses impulses)
-                where TPrestep : struct, ITwoBodyNonconvexContactPrestep<TPrestep>
-                where TAccumulatedImpulses : struct, INonconvexContactAccumulatedImpulses<TAccumulatedImpulses>
-            {
-                ref var constraintContacts = ref ConstraintContacts.Allocate(Pool);
-                constraintContacts = new ConstraintContacts(Pool, bodyHandleA, bodyHandleB);
-                ExtractNonconvexData(ref constraintContacts, ref prestep, ref impulses);
-            }
+        sensorBodyHandle = Simulation.Bodies.Add(BodyDescription.CreateConvexDynamic(new Vector3(0, 0, 1), 10, Simulation.Shapes, new Box(4, 2, 6)));
 
-            public void Dispose()
+        //Put a mesh under the sensor so that nonconvex contacts are shown.
+        const int planeWidth = 128;
+        const int planeHeight = 128;
+        var planeMesh = DemoMeshHelper.CreateDeformedPlane(planeWidth, planeHeight,
+            (int x, int y) =>
             {
-                for (int i = 0; i < ConstraintContacts.Count; ++i)
-                {
-                    ref var constraintContacts = ref ConstraintContacts[i];
-                    constraintContacts.Contacts.Dispose(Pool);
-                }
-                ConstraintContacts.Dispose(Pool);
-            }
-        }
+                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(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.GetBodyReference(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.
-            //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.ConstraintContacts.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.ConstraintContacts[manifoldIndex];
-                var bodyA = Simulation.Bodies.GetBodyReference(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, ref 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));
             }
-            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();
-            base.Render(renderer, camera, input, text, 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.Constraints.Count).Append(", contact count: ").Append(sensorContactCount), new Vector2(16, resolution.Y - 20), 20, Vector3.One, font);
+        base.Render(renderer, camera, input, text, font);
     }
 }
diff --git a/Demos/Demos/Sponsors/SponsorCharacterAI.cs b/Demos/Demos/Sponsors/SponsorCharacterAI.cs
index fbf2cca52..94420fa58 100644
--- a/Demos/Demos/Sponsors/SponsorCharacterAI.cs
+++ b/Demos/Demos/Sponsors/SponsorCharacterAI.cs
@@ -3,64 +3,63 @@
 using BepuUtilities.Collections;
 using Demos.Demos.Characters;
 using System;
-using System.Collections.Generic;
 using System.Numerics;
-using System.Text;
 
-namespace Demos.Demos.Sponsors
+namespace Demos.Demos.Sponsors;
+
+public struct SponsorCharacterAI
 {
-    public struct SponsorCharacterAI
+    BodyHandle bodyHandle;
+    Vector2 targetLocation;
+    public SponsorCharacterAI(CharacterControllers characters, in CollidableDescription characterCollidable, Vector3 initialPosition, in Vector2 targetLocation)
     {
-        BodyHandle bodyHandle;
-        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);
-            character.CosMaximumSlope = MathF.Cos(MathF.PI * 0.48f);
-            character.JumpVelocity = 4;
-            character.MaximumVerticalForce = 10f;
-            character.MaximumHorizontalForce = 5f;
-            character.MinimumSupportDepth = -0.01f;
-            character.MinimumSupportContinuationDepth = -0.1f;
-            character.ViewDirection = new Vector3(0, 0, -1);
-            this.targetLocation = targetLocation;
-        }
+        ref var character = ref characters.AllocateCharacter(bodyHandle);
+        character.LocalUp = new Vector3(0, 1, 0);
+        character.CosMaximumSlope = MathF.Cos(MathF.PI * 0.48f);
+        character.JumpVelocity = 4;
+        character.MaximumVerticalForce = 10f;
+        character.MaximumHorizontalForce = 5f;
+        character.MinimumSupportDepth = -0.01f;
+        character.MinimumSupportContinuationDepth = -0.1f;
+        character.ViewDirection = new Vector3(0, 0, -1);
+        this.targetLocation = targetLocation;
+    }
 
-        public void Update(CharacterControllers characters, Simulation simulation, ref QuickList<SponsorNewt> newts, in Vector2 arenaMin, in Vector2 arenaMax, Random random)
+    public void Update(CharacterControllers characters, Simulation simulation, ref QuickList<SponsorNewt> newts, Random random)
+    {
+        var body = simulation.Bodies[bodyHandle];
+        Vector2 influenceSum = default;
+        bool spooked = false;
+        for (int i = 0; i < newts.Count; ++i)
         {
-            var body = simulation.Bodies.GetBodyReference(bodyHandle);
-            Vector2 influenceSum = default;
-            bool spooked = false;
-            for (int i = 0; i < newts.Count; ++i)
+            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)
             {
-                ref var newtPosition = ref simulation.Bodies.GetBodyReference(newts[i].BodyHandle).Pose.Position;
-                var offset = newtPosition - body.Pose.Position;
-                var distance = offset.Length();
-                if (distance > 1e-10f)
-                {
-                    var influenceMagnitude = 1f / (distance * 0.1f + .1f);
-                    influenceSum -= new Vector2(offset.X, offset.Z) * influenceMagnitude / distance;
-                }
-                if(distance < 20)
-                {
-                    spooked = true;
-                }
+                var influenceMagnitude = 1f / (distance * 0.1f + .1f);
+                influenceSum -= new Vector2(offset.X, offset.Z) * influenceMagnitude / distance;
             }
-            //We want target position relative influence to be consistent regardless of newt count.
-            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);
-            //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)
+            if (distance < 20)
             {
-                //oh no oh no oh no he gonna get me
-                character.TryJump = true;
+                spooked = true;
             }
         }
+        //We want target position relative influence to be consistent regardless of newt count.
+        influenceSum /= newts.Count;
+        ref var character = ref characters.GetCharacterByBodyHandle(bodyHandle);
+        influenceSum -= (new Vector2(body.Pose.Position.X, body.Pose.Position.Z) - targetLocation) * 0.001f;
+        //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)
+        {
+            //oh no oh no oh no he gonna get me
+            character.TryJump = true;
+        }
     }
 }
diff --git a/Demos/Demos/Sponsors/SponsorDemo.cs b/Demos/Demos/Sponsors/SponsorDemo.cs
index ea4021e1d..16bc80e19 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;
@@ -14,282 +13,284 @@
 using Demos.Demos.Characters;
 using Helpers = DemoRenderer.Helpers;
 
-namespace Demos.Demos.Sponsors
+namespace Demos.Demos.Sponsors;
+
+public struct Sponsor
 {
-    public struct Sponsor
-    {
-        public string Name;
-        public RenderableImage RewardImage;
-    }
+    public string Name;
+    public RenderableImage RewardImage;
+}
 
-    public class SponsorDemo : Demo
-    {
-        List<string> sponsors0 = new List<string>();
-        List<Sponsor> sponsors1 = new List<Sponsor>();
-        List<Sponsor> sponsors2 = new List<Sponsor>();
-        List<Sponsor> sponsors3 = new List<Sponsor>();
+public class SponsorDemo : Demo
+{
+    List<string> sponsors0 = new List<string>();
+    List<Sponsor> sponsors1 = new List<Sponsor>();
+    List<Sponsor> sponsors2 = new List<Sponsor>();
+    List<Sponsor> sponsors3 = new List<Sponsor>();
 
-        QuickList<SponsorNewt> newts;
-        StaticHandle overlordNewtHandle;
+    QuickList<SponsorNewt> newts;
+    StaticHandle overlordNewtHandle;
 
-        RenderableImage CreateRewardImage(string rewardImagePath, ContentArchive content, RenderSurface surface)
-        {
-            var textureContent = content.Load<Texture2DContent>(rewardImagePath);
-            return new RenderableImage(
+    RenderableImage CreateRewardImage(string rewardImagePath, ContentArchive content, RenderSurface surface)
+    {
+        var textureContent = content.Load<Texture2DContent>(rewardImagePath);
+        return new RenderableImage(
 #if !OPENGL
-                surface.Device, surface.Context,
+            surface.Device, surface.Context,
 #endif
-                textureContent, debugName: Path.GetFileNameWithoutExtension(rewardImagePath)
-            );
-        }
-        void Add(List<Sponsor> sponsors, string name, string rewardImagePath, ContentArchive content, RenderSurface surface)
-        {
-            Sponsor sponsor;
-            sponsor.RewardImage = CreateRewardImage(rewardImagePath, content, surface);
-            sponsor.Name = name;
-            sponsors.Add(sponsor);
-
-        }
-        public override void LoadGraphicalContent(ContentArchive content, RenderSurface surface)
-        {
-            //I tried to reach out to all historical backers, but given the timescales involved, it's understandable that not everyone responded.
-            //I don't want to publish their full names without permission, but I also want to give them some credit, so the initials will have to do for now.
-            //If you're a supporter and don't see yourself on here, get in touch and I'll add you.
+            textureContent, debugName: Path.GetFileNameWithoutExtension(rewardImagePath)
+        );
+    }
+    void Add(List<Sponsor> sponsors, string name, string rewardImagePath, ContentArchive content, RenderSurface surface)
+    {
+        Sponsor sponsor;
+        sponsor.RewardImage = CreateRewardImage(rewardImagePath, content, surface);
+        sponsor.Name = name;
+        sponsors.Add(sponsor);
 
-            //These supporters are those who gave 5 dollars a month (or historical backers of roughly equivalent or greater total contribution).
-            sponsors0.Add("J. G.");
-            sponsors0.Add("J. Y.");
-            sponsors0.Add("J. F.");
-            sponsors0.Add("C. S.");
-            sponsors0.Add("R. W.");
-            sponsors0.Add("AshleighAdams");
+    }
+    public override void LoadGraphicalContent(ContentArchive content, RenderSurface surface)
+    {
+        //I tried to reach out to all historical backers, but given the timescales involved, it's understandable that not everyone responded.
+        //I don't want to publish their full names without permission, but I also want to give them some credit, so the initials will have to do for now.
+        //If you're a supporter and don't see yourself on here, get in touch and I'll add you.
 
-            //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!
-            Add(sponsors1, "Y. V.", @"Content\Sponsors\smugturt.png", content, surface);
-            Add(sponsors1, "R. S. K.", @"Content\Sponsors\squidhanginout.png", content, surface);
-            Add(sponsors1, "Gavin-Williams", @"Content\Sponsors\angerybite.png", content, surface);
-            Add(sponsors1, "N.", @"Content\Sponsors\normalgiraffe.png", content, surface);
-            Add(sponsors1, "M. H.", @"Content\Sponsors\tarbeenus.png", content, surface);
-            Add(sponsors1, "K. C.", @"Content\Sponsors\physquirrel.png", content, surface);
-            Add(sponsors1, "P. S.", @"Content\Sponsors\borb.png", content, surface);
-            Add(sponsors1, "A different P. S.", @"Content\Sponsors\pleasedfrog.png", content, surface);
-            Add(sponsors1, "E. C.", @"Content\Sponsors\disturbingkoala.png", content, surface);
-            Add(sponsors1, "L. K. L.", @"Content\Sponsors\bork.png", content, surface);
-            Add(sponsors1, "A. L.", @"Content\Sponsors\waryoctopus.png", content, surface);
-            Add(sponsors1, "D. P.", @"Content\Sponsors\contentsluginthevoid.png", content, surface);
-            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);
+        //These supporters are those who gave 5 dollars a month (or historical backers of roughly equivalent or greater total contribution).
+        sponsors0.Add("J. G.");
+        sponsors0.Add("J. Y.");
+        sponsors0.Add("J. F.");
+        sponsors0.Add("C. S.");
+        sponsors0.Add("R. W.");
+        sponsors0.Add("AshleighAdams");
+        sponsors0.Add("LoicBaumann");
+        sponsors0.Add("Slayuh9");
+        sponsors0.Add("rejurime");
 
-            //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.
-            //You may notice that this tier does not have much in the way of restrictions on the "name"/text.
-            Add(sponsors2, @"M. T.", @"Content\Sponsors\goose.png", content, surface);
-            Add(sponsors2, @"Yo Mamma wants you to go to bed", @"Content\Sponsors\spide.png", content, surface);
-            Add(sponsors2, @"A. R.", @"Content\Sponsors\scootybun.png", content, surface);
-            Add(sponsors2, @"Alex 'mcmonkey' Goodwin, @mcmonkey4eva on github and twitter", @"Content\Sponsors\mcmonkey.png", content, surface);
-            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);
+        //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!
+        Add(sponsors1, "Y. V.", @"Content\Sponsors\smugturt.png", content, surface);
+        Add(sponsors1, "R. S. K.", @"Content\Sponsors\squidhanginout.png", content, surface);
+        Add(sponsors1, "Gavin-Williams", @"Content\Sponsors\angerybite.png", content, surface);
+        Add(sponsors1, "N.", @"Content\Sponsors\normalgiraffe.png", content, surface);
+        Add(sponsors1, "M. H.", @"Content\Sponsors\tarbeenus.png", content, surface);
+        Add(sponsors1, "K. C.", @"Content\Sponsors\physquirrel.png", content, surface);
+        Add(sponsors1, "P. S.", @"Content\Sponsors\borb.png", content, surface);
+        Add(sponsors1, "A different P. S.", @"Content\Sponsors\pleasedfrog.png", content, surface);
+        Add(sponsors1, "E. C.", @"Content\Sponsors\disturbingkoala.png", content, surface);
+        Add(sponsors1, "L. K. L.", @"Content\Sponsors\bork.png", content, surface);
+        Add(sponsors1, "A. L.", @"Content\Sponsors\waryoctopus.png", content, surface);
+        Add(sponsors1, "D. P.", @"Content\Sponsors\contentsluginthevoid.png", content, surface);
+        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);
+        Add(sponsors1, "Claytonious", @"Content\Sponsors\beaverboss.png", content, surface);
+        Add(sponsors1, "Damian", @"Content\Sponsors\marmottourism.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);
+        //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.
+        //You may notice that this tier does not have much in the way of restrictions on the "name"/text.
+        Add(sponsors2, @"M. T.", @"Content\Sponsors\goose.png", content, surface);
+        Add(sponsors2, @"Yo Mamma wants you to go to bed", @"Content\Sponsors\spide.png", content, surface);
+        Add(sponsors2, @"A. R.", @"Content\Sponsors\scootybun.png", content, surface);
+        Add(sponsors2, @"Alex 'mcmonkey' Goodwin, @mcmonkey4eva on github and twitter", @"Content\Sponsors\mcmonkey.png", content, surface);
+        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(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);
 
-        Vector2 newtArenaMin, newtArenaMax;
-        Random random;
-        CharacterControllers characterControllers;
-        QuickList<SponsorCharacterAI> 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);
-            //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());
+    Vector2 newtArenaMin, newtArenaMax;
+    Random random;
+    CharacterControllers characterControllers;
+    QuickList<SponsorCharacterAI> 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;
 
-            DemoMeshHelper.LoadModel(content, BufferPool, @"Content\newt.obj", new Vector3(-10, 10, -10), out var newtMesh);
-            var newtShape = Simulation.Shapes.Add(newtMesh);
-            newts = new QuickList<SponsorNewt>(sponsors2.Count, BufferPool);
-            newtArenaMin = new Vector2(-100);
-            newtArenaMax = new Vector2(100);
-            random = new Random(6);
-            for (int i = 0; i < sponsors2.Count; ++i)
-            {
-                ref var newt = ref newts.AllocateUnsafely();
-                newt = new SponsorNewt(Simulation, newtShape, 0, newtArenaMin, newtArenaMax, random, i);
-            }
+        characterControllers = new CharacterControllers(BufferPool);
+        Simulation = Simulation.Create(BufferPool, new CharacterNarrowphaseCallbacks(characterControllers), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(8, 1));
 
-            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)));
+        var newtMesh = DemoMeshHelper.LoadModel(content, BufferPool, @"Content\newt.obj", new Vector3(-10, 10, -10));
+        var newtShape = Simulation.Shapes.Add(newtMesh);
+        newts = new QuickList<SponsorNewt>(sponsors2.Count, BufferPool);
+        newtArenaMin = new Vector2(-100);
+        newtArenaMax = new Vector2(100);
+        random = new Random(6);
+        for (int i = 0; i < sponsors2.Count; ++i)
+        {
+            ref var newt = ref newts.AllocateUnsafely();
+            newt = new SponsorNewt(Simulation, newtShape, 0, newtArenaMin, newtArenaMax, random, i);
+        }
 
-            const int characterCount = 1000;
-            characterAIs = new QuickList<SponsorCharacterAI>(characterCount, BufferPool);
-            var characterCollidable = new CollidableDescription(Simulation.Shapes.Add(new Capsule(0.5f, 1f)), 0.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()));
-                characterAIs.AllocateUnsafely() = new SponsorCharacterAI(characterControllers, characterCollidable, new Vector3(position2D.X, 5, position2D.Y), targetPosition);
-            }
+        const float floorSize = 240;
+        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 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));
+        const int characterCount = 1000;
+        characterAIs = new QuickList<SponsorCharacterAI>(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);
+        }
 
-            for (int i = 0; i < hutCount; ++i)
-            {
-                var position2D = newtArenaMin + (newtArenaMax - newtArenaMin) * new Vector2((float)random.NextDouble(), (float)random.NextDouble());
-                ColosseumDemo.CreateRing(Simulation, new Vector3(position2D.X, 0, position2D.Y), hutBoxShape, obstacleDescription, 5, 2, random.Next(1, 5));
+        const int hutCount = 30;
+        var hutBoxShape = new Box(0.4f, 2, 3);
+        var obstacleDescription = BodyDescription.CreateDynamic(new Vector3(), hutBoxShape.ComputeInertia(20), Simulation.Shapes.Add(hutBoxShape), 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, 5, 2, random.Next(1, 5));
 
-            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)));
         }
 
+        var overlordNewtShape = newtMesh;
+        overlordNewtShape.Scale = new Vector3(60, 60, 60);
+        overlordNewtHandle = Simulation.Statics.Add(new StaticDescription(new Vector3(0, 10, -floorSize * 0.5f - 70), Simulation.Shapes.Add(overlordNewtShape)));
+    }
 
-        static void Get(int sponsorCount, int maximumBatchSize, double time, float timePerBatch, float fadeTime, out float alpha, out int start, out int end)
+
+    static void Get(int sponsorCount, int maximumBatchSize, double time, float timePerBatch, float fadeTime, out float alpha, out int start, out int end)
+    {
+        var batchCount = (int)MathF.Ceiling((float)sponsorCount / maximumBatchSize);
+        if (batchCount > 1)
         {
-            var batchCount = (int)MathF.Ceiling((float)sponsorCount / maximumBatchSize);
-            if (batchCount > 1)
-            {
-                var batchIndex = (time / timePerBatch) % batchCount;
-                //Fade time covers the time from full alpha to 0 and back to full alpha again. In other words, the full time for a given batch is:
-                //[0, 0.5 * fadeTime) [0.5 * fadeTime, timePerBatch - 0.5 * fadeT6ime ) [timePerBatch - 0.5 * fadeTime, timePerBatch)
-                Debug.Assert(fadeTime <= timePerBatch);
-                var batchProgress = (float)(timePerBatch * (batchIndex - MathF.Truncate((float)batchIndex)));
-                var inverseFadeTime = 1f / fadeTime;
-                alpha = MathF.Min(MathF.Min(1f, batchProgress * inverseFadeTime), inverseFadeTime * timePerBatch - batchProgress * inverseFadeTime);
-                start = (int)batchIndex * maximumBatchSize;
-                end = Math.Min(start + maximumBatchSize, sponsorCount);
-            }
-            else
-            {
-                alpha = 1;
-                start = 0;
-                end = sponsorCount;
-            }
+            var batchIndex = (time / timePerBatch) % batchCount;
+            //Fade time covers the time from full alpha to 0 and back to full alpha again. In other words, the full time for a given batch is:
+            //[0, 0.5 * fadeTime) [0.5 * fadeTime, timePerBatch - 0.5 * fadeT6ime ) [timePerBatch - 0.5 * fadeTime, timePerBatch)
+            Debug.Assert(fadeTime <= timePerBatch);
+            var batchProgress = (float)(timePerBatch * (batchIndex - MathF.Truncate((float)batchIndex)));
+            var inverseFadeTime = 1f / fadeTime;
+            alpha = MathF.Min(MathF.Min(1f, batchProgress * inverseFadeTime), inverseFadeTime * timePerBatch - batchProgress * inverseFadeTime);
+            start = (int)batchIndex * maximumBatchSize;
+            end = Math.Min(start + maximumBatchSize, sponsorCount);
         }
+        else
+        {
+            alpha = 1;
+            start = 0;
+            end = sponsorCount;
+        }
+    }
 
-        static float DrawSponsors(string groupTitle, List<Sponsor> sponsors, Vector2 position, Vector2 mousePosition, Renderer renderer, TextBuilder text, Font font,
-            double time, int maximumBatchSize, float timePerBatch, float fadeTime, float fontSize, float padding, float lineSpacing)
+    static float DrawSponsors(string groupTitle, List<Sponsor> sponsors, Vector2 position, Vector2 mousePosition, Renderer renderer, TextBuilder text, Font font,
+        double time, int maximumBatchSize, float timePerBatch, float fadeTime, float fontSize, float padding, float lineSpacing)
+    {
+        var initialY = position.Y;
+        renderer.TextBatcher.Write(text.Clear().Append(groupTitle), position, fontSize * 1.5f, new Vector4(1), font);
+        position += new Vector2(fontSize * 0.5f, lineSpacing);
+        Get(sponsors.Count, maximumBatchSize, time, timePerBatch, fadeTime, out var alpha, out var start, out var end);
+        for (int i = start; i < end; ++i)
         {
-            var initialY = position.Y;
-            renderer.TextBatcher.Write(text.Clear().Append(groupTitle), position, fontSize * 1.5f, new Vector4(1), font);
-            position += new Vector2(fontSize * 0.5f, lineSpacing);
-            Get(sponsors.Count, maximumBatchSize, time, timePerBatch, fadeTime, out var alpha, out var start, out var end);
-            for (int i = start; i < end; ++i)
+            var sponsor = sponsors[i];
+            renderer.TextBatcher.Write(text.Clear().Append(sponsor.Name), position, fontSize, new Vector4(new Vector3(1), alpha), font);
+            var width = GlyphBatch.MeasureLength(sponsor.Name, font, fontSize);
+            var boundingBoxMin = position - new Vector2(padding, padding + fontSize);
+            var boundingBoxMax = position + new Vector2(width + padding, padding);
+            if (Vector2.Min(boundingBoxMax, Vector2.Max(boundingBoxMin, mousePosition)) == mousePosition)
             {
-                var sponsor = sponsors[i];
-                renderer.TextBatcher.Write(text.Clear().Append(sponsor.Name), position, fontSize, new Vector4(new Vector3(1), alpha), font);
-                var width = GlyphBatch.MeasureLength(sponsor.Name, font, fontSize);
-                var boundingBoxMin = position - new Vector2(padding, padding + fontSize);
-                var boundingBoxMax = position + new Vector2(width + padding, padding);
-                if (Vector2.Min(boundingBoxMax, Vector2.Max(boundingBoxMin, mousePosition)) == mousePosition)
-                {
-                    //Mouse is within the mouseover region. Show reward image.
-                    //(I wondered what to do with the sponsor images for a while. It seems odd to display them always, since it puts the focus on weird animal pictures 
-                    //as much as on the sponsor themselves. At the same time, gotta advertise the sweet weird animal pictures you could get by being a sponsor.
-                    //Mouseover seems like a decent compromise.)
-                    renderer.ImageBatcher.Draw(sponsor.RewardImage, Vector2.Max(Vector2.Zero, position + new Vector2(width + 8f * padding, fontSize * -0.4f - 2.5f * lineSpacing)), 5 * lineSpacing, new Vector4(new Vector3(1), alpha));
-                }
-                position.Y += lineSpacing;
+                //Mouse is within the mouseover region. Show reward image.
+                //(I wondered what to do with the sponsor images for a while. It seems odd to display them always, since it puts the focus on weird animal pictures 
+                //as much as on the sponsor themselves. At the same time, gotta advertise the sweet weird animal pictures you could get by being a sponsor.
+                //Mouseover seems like a decent compromise.)
+                renderer.ImageBatcher.Draw(sponsor.RewardImage, Vector2.Max(Vector2.Zero, position + new Vector2(width + 8f * padding, fontSize * -0.4f - 2.5f * lineSpacing)), 5 * lineSpacing, new Vector4(new Vector3(1), alpha));
             }
-            return position.Y - initialY;
+            position.Y += lineSpacing;
         }
+        return position.Y - initialY;
+    }
 
-        float DrawSponsors(string groupTitle, List<string> sponsors, Vector2 position, Renderer renderer, TextBuilder text, Font font,
-            double time, int maximumBatchSize, float timePerBatch, float fadeTime, float fontSize, float lineSpacing)
+    float DrawSponsors(string groupTitle, List<string> sponsors, Vector2 position, Renderer renderer, TextBuilder text, Font font,
+        double time, int maximumBatchSize, float timePerBatch, float fadeTime, float fontSize, float lineSpacing)
+    {
+        var initialY = position.Y;
+        renderer.TextBatcher.Write(text.Clear().Append(groupTitle), position, fontSize * 1.5f, new Vector4(1), font);
+        position += new Vector2(fontSize * 0.5f, lineSpacing);
+        Get(sponsors.Count, maximumBatchSize, time, timePerBatch, fadeTime, out var alpha, out var start, out var end);
+        for (int i = start; i < end; ++i)
         {
-            var initialY = position.Y;
-            renderer.TextBatcher.Write(text.Clear().Append(groupTitle), position, fontSize * 1.5f, new Vector4(1), font);
-            position += new Vector2(fontSize * 0.5f, lineSpacing);
-            Get(sponsors.Count, maximumBatchSize, time, timePerBatch, fadeTime, out var alpha, out var start, out var end);
-            for (int i = start; i < end; ++i)
-            {
-                var sponsor = sponsors[i];
-                renderer.TextBatcher.Write(text.Clear().Append(sponsor), position, fontSize, new Vector4(new Vector3(1), alpha), font);
-                position.Y += lineSpacing;
-            }
-            return position.Y - initialY;
+            var sponsor = sponsors[i];
+            renderer.TextBatcher.Write(text.Clear().Append(sponsor), position, fontSize, new Vector4(new Vector3(1), alpha), font);
+            position.Y += lineSpacing;
         }
+        return position.Y - initialY;
+    }
 
-        double realTime;
-        double simulationTime;
-        public override void Update(Window window, Camera camera, Input input, float dt)
+    double realTime;
+    double simulationTime;
+    public override void Update(Window window, Camera camera, Input input, float dt)
+    {
+        Simulation.Timestep(TimestepDuration, ThreadDispatcher);
+        for (int i = 0; i < newts.Count; ++i)
         {
-            const float simulationDt = 1 / 60f;
-            Simulation.Timestep(simulationDt, ThreadDispatcher);
-            for (int i = 0; i < newts.Count; ++i)
-            {
-                newts[i].Update(Simulation, simulationTime, 0, newtArenaMin, newtArenaMax, random, 1f / simulationDt);
-            }
-            for (int i = 0; i < characterAIs.Count; ++i)
-            {
-                characterAIs[i].Update(characterControllers, Simulation, ref newts, newtArenaMin, newtArenaMax, random);
-            }
-            simulationTime += simulationDt;
-            realTime += dt;
+            newts[i].Update(Simulation, simulationTime, 0, newtArenaMin, newtArenaMax, random, 1f / TimestepDuration);
         }
-        public override void Render(Renderer renderer, Camera camera, Input input, TextBuilder text, Font font)
+        for (int i = 0; i < characterAIs.Count; ++i)
         {
-            var viewProjection = camera.ViewProjection;
-            var integerResolution = renderer.Surface.Resolution;
-            var resolution = new Vector2(integerResolution.X, integerResolution.Y);
-            for (int i = 0; i < newts.Count; ++i)
-            {
-                newts[i].Render(Simulation, sponsors2, renderer, viewProjection, resolution, text, font);
-            }
+            characterAIs[i].Update(characterControllers, Simulation, ref newts, random);
+        }
+        simulationTime += TimestepDuration;
+        realTime += dt;
+    }
+    public override void Render(Renderer renderer, Camera camera, Input input, TextBuilder text, Font font)
+    {
+        var viewProjection = camera.ViewProjection;
+        var integerResolution = renderer.Surface.Resolution;
+        var resolution = new Vector2(integerResolution.X, integerResolution.Y);
+        for (int i = 0; i < newts.Count; ++i)
+        {
+            newts[i].Render(Simulation, sponsors2, renderer, viewProjection, resolution, text, font);
+        }
 
-            //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);
-                Helpers.GetScreenLocation(worldTextPosition, viewProjection, resolution, out var screenspacePosition);
-                const float nameHeight = 14;
-                var name = sponsors3[0].Name;
-                var nameLength = GlyphBatch.MeasureLength(name, font, nameHeight);
-                screenspacePosition.X -= nameLength * 0.5f;
-                renderer.TextBatcher.Write(text.Clear().Append(name), screenspacePosition, nameHeight, new Vector3(0.3f, 0f, 0f), font);
-            }
+        //We'll hardcode the overlord newts. Not going to be a problem, I suspect.
+        {
+            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;
+            var nameLength = GlyphBatch.MeasureLength(name, font, nameHeight);
+            screenspacePosition.X -= nameLength * 0.5f;
+            renderer.TextBatcher.Write(text.Clear().Append(name), screenspacePosition, nameHeight, new Vector3(0.3f, 0f, 0f), font);
+        }
 
-            var integralMousePosition = input.MousePosition;
-            var mousePosition = new Vector2(integralMousePosition.X, integralMousePosition.Y);
-            renderer.TextBatcher.Write(text.Clear().Append("Mouseover entries to view additional very important tier rewards."), new Vector2(32, resolution.Y - 50), 14, new Vector3(1), font);
-            renderer.TextBatcher.Write(text.Clear().Append("Are you a sponsor, but missing from this list? Want a different name/nickname for your entry? Send me a message!"), new Vector2(32, resolution.Y - 32), 14, new Vector3(1), font);
-            DrawSponsors("Super duper sponsors", sponsors3, new Vector2(32, resolution.Y - 480), mousePosition, renderer, text, font, realTime, 1, 4, 0.25f, 32, 6, 48);
-            DrawSponsors("Very neat sponsors", sponsors2, new Vector2(32, resolution.Y - 288), mousePosition, renderer, text, font, realTime, 4, 4, 0.25f, 24, 4, 36);
-            DrawSponsors("Sponsors", sponsors1, new Vector2(renderer.Surface.Resolution.X - 512, resolution.Y - 480), mousePosition, renderer, text, font, realTime, 4, 4, 0.25f, 24, 4, 36);
-            DrawSponsors("Smaller sponsors who are still cool", sponsors0, new Vector2(renderer.Surface.Resolution.X - 512, resolution.Y - 288), renderer, text, font, realTime, 4, 4, 0.25f, 16, 24);
+        var integralMousePosition = input.MousePosition;
+        var mousePosition = new Vector2(integralMousePosition.X, integralMousePosition.Y);
+        renderer.TextBatcher.Write(text.Clear().Append("Mouseover entries to view additional very important tier rewards."), new Vector2(32, resolution.Y - 50), 14, new Vector3(1), font);
+        renderer.TextBatcher.Write(text.Clear().Append("Are you a sponsor, but missing from this list? Want a different name/nickname for your entry? Send me a message!"), new Vector2(32, resolution.Y - 32), 14, new Vector3(1), font);
+        DrawSponsors("Super duper sponsors", sponsors3, new Vector2(32, resolution.Y - 480), mousePosition, renderer, text, font, realTime, 1, 4, 0.25f, 32, 6, 48);
+        DrawSponsors("Very neat sponsors", sponsors2, new Vector2(32, resolution.Y - 288), mousePosition, renderer, text, font, realTime, 4, 4, 0.25f, 24, 4, 36);
+        DrawSponsors("Sponsors", sponsors1, new Vector2(renderer.Surface.Resolution.X - 512, resolution.Y - 480), mousePosition, renderer, text, font, realTime, 4, 4, 0.25f, 24, 4, 36);
+        DrawSponsors("Smaller sponsors who are still cool", sponsors0, new Vector2(renderer.Surface.Resolution.X - 512, resolution.Y - 288), renderer, text, font, realTime, 4, 4, 0.25f, 16, 24);
 
-        }
+    }
 
-        protected override void OnDispose()
+    protected override void OnDispose()
+    {
+        foreach (var sponsor in sponsors1)
         {
-            foreach (var sponsor in sponsors1)
-            {
-                sponsor.RewardImage.Dispose();
-            }
-            foreach (var sponsor in sponsors2)
-            {
-                sponsor.RewardImage.Dispose();
-            }
-            foreach (var sponsor in sponsors3)
-            {
-                sponsor.RewardImage.Dispose();
-            }
+            sponsor.RewardImage.Dispose();
+        }
+        foreach (var sponsor in sponsors2)
+        {
+            sponsor.RewardImage.Dispose();
+        }
+        foreach (var sponsor in sponsors3)
+        {
+            sponsor.RewardImage.Dispose();
         }
     }
 }
diff --git a/Demos/Demos/Sponsors/SponsorNewt.cs b/Demos/Demos/Sponsors/SponsorNewt.cs
index 7dfcba36b..68c866f24 100644
--- a/Demos/Demos/Sponsors/SponsorNewt.cs
+++ b/Demos/Demos/Sponsors/SponsorNewt.cs
@@ -9,99 +9,98 @@
 using System.Numerics;
 using Helpers = DemoRenderer.Helpers;
 
-namespace Demos.Demos.Sponsors
+namespace Demos.Demos.Sponsors;
+
+public struct SponsorNewt
 {
-    public struct SponsorNewt
+    public int SponsorIndex;
+    public BodyHandle BodyHandle;
+    public SponsorNewt(Simulation simulation, TypedIndex shape, float height, in Vector2 arenaMin, in Vector2 arenaMax, Random random, int sponsorIndex) : this()
     {
-        public int SponsorIndex;
-        public BodyHandle BodyHandle;
-        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();
-            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)));
-            SponsorIndex = sponsorIndex;
-        }
+        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 Vector3(position.X, height, position.Y), QuaternionEx.CreateFromAxisAngle(Vector3.UnitY, angle)), shape, -1));
+        SponsorIndex = sponsorIndex;
+    }
 
-        //The newt hops between predetermined points unstoppably, waiting a moment in between jumps.
-        double nextAllowedJump;
-        Vector2 jumpStart, jumpEnd;
-        //The orientation will interpolate while jumping.
-        Vector2 forwardAtJumpStart, forwardAtJumpEnd;
-        double jumpStartTime, jumpEndTime;
+    //The newt hops between predetermined points unstoppably, waiting a moment in between jumps.
+    double nextAllowedJump;
+    Vector2 jumpStart, jumpEnd;
+    //The orientation will interpolate while jumping.
+    Vector2 forwardAtJumpStart, forwardAtJumpEnd;
+    double jumpStartTime, jumpEndTime;
 
-        public void Update(Simulation simulation, double time, float height, in Vector2 arenaMin, in Vector2 arenaMax, Random random, float inverseDt)
+    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[BodyHandle];
+        if (time >= nextAllowedJump)
+        {
+            //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 + 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;
+            jumpEndTime = time + jumpDuration;
+            forwardAtJumpStart = -new Vector2(backward.X, backward.Z);
+            forwardAtJumpEnd = jumpEnd - jumpStart;
+            var newForwardLengthSquared = forwardAtJumpEnd.LengthSquared();
+            forwardAtJumpEnd = newForwardLengthSquared < 1e-10f ? forwardAtJumpStart : forwardAtJumpEnd / MathF.Sqrt(newForwardLengthSquared);
+            nextAllowedJump = jumpEndTime + (1 + random.NextDouble() * 2.5f);
+        }
+        Vector3 targetPosition;
+        Vector2 targetForward;
+        if (time >= jumpStartTime && time <= jumpEndTime)
         {
-            const float jumpDuration = 1;
-            var body = simulation.Bodies.GetBodyReference(BodyHandle);
-            if (time >= nextAllowedJump)
+            //The newt's in the middle of a jump. Choose a target position/orientation by interpolation.
+            const float maximumJumpHeight = 5;
+            var jumpProgress = (float)(time - jumpStartTime) / jumpDuration;
+            var targetPosition2D = (float)jumpProgress * (jumpEnd - jumpStart) + jumpStart;
+            var parabolaTerm = (2 * jumpProgress - 1);
+            var currentHeight = height + (1 - parabolaTerm * parabolaTerm) * maximumJumpHeight;
+            targetPosition = new Vector3(targetPosition2D.X, currentHeight, targetPosition2D.Y);
+            targetForward = jumpProgress * (forwardAtJumpEnd - forwardAtJumpStart) + forwardAtJumpStart;
+            var targetForwardLengthSquared = targetForward.LengthSquared();
+            if (targetForwardLengthSquared < 1e-10f)
             {
-                //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 * 0.05f;
-                jumpEnd = Vector2.Max(arenaMin, Vector2.Min(arenaMax, jumpEnd));
-                jumpStartTime = time;
-                jumpEndTime = time + jumpDuration;
-                forwardAtJumpStart = -new Vector2(backward.X, backward.Z);
-                forwardAtJumpEnd = jumpEnd - jumpStart;
-                var newForwardLengthSquared = forwardAtJumpEnd.LengthSquared();
-                forwardAtJumpEnd = newForwardLengthSquared < 1e-10f ? forwardAtJumpStart : forwardAtJumpEnd / MathF.Sqrt(newForwardLengthSquared);
-                nextAllowedJump = jumpEndTime + (1 + random.NextDouble() * 2.5f);
-            }
-            Vector3 targetPosition;
-            Vector2 targetForward;
-            if (time >= jumpStartTime && time <= jumpEndTime)
-            {
-                //The newt's in the middle of a jump. Choose a target position/orientation by interpolation.
-                const float maximumJumpHeight = 5;
-                var jumpProgress = (float)(time - jumpStartTime) / jumpDuration;
-                var targetPosition2D = (float)jumpProgress * (jumpEnd - jumpStart) + jumpStart;
-                var parabolaTerm = (2 * jumpProgress - 1);
-                var currentHeight = height + (1 - parabolaTerm * parabolaTerm) * maximumJumpHeight;
-                targetPosition = new Vector3(targetPosition2D.X, currentHeight, targetPosition2D.Y);
-                targetForward = jumpProgress * (forwardAtJumpEnd - forwardAtJumpStart) + forwardAtJumpStart;
-                var targetForwardLengthSquared = targetForward.LengthSquared();
-                if (targetForwardLengthSquared < 1e-10f)
-                {
-                    QuaternionEx.TransformUnitZ(body.Pose.Orientation, out var backward);
-                    targetForward = -new Vector2(backward.X, backward.Z);
-                }
-                else
-                {
-                    targetForward /= MathF.Sqrt(targetForwardLengthSquared);
-                }
+                targetForward = -new Vector2(backward.X, backward.Z);
             }
             else
             {
-                //The target pose is just wherever the previous jump ended.
-                targetPosition = new Vector3(jumpEnd.X, height, jumpEnd.Y);
-                targetForward = forwardAtJumpEnd;
+                targetForward /= MathF.Sqrt(targetForwardLengthSquared);
             }
-
-            //Since it's a kinematic body, we'll compute the current pose error, and then the velocity to correct that error within a single frame.
-            body.Velocity.Linear = (targetPosition - body.Pose.Position) * inverseDt;
-            Matrix3x3 targetOrientationBasis;
-            targetOrientationBasis.X = new Vector3(-targetForward.Y, 0, targetForward.X);
-            targetOrientationBasis.Y = Vector3.UnitY;
-            targetOrientationBasis.Z = -new Vector3(targetForward.X, 0, targetForward.Y);
-            QuaternionEx.CreateFromRotationMatrix(targetOrientationBasis, out var targetOrientation);
-            QuaternionEx.GetRelativeRotationWithoutOverlap(body.Pose.Orientation, targetOrientation, out var orientationError);
-            QuaternionEx.GetAxisAngleFromQuaternion(orientationError, out var errorAxis, out var errorAngle);
-            body.Velocity.Angular = errorAxis * (errorAngle * inverseDt);
         }
-
-        public readonly void Render(Simulation simulation, List<Sponsor> sponsors, Renderer renderer, in Matrix viewProjection, in Vector2 resolution, TextBuilder text, Font font)
+        else
         {
-            var name = sponsors[SponsorIndex].Name;
-            var body = simulation.Bodies.GetBodyReference(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);
-            screenspacePosition.X -= nameLength * 0.5f;
-            renderer.TextBatcher.Write(text.Clear().Append(name), screenspacePosition, nameHeight, new Vector3(0.3f, 0f, 0f), font);
+            //The target pose is just wherever the previous jump ended.
+            targetPosition = new Vector3(jumpEnd.X, height, jumpEnd.Y);
+            targetForward = forwardAtJumpEnd;
         }
+
+        //Since it's a kinematic body, we'll compute the current pose error, and then the velocity to correct that error within a single frame.
+        body.Velocity.Linear = (targetPosition - body.Pose.Position) * inverseDt;
+        Matrix3x3 targetOrientationBasis;
+        targetOrientationBasis.X = new Vector3(-targetForward.Y, 0, targetForward.X);
+        targetOrientationBasis.Y = Vector3.UnitY;
+        targetOrientationBasis.Z = -new Vector3(targetForward.X, 0, targetForward.Y);
+        QuaternionEx.CreateFromRotationMatrix(targetOrientationBasis, out var targetOrientation);
+        QuaternionEx.GetRelativeRotationWithoutOverlap(body.Pose.Orientation, targetOrientation, out var orientationError);
+        QuaternionEx.GetAxisAngleFromQuaternion(orientationError, out var errorAxis, out var errorAngle);
+        body.Velocity.Angular = errorAxis * (errorAngle * inverseDt);
+    }
+
+    public readonly void Render(Simulation simulation, List<Sponsor> sponsors, Renderer renderer, in Matrix viewProjection, in Vector2 resolution, TextBuilder text, Font font)
+    {
+        var name = sponsors[SponsorIndex].Name;
+        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);
+        screenspacePosition.X -= nameLength * 0.5f;
+        renderer.TextBatcher.Write(text.Clear().Append(name), screenspacePosition, nameHeight, new Vector3(0.3f, 0f, 0f), font);
     }
 }
diff --git a/Demos/Demos/SubsteppingDemo.cs b/Demos/Demos/SubsteppingDemo.cs
index 54bbe44d3..198424f99 100644
--- a/Demos/Demos/SubsteppingDemo.cs
+++ b/Demos/Demos/SubsteppingDemo.cs
@@ -2,144 +2,170 @@
 using BepuPhysics.Collidables;
 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;
-namespace Demos.Demos
+namespace Demos.Demos;
+
+/// <summary>
+/// Shows how to use substepping to solve extremely difficult simulations at a not-completely-absurd cost.
+/// </summary>
+public class SubsteppingDemo : Demo
 {
-    /// <summary>
-    /// Shows how to use substepping to solve extremely difficult simulations at a not-completely-absurd cost.
-    /// </summary>
-    public class SubsteppingDemo : Demo
+    RolloverInfo rolloverInfo;
+
+    public override void Initialize(ContentArchive content, Camera camera)
     {
-        RolloverInfo rolloverInfo;
+        camera.Position = new Vector3(0, 25, 80);
+        camera.Yaw = 0;
+        camera.Pitch = 0;
+        Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(new SpringSettings(640, 480), float.MaxValue, 1), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(2, 48));
 
-        SubsteppingTimestepper timestepper;
-        public unsafe override void Initialize(ContentArchive content, Camera camera)
+        rolloverInfo = new RolloverInfo();
         {
-            camera.Position = new Vector3(0, 25, 80);
-            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);
+            //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(480, 480);
+            var bodyHandles = RopeStabilityDemo.BuildRope(Simulation, startLocation, 12, bodyRadius, bodySpacing, 0, 1, 0, springSettings);
+
+            var bigWreckingBall = new Sphere(5);
+            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), $"{mass}:1 mass ratio");
+        }
 
-            rolloverInfo = new RolloverInfo();
+        {
+            //Stack with a heavy block on top. Note that the contact springiness we chose in the DemoNarrowPhaseCallbacks above is important to making the stack resist the weight of the top block.
+            //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);
+            var boxInertia = boxShape.ComputeInertia(1);
+            var boxDescription = BodyDescription.CreateDynamic(new Vector3(), boxInertia, Simulation.Shapes.Add(boxShape), 0.01f);
+            for (int i = 0; i < 20; ++i)
             {
-                //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!
-                var startLocation = new Vector3(15, 40, 0);
-                const float bodySpacing = 0.3f;
-                const float bodyRadius = 0.5f;
-                var springSettings = new SpringSettings(240, 480);
-                var bodyHandles = RopeStabilityDemo.BuildRope(Simulation, startLocation, 12, bodyRadius, bodySpacing, 0, 1, 0, springSettings);
-
-                var bigWreckingBall = new Sphere(5);
-                bigWreckingBall.ComputeInertia(1000, out var bigWreckingBallInertia);
-
-                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");
+                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(mass),
+                Simulation.Shapes.Add(topBlockShape), .01f));
 
-            {
-                //Stack with a heavy block on top. Note that the contact springiness we chose in the DemoNarrowPhaseCallbacks above is important to making the stack resist the weight of the top block.
-                //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);
-                //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));
-                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));
-                    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,
-                    new CollidableDescription(Simulation.Shapes.Add(topBlockShape), 0.1f), new BodyActivityDescription(-1f)));
+            rolloverInfo.Add(boxDescription.Pose.Position + new Vector3(0, 4, 0), $"{mass}:1 mass ratio");
+        }
 
-                rolloverInfo.Add(boxDescription.Pose.Position + new Vector3(0, 4, 0), "200:1 mass ratio");
-            }
+        {
+            //Now a weird rotating multi-arm thing. Long constraint sequences with high leverages under stress are a really tough problem for iterative velocity solvers.
+            //(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 boxShapeIndex = Simulation.Shapes.Add(boxShape);
+            var boxInertia = boxShape.ComputeInertia(1);
+            var linkDescription = BodyDescription.CreateDynamic(new Vector3(), boxInertia, boxShapeIndex, 0.01f);
 
+            for (int chainIndex = 0; chainIndex < 4; ++chainIndex)
             {
-                //Now a weird rotating multi-arm thing. Long constraint sequences with high leverages under stress are a really tough problem for iterative velocity solvers.
-                //(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);
-                boxShape.ComputeInertia(1, out var boxInertia);
-                var linkDescription = BodyDescription.CreateDynamic(new Vector3(), boxInertia, boxCollidable, new BodyActivityDescription(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, boxShapeIndex, 0.01f));
+                for (int linkIndex = 0; linkIndex < 8; ++linkIndex)
                 {
-                    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)));
-                    for (int linkIndex = 0; linkIndex < 8; ++linkIndex)
+                    var offset = new Vector3(boxShape.Width * 1.05f, 0, boxShape.Length - boxShape.Width);
+                    linkDescription.Pose.Position += offset;
+                    var linkHandle = Simulation.Bodies.Add(linkDescription);
+                    Simulation.Solver.Add(previousLinkHandle, linkHandle, new Hinge
                     {
-                        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);
-                        Simulation.Solver.Add(previousLinkHandle, linkHandle, new Hinge
-                        {
-                            LocalHingeAxisA = Vector3.UnitX,
-                            LocalHingeAxisB = Vector3.UnitX,
-                            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)
-                        });
-                        Simulation.Solver.Add(previousLinkHandle, linkHandle, new AngularAxisMotor
-                        {
-                            LocalAxisA = Vector3.UnitX,
-                            TargetVelocity = .25f,
-                            Settings = new MotorSettings(float.MaxValue, 0.0001f)
-                        });
-                        previousLinkHandle = linkHandle;
-                    }
+                        LocalHingeAxisA = Vector3.UnitX,
+                        LocalHingeAxisB = Vector3.UnitX,
+                        LocalOffsetA = offset * 0.5f,
+                        LocalOffsetB = offset * -0.5f,
+                        //Once again, the choice of high stiffness makes this potentially unstable without substepping.
+                        SpringSettings = new SpringSettings(640, 1)
+                    });
+                    Simulation.Solver.Add(previousLinkHandle, linkHandle, new AngularAxisMotor
+                    {
+                        LocalAxisA = Vector3.UnitX,
+                        TargetVelocity = .25f,
+                        Settings = new MotorSettings(float.MaxValue, 0.00001f)
+                    });
+                    previousLinkHandle = linkHandle;
                 }
-                rolloverInfo.Add(basePosition + new Vector3(0, 4, 0), "High stiffness, long lever arm motorized chain");
             }
+            rolloverInfo.Add(basePosition + new Vector3(0, 4, 0), "High stiffness, long lever arm motorized chain");
+        }
 
 
-            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), Simulation.Shapes.Add(new Box(200, 1, 200))));
 
-        }
-        public override void Update(Window window, Camera camera, Input input, float dt)
+    }
+
+    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<int>(new Buffer<int>(sleepingSetsMemory, Simulation.Bodies.Sets.Length - 1));
+        for (int i = 1; i < Simulation.Bodies.Sets.Length; ++i)
         {
-            var substepCountChange = (int)MathF.Max(1f, timestepper.SubstepCount * 0.25f);
-            var iterationCountChange = (int)MathF.Max(1f, Simulation.Solver.IterationCount * 0.25f);
-            if (input.WasPushed(OpenTK.Input.Key.Z))
+            if (Simulation.Bodies.Sets[i].Allocated)
             {
-                timestepper.SubstepCount = Math.Max(1, timestepper.SubstepCount - substepCountChange);
+                sleepingSets.AllocateUnsafely() = i;
             }
-            if (input.WasPushed(OpenTK.Input.Key.X))
-            {
-                timestepper.SubstepCount = Math.Min(8192, timestepper.SubstepCount + substepCountChange);
-            }
-            if (input.WasPushed(OpenTK.Input.Key.C))
-            {
-                Simulation.Solver.IterationCount = Math.Max(1, Simulation.Solver.IterationCount - iterationCountChange);
-            }
-            if (input.WasPushed(OpenTK.Input.Key.V))
-            {
-                Simulation.Solver.IterationCount = Math.Min(8192, Simulation.Solver.IterationCount + iterationCountChange);
-            }
-            base.Update(window, camera, input, dt);
         }
+        Simulation.Awakener.AwakenSets(ref sleepingSets);
+    }
 
-        public override void Render(Renderer renderer, Camera camera, Input input, TextBuilder text, Font font)
+    public override void Update(Window window, Camera camera, Input input, float dt)
+    {
+        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))
         {
-            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("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);
+            Simulation.Solver.SubstepCount = Math.Max(1, Simulation.Solver.SubstepCount - substepCountChange);
+            AwakenAllBodies();
         }
+        if (input.WasPushed(OpenTK.Input.Key.X))
+        {
+            Simulation.Solver.SubstepCount = Math.Min(512, 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(512, 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)
+    {
+        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 755a1f6e5..2059de217 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;
@@ -11,384 +12,389 @@
 using System.Numerics;
 using System.Runtime.CompilerServices;
 
-namespace Demos.Demos
+namespace Demos.Demos;
+
+public class SweepDemo : Demo
 {
-    public class SweepDemo : Demo
+    struct Filter : ISweepFilter
     {
-        struct Filter : ISweepFilter
+        [MethodImpl(MethodImplOptions.AggressiveInlining)]
+        public bool AllowTest(int childA, int childB)
         {
-            [MethodImpl(MethodImplOptions.AggressiveInlining)]
-            public bool AllowTest(int childA, int childB)
-            {
-                return true;
-            }
+            return true;
         }
+    }
 
-        ConvexHull hull;
+    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;
+        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 box = new Box(2f, 2f, 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 = 12;
+        const int height = 3;
+        const int length = 12;
+        for (int i = 0; i < width; ++i)
         {
-            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());
-
-            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 boxIndex = Simulation.Shapes.Add(box);
-            var capsuleIndex = Simulation.Shapes.Add(capsule);
-            var sphereIndex = Simulation.Shapes.Add(sphere);
-            const int width = 12;
-            const int height = 3;
-            const int length = 12;
-            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 * 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, default, 0.1f);
+                    switch (j % 3)
                     {
-                        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));
-                        switch (j % 3)
-                        {
-                            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;
-                        }
-                        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;
                     }
+                    Simulation.Bodies.Add(bodyDescription);
+
                 }
             }
+        }
 
-            //Don't really want to regenerate a convex hull every frame; just cache one out.
-            const int pointCount = 32;
-            var points = new QuickList<Vector3>(pointCount, BufferPool);
-            var random = new Random(5);
-            for (int i = 0; i < pointCount; ++i)
+        //Don't really want to regenerate a convex hull every frame; just cache one out.
+        const int pointCount = 32;
+        var points = new QuickList<Vector3>(pointCount, BufferPool);
+        var random = new Random(5);
+        for (int i = 0; i < pointCount; ++i)
+        {
+            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);
+
+        //var staticShapeIndex = Simulation.Shapes.Add(new Box(100, 1, 100));
+        //var staticDescription = new StaticDescription
+        //{
+        //    Collidable = new CollidableDescription
+        //    {
+        //        Continuity = new ContinuousDetectionSettings { Mode = ContinuousDetectionMode.Discrete },
+        //        Shape = Simulation.Shapes.Add(new Box(100, 1, 100)),
+        //        SpeculativeMargin = 0.1f
+        //    },
+        //    Pose = new RigidPose { Position = new Vector3(0, -1, 0), Orientation = Quaternion.Identity }
+        //};
+        //Simulation.Statics.Add(staticDescription);
+
+        const int planeWidth = 64;
+        const int planeHeight = 64;
+        var planeMesh = DemoMeshHelper.CreateDeformedPlane(planeWidth, planeHeight,
+            (int x, int y) =>
             {
-                points.AllocateUnsafely() = new Vector3((float)random.NextDouble() - 0.5f, (float)random.NextDouble() - 0.5f, (float)random.NextDouble() - 0.5f);
-            }
-            ConvexHullHelper.CreateShape(points.Span.Slice(points.Count), BufferPool, out _, out hull);
-            points.Dispose(BufferPool);
-
-            //var staticShapeIndex = Simulation.Shapes.Add(new Box(100, 1, 100));
-            //var staticDescription = new StaticDescription
-            //{
-            //    Collidable = new CollidableDescription
-            //    {
-            //        Continuity = new ContinuousDetectionSettings { Mode = ContinuousDetectionMode.Discrete },
-            //        Shape = Simulation.Shapes.Add(new Box(100, 1, 100)),
-            //        SpeculativeMargin = 0.1f
-            //    },
-            //    Pose = new RigidPose { Position = new Vector3(0, -1, 0), Orientation = Quaternion.Identity }
-            //};
-            //Simulation.Statics.Add(staticDescription);
-
-            const int planeWidth = 64;
-            const int planeHeight = 64;
-            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);
-            Simulation.Statics.Add(new StaticDescription(new Vector3(-64, -10, -64), new CollidableDescription(Simulation.Shapes.Add(planeMesh), 0.1f)));
+                return new Vector3(x, 1 * MathF.Cos(x / 4f) * MathF.Sin(y / 4f), y);
+            }, new Vector3(2, 3, 2), BufferPool);
+        Simulation.Statics.Add(new StaticDescription(new Vector3(-64, -10, -64), Simulation.Shapes.Add(planeMesh)));
 
-        }
+    }
 
-        void DrawShape<TShape>(ref TShape shape, ref RigidPose pose, in Vector3 color, Shapes shapes, Renderer renderer)
-            where TShape : struct, IShape
+    void DrawShape<TShape>(ref TShape shape, in RigidPose pose, Vector3 color, Shapes shapes, Renderer renderer)
+        where TShape : struct, IShape
+    {
+        if (typeof(TShape) == typeof(Triangle))
         {
-            if (typeof(TShape) == typeof(Triangle))
-            {
-                //For the sake of visualization in this demo, give the triangles a backface. Collisions don't have backfaces, but sweeps do, and it's nice to be able to see the shape.
-                //A little bit hacky, but hey, it works.
-                ref var triangle = ref Unsafe.As<TShape, Triangle>(ref shape);
-                Triangle flippedTriangle;
-                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);
-            }
-            else
-            {
-                renderer.Shapes.AddShape(shape, shapes, ref pose, color);
-            }
+            //For the sake of visualization in this demo, give the triangles a backface. Collisions don't have backfaces, but sweeps do, and it's nice to be able to see the shape.
+            //A little bit hacky, but hey, it works.
+            ref var triangle = ref Unsafe.As<TShape, Triangle>(ref shape);
+            Triangle flippedTriangle;
+            flippedTriangle.C = triangle.C;
+            flippedTriangle.A = triangle.B;
+            flippedTriangle.B = triangle.A;
+            renderer.Shapes.AddShape(triangle, shapes, pose, color);
+            renderer.Shapes.AddShape(flippedTriangle, shapes, pose, color);
         }
-
-        unsafe void DrawSweep<TShape>(TShape shape, ref RigidPose pose, in BodyVelocity velocity, int steps,
-            float t, Renderer renderer, in Vector3 color)
-            where TShape : struct, IShape
+        else
         {
-            if (steps == 1)
-            {
-                DrawShape(ref shape, ref pose, color, Simulation.Shapes, renderer);
-            }
-            else
-            {
-                var inverse = 1f / (steps - 1);
-                for (int i = steps - 1; i >= 0; --i)
-                {
-                    var stepProgression = i * inverse;
-                    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);
-                }
-            }
+            renderer.Shapes.AddShape(shape, shapes, pose, color);
         }
+    }
 
-        unsafe void DrawImpact(Renderer renderer, ref Vector3 hitLocation, ref Vector3 hitNormal)
+    void DrawSweep<TShape>(TShape shape, in RigidPose pose, in BodyVelocity velocity, int steps,
+        float t, Renderer renderer, Vector3 color)
+        where TShape : struct, IShape
+    {
+        if (steps == 1)
         {
-            //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);
-            renderer.Lines.Allocate() = new DemoRenderer.Constraints.LineInstance(hitLocation - tangent1, hitLocation + tangent1, new Vector3(0, 1, 0), new Vector3());
-            renderer.Lines.Allocate() = new DemoRenderer.Constraints.LineInstance(hitLocation - tangent2, hitLocation + tangent2, new Vector3(0, 1, 0), new Vector3());
+            DrawShape(ref shape, pose, color, Simulation.Shapes, renderer);
         }
-        unsafe void TestSweep<TShapeA, TShapeB>(
-            TShapeA a, RigidPose poseA, in BodyVelocity velocityA,
-            TShapeB b, RigidPose poseB, in BodyVelocity velocityB,
-            float maximumT, Renderer renderer)
-            where TShapeA : struct, IShape
-            where TShapeB : struct, IShape
+        else
         {
-            var filter = new Filter();
-
-            var task = Simulation.NarrowPhase.SweepTaskRegistry.GetTask(a.TypeId, b.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,
-                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;
-
-            var hitTint = intersected ? new Vector3(0.5f, 1, 0.5f) : new Vector3(1f, 0.5f, 0.5f);
-            var colorA = new Vector3(0.75f, 0.75f, 1) * hitTint;
-            var colorB = new Vector3(0.75f, 1f, 1) * hitTint;
-
-            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);
-
-            if (intersected && t1 > 0)
+            var inverse = 1f / (steps - 1);
+            for (int i = steps - 1; i >= 0; --i)
             {
-                DrawImpact(renderer, ref hitLocation, ref hitNormal);
+                var stepProgression = i * inverse;
+                var stepT = stepProgression * t;
+                PoseIntegration.Integrate(pose, velocity, stepT, out var stepPose);
+                var stepColor = color * (0.2f + 0.8f * stepProgression);
+                DrawShape(ref shape, stepPose, stepColor, Simulation.Shapes, renderer);
             }
         }
+    }
 
-        public override void Update(Window window, Camera camera, Input input, float dt)
+    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);
+        renderer.Lines.Allocate() = new DemoRenderer.Constraints.LineInstance(hitLocation - tangent1, hitLocation + tangent1, new Vector3(0, 1, 0), new Vector3());
+        renderer.Lines.Allocate() = new DemoRenderer.Constraints.LineInstance(hitLocation - tangent2, hitLocation + tangent2, new Vector3(0, 1, 0), new Vector3());
+    }
+    unsafe void TestSweep<TShapeA, TShapeB>(
+        TShapeA a, RigidPose poseA, in BodyVelocity velocityA,
+        TShapeB b, RigidPose poseB, in BodyVelocity velocityB,
+        float maximumT, Renderer renderer)
+        where TShapeA : struct, IShape
+        where TShapeB : struct, IShape
+    {
+        var filter = new Filter();
+
+        var task = Simulation.NarrowPhase.SweepTaskRegistry.GetTask(TShapeA.TypeId, TShapeB.TypeId);
+        if (task == null)
+            return;
+        var intersected = task.Sweep(
+            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;
+
+        var hitTint = intersected ? new Vector3(0.5f, 1, 0.5f) : new Vector3(1f, 0.5f, 0.5f);
+        var colorA = new Vector3(0.75f, 0.75f, 1) * hitTint;
+        var colorB = new Vector3(0.75f, 1f, 1) * hitTint;
+
+        var stepCount = (intersected && t1 > 0) || !intersected ? 100 : 1;
+        var visualizedT = intersected ? t1 : maximumT;
+        DrawSweep(a, poseA, velocityA, stepCount, visualizedT, renderer, colorA);
+        DrawSweep(b, poseB, velocityB, stepCount, visualizedT, renderer, colorB);
+
+        if (intersected && t1 > 0)
         {
-            base.Update(window, camera, input, dt);
-
-            if (!input.WasDown(OpenTK.Input.Key.P))
-                animationT = (animationT + 1 / 60f) % (128);
+            DrawImpact(renderer, ref hitLocation, ref hitNormal);
         }
+    }
 
-        float animationT;
+    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 + TimestepDuration) % (128);
+    }
 
-        struct SceneSweepHitHandler : ISweepHitHandler
-        {
-            public Vector3 HitLocation;
-            public Vector3 HitNormal;
-            public float T;
+    float animationT;
 
-            [MethodImpl(MethodImplOptions.AggressiveInlining)]
-            public bool AllowTest(CollidableReference collidable)
-            {
-                return true;
-            }
 
-            [MethodImpl(MethodImplOptions.AggressiveInlining)]
-            public bool AllowTest(CollidableReference collidable, int child)
-            {
-                return true;
-            }
+    struct SceneSweepHitHandler : ISweepHitHandler
+    {
+        public Vector3 HitLocation;
+        public Vector3 HitNormal;
+        public float T;
 
-            [MethodImpl(MethodImplOptions.AggressiveInlining)]
-            public void OnHit(ref float maximumT, float t, in Vector3 hitLocation, in 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.
-                if (t < maximumT)
-                    maximumT = t;
-                if (t < T)
-                {
-                    T = t;
-                    HitLocation = hitLocation;
-                    HitNormal = hitNormal;
-                }
-            }
+        [MethodImpl(MethodImplOptions.AggressiveInlining)]
+        public bool AllowTest(CollidableReference collidable)
+        {
+            return true;
+        }
+
+        [MethodImpl(MethodImplOptions.AggressiveInlining)]
+        public bool AllowTest(CollidableReference collidable, int child)
+        {
+            return true;
+        }
 
-            [MethodImpl(MethodImplOptions.AggressiveInlining)]
-            public void OnHitAtZeroT(ref float maximumT, CollidableReference collidable)
+        [MethodImpl(MethodImplOptions.AggressiveInlining)]
+        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.
+            if (t < maximumT)
+                maximumT = t;
+            if (t < T)
             {
-                maximumT = 0;
-                T = 0;
-                HitLocation = new Vector3();
-                HitNormal = new Vector3();
+                T = t;
+                HitLocation = hitLocation;
+                HitNormal = hitNormal;
             }
         }
 
-        void StandardTestSweep<TA, TB>(in TA a, in TB b, ref Vector3 position, in Quaternion initialOrientationA, in Quaternion initialOrientationB, Renderer renderer)
-            where TA : struct, IShape where TB : struct, IShape
+        [MethodImpl(MethodImplOptions.AggressiveInlining)]
+        public void OnHitAtZeroT(ref float maximumT, CollidableReference collidable)
         {
-            TestSweep(
-                a,
-                new RigidPose { Position = new Vector3(-10, 0, 0) + position, Orientation = initialOrientationA },
-                new BodyVelocity { Linear = new Vector3(1, -1, 0), Angular = new Vector3(1, 0, 1) },
-                b,
-                new RigidPose { Position = new Vector3(10, 0, 0) + position, Orientation = initialOrientationB },
-                new BodyVelocity { Linear = new Vector3(-1, -1, 0), Angular = new Vector3(0, 1, 0) },
-                50f, renderer);
-            position.Y -= 5;
+            maximumT = 0;
+            T = 0;
+            HitLocation = new Vector3();
+            HitNormal = new Vector3();
         }
-        public override void Render(Renderer renderer, Camera camera, Input input, TextBuilder text, Font font)
-        {
-            var xRotation = QuaternionEx.CreateFromAxisAngle(new Vector3(1, 0, 0), 0.02f * animationT * MathHelper.Pi);
-            var yRotation = QuaternionEx.CreateFromAxisAngle(new Vector3(0, 1, 0), 0.04f * animationT * MathHelper.Pi);
-            var zRotation = QuaternionEx.CreateFromAxisAngle(new Vector3(0, 0, 1), 0.06f * animationT * MathHelper.Pi);
-            var worldA = QuaternionEx.Concatenate(xRotation, QuaternionEx.Concatenate(yRotation, zRotation));
-            var worldB = QuaternionEx.Concatenate(yRotation, QuaternionEx.Concatenate(zRotation, xRotation));
-            base.Render(renderer, camera, input, text, font);
-
-            var compoundBuilder = new CompoundBuilder(BufferPool, Simulation.Shapes, 8);
-            compoundBuilder.Add(new Box(1f, 0.5f, 0.75f), new RigidPose { Orientation = Quaternion.Identity, Position = new Vector3(-0.5f, 0, 0) }, 1);
-            compoundBuilder.Add(new Sphere(0.5f), new RigidPose { Orientation = Quaternion.Identity, Position = new Vector3(0.5f, 0, 0) }, 1);
-            compoundBuilder.BuildKinematicCompound(out var compoundChildren);
-            var compound = new Compound(compoundChildren);
-            var bigCompound = new BigCompound(compoundChildren, Simulation.Shapes, BufferPool);
-            compoundBuilder.Dispose();
-
-            const int planeWidth = 3;
-            const int planeHeight = 3;
-            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);
-
-
-            var triangle = new Triangle(new Vector3(0, 0, 0), new Vector3(2, 0, -1), new Vector3(-1, 0, 1.5f));
-            var triangleCenter = (triangle.A + triangle.B + triangle.C) / 3f;
-            triangle.A -= triangleCenter;
-            triangle.B -= triangleCenter;
-            triangle.C -= triangleCenter;
-            var position = new Vector3(-90, 60, -75);
-            StandardTestSweep(new Sphere(0.5f), new Sphere(.25f), ref position, worldA, worldB, renderer);
-            StandardTestSweep(new Sphere(0.5f), new Capsule(.25f, 1f), ref position, worldA, worldB, renderer);
-            StandardTestSweep(new Sphere(0.5f), new Box(.5f, 1f, 1.5f), ref position, worldA, worldB, renderer);
-            StandardTestSweep(new Sphere(0.5f), triangle, ref position, worldA, worldB, renderer);
-            StandardTestSweep(new Sphere(0.5f), new Cylinder(.25f, 1f), ref position, worldA, worldB, renderer);
-            StandardTestSweep(new Sphere(0.5f), hull, ref position, worldA, worldB, renderer);
-            StandardTestSweep(new Sphere(0.5f), compound, ref position, worldA, worldB, renderer);
-            StandardTestSweep(new Sphere(0.5f), bigCompound, ref position, worldA, worldB, renderer);
-            StandardTestSweep(new Sphere(0.5f), mesh, ref position, worldA, worldB, renderer);
-
-            position = new Vector3(-60, 60, -75);
-            StandardTestSweep(new Capsule(0.5f, 1), new Capsule(.25f, 1.5f), ref position, worldA, worldB, renderer);
-            StandardTestSweep(new Capsule(0.5f, 1), new Box(.5f, 1f, 1.5f), ref position, worldA, worldB, renderer);
-            StandardTestSweep(new Capsule(0.5f, 1), triangle, ref position, worldA, worldB, renderer);
-            StandardTestSweep(new Capsule(0.5f, 1), new Cylinder(.25f, 1.5f), ref position, worldA, worldB, renderer);
-            StandardTestSweep(new Capsule(0.5f, 1), hull, ref position, worldA, worldB, renderer);
-            StandardTestSweep(new Capsule(0.5f, 1), compound, ref position, worldA, worldB, renderer);
-            StandardTestSweep(new Capsule(0.5f, 1), bigCompound, ref position, worldA, worldB, renderer);
-            StandardTestSweep(new Capsule(0.5f, 1), mesh, ref position, worldA, worldB, renderer);
-
-            position = new Vector3(-30, 60, -75);
-            StandardTestSweep(new Box(0.5f, 0.5f, 0.5f), new Box(.5f, 1f, 1.5f), ref position, worldA, worldB, renderer);
-            StandardTestSweep(new Box(0.5f, 0.5f, 0.5f), triangle, ref position, worldA, worldB, renderer);
-            StandardTestSweep(new Box(0.5f, 0.5f, 0.5f), new Cylinder(.25f, 1.5f), ref position, worldA, worldB, renderer);
-            StandardTestSweep(new Box(0.5f, 0.5f, 0.5f), hull, ref position, worldA, worldB, renderer);
-            StandardTestSweep(new Box(0.5f, 0.5f, 0.5f), compound, ref position, worldA, worldB, renderer);
-            StandardTestSweep(new Box(0.5f, 0.5f, 0.5f), bigCompound, ref position, worldA, worldB, renderer);
-            StandardTestSweep(new Box(0.5f, 0.5f, 0.5f), mesh, ref position, worldA, worldB, renderer);
-
-            position = new Vector3(0, 60, -75);
-            StandardTestSweep(triangle, triangle, ref position, worldA, worldB, renderer);
-            StandardTestSweep(triangle, new Cylinder(0.5f, 1), ref position, worldA, worldB, renderer);
-            StandardTestSweep(triangle, hull, ref position, worldA, worldB, renderer);
-            StandardTestSweep(triangle, compound, ref position, worldA, worldB, renderer);
-            StandardTestSweep(triangle, bigCompound, ref position, worldA, worldB, renderer);
-            StandardTestSweep(triangle, mesh, ref position, worldA, worldB, renderer);
-
-            position = new Vector3(30, 60, -75);
-            StandardTestSweep(new Cylinder(0.5f, 1), new Cylinder(.25f, 1.5f), ref position, worldA, worldB, renderer);
-            StandardTestSweep(new Cylinder(0.5f, 1), hull, ref position, worldA, worldB, renderer);
-            StandardTestSweep(new Cylinder(0.5f, 1), compound, ref position, worldA, worldB, renderer);
-            StandardTestSweep(new Cylinder(0.5f, 1), bigCompound, ref position, worldA, worldB, renderer);
-            StandardTestSweep(new Cylinder(0.5f, 1), mesh, ref position, worldA, worldB, renderer);
-
-            position = new Vector3(60, 60, -75);
-            StandardTestSweep(hull, hull, ref position, worldA, worldB, renderer);
-            StandardTestSweep(hull, compound, ref position, worldA, worldB, renderer);
-            StandardTestSweep(hull, bigCompound, ref position, worldA, worldB, renderer);
-            StandardTestSweep(hull, mesh, ref position, worldA, worldB, renderer);
-
-            position = new Vector3(90, 60, -75);
-            StandardTestSweep(compound, compound, ref position, worldA, worldB, renderer);
-            StandardTestSweep(compound, bigCompound, ref position, worldA, worldB, renderer);
-            StandardTestSweep(compound, mesh, ref position, worldA, worldB, renderer);
-
-            position = new Vector3(120, 60, -75);
-            StandardTestSweep(bigCompound, bigCompound, ref position, worldA, worldB, renderer);
-            StandardTestSweep(bigCompound, mesh, ref position, worldA, worldB, renderer);
-
-
-            //Get rid of the compound children registries so that we don't spam allocations.
-            for (int i = 0; i < compound.Children.Length; ++i)
+    }
+
+    void StandardTestSweep<TA, TB>(in TA a, in TB b, ref Vector3 position, Quaternion initialOrientationA, Quaternion initialOrientationB, Renderer renderer)
+        where TA : struct, IShape where TB : struct, IShape
+    {
+        TestSweep(
+            a,
+            new RigidPose { Position = new Vector3(-10, 0, 0) + position, Orientation = initialOrientationA },
+            new BodyVelocity { Linear = new Vector3(1, -1, 0), Angular = new Vector3(1, 0, 1) },
+            b,
+            new RigidPose { Position = new Vector3(10, 0, 0) + position, Orientation = initialOrientationB },
+            new BodyVelocity { Linear = new Vector3(-1, -1, 0), Angular = new Vector3(0, 1, 0) },
+            50f, renderer);
+        position.Y -= 5;
+    }
+    public override void Render(Renderer renderer, Camera camera, Input input, TextBuilder text, Font font)
+    {
+        var xRotation = QuaternionEx.CreateFromAxisAngle(new Vector3(1, 0, 0), 0.02f * animationT * MathHelper.Pi);
+        var yRotation = QuaternionEx.CreateFromAxisAngle(new Vector3(0, 1, 0), 0.04f * animationT * MathHelper.Pi);
+        var zRotation = QuaternionEx.CreateFromAxisAngle(new Vector3(0, 0, 1), 0.06f * animationT * MathHelper.Pi);
+        var worldA = QuaternionEx.Concatenate(xRotation, QuaternionEx.Concatenate(yRotation, zRotation));
+        var worldB = QuaternionEx.Concatenate(yRotation, QuaternionEx.Concatenate(zRotation, xRotation));
+        base.Render(renderer, camera, input, text, font);
+
+        var compoundBuilder = new CompoundBuilder(BufferPool, Simulation.Shapes, 8);
+        compoundBuilder.Add(new Box(1f, 0.5f, 0.75f), new RigidPose { Orientation = Quaternion.Identity, Position = new Vector3(-0.5f, 0, 0) }, 1);
+        compoundBuilder.Add(new Sphere(0.5f), new RigidPose { Orientation = Quaternion.Identity, Position = new Vector3(0.5f, 0, 0) }, 1);
+        compoundBuilder.BuildKinematicCompound(out var compoundChildren);
+        var compound = new Compound(compoundChildren);
+        var bigCompound = new BigCompound(compoundChildren, Simulation.Shapes, BufferPool);
+        compoundBuilder.Dispose();
+
+        const int planeWidth = 3;
+        const int planeHeight = 3;
+        var mesh = DemoMeshHelper.CreateDeformedPlane(planeWidth, planeHeight,
+            (int x, int y) =>
             {
-                Simulation.Shapes.Remove(compound.Children[i].ShapeIndex);
-            }
-            //The resources of compound are a subset of those used by the bigCompoound, so we just dispose the bigCompound.
-            bigCompound.Dispose(BufferPool);
-            mesh.Dispose(BufferPool);
-
-            //Perform simulation-wide queries against the other collidables in the scene.
-            var localOrigin = new Vector3(-25, 15, 0);
-            var localDirection = new Vector3(7, -10, 0);
-            var sweepCount = 16;
-            for (int i = 0; i < sweepCount; ++i)
+                return new Vector3(x - 1.5f, 0.1f * MathF.Cos(x) * MathF.Sin(y), y - 1.5f);
+            }, 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));
+        var triangleCenter = (triangle.A + triangle.B + triangle.C) / 3f;
+        triangle.A -= triangleCenter;
+        triangle.B -= triangleCenter;
+        triangle.C -= triangleCenter;
+        var position = new Vector3(-90, 60, -75);
+        StandardTestSweep(new Sphere(0.5f), new Sphere(.25f), ref position, worldA, worldB, renderer);
+        StandardTestSweep(new Sphere(0.5f), new Capsule(.25f, 1f), ref position, worldA, worldB, renderer);
+        StandardTestSweep(new Sphere(0.5f), new Box(.5f, 1f, 1.5f), ref position, worldA, worldB, renderer);
+        StandardTestSweep(new Sphere(0.5f), triangle, ref position, worldA, worldB, renderer);
+        StandardTestSweep(new Sphere(0.5f), new Cylinder(.25f, 1f), ref position, worldA, worldB, renderer);
+        StandardTestSweep(new Sphere(0.5f), hull, ref position, worldA, worldB, renderer);
+        StandardTestSweep(new Sphere(0.5f), compound, ref position, worldA, worldB, renderer);
+        StandardTestSweep(new Sphere(0.5f), bigCompound, ref position, worldA, worldB, renderer);
+        StandardTestSweep(new Sphere(0.5f), mesh, ref position, worldA, worldB, renderer);
+
+        position = new Vector3(-60, 60, -75);
+        StandardTestSweep(new Capsule(0.5f, 1), new Capsule(.25f, 1.5f), ref position, worldA, worldB, renderer);
+        StandardTestSweep(new Capsule(0.5f, 1), new Box(.5f, 1f, 1.5f), ref position, worldA, worldB, renderer);
+        StandardTestSweep(new Capsule(0.5f, 1), triangle, ref position, worldA, worldB, renderer);
+        StandardTestSweep(new Capsule(0.5f, 1), new Cylinder(.25f, 1.5f), ref position, worldA, worldB, renderer);
+        StandardTestSweep(new Capsule(0.5f, 1), hull, ref position, worldA, worldB, renderer);
+        StandardTestSweep(new Capsule(0.5f, 1), compound, ref position, worldA, worldB, renderer);
+        StandardTestSweep(new Capsule(0.5f, 1), bigCompound, ref position, worldA, worldB, renderer);
+        StandardTestSweep(new Capsule(0.5f, 1), mesh, ref position, worldA, worldB, renderer);
+
+        position = new Vector3(-30, 60, -75);
+        StandardTestSweep(new Box(0.5f, 0.5f, 0.5f), new Box(.5f, 1f, 1.5f), ref position, worldA, worldB, renderer);
+        StandardTestSweep(new Box(0.5f, 0.5f, 0.5f), triangle, ref position, worldA, worldB, renderer);
+        StandardTestSweep(new Box(0.5f, 0.5f, 0.5f), new Cylinder(.25f, 1.5f), ref position, worldA, worldB, renderer);
+        StandardTestSweep(new Box(0.5f, 0.5f, 0.5f), hull, ref position, worldA, worldB, renderer);
+        StandardTestSweep(new Box(0.5f, 0.5f, 0.5f), compound, ref position, worldA, worldB, renderer);
+        StandardTestSweep(new Box(0.5f, 0.5f, 0.5f), bigCompound, ref position, worldA, worldB, renderer);
+        StandardTestSweep(new Box(0.5f, 0.5f, 0.5f), mesh, ref position, worldA, worldB, renderer);
+
+        position = new Vector3(0, 60, -75);
+        StandardTestSweep(triangle, triangle, ref position, worldA, worldB, renderer);
+        StandardTestSweep(triangle, new Cylinder(0.5f, 1), ref position, worldA, worldB, renderer);
+        StandardTestSweep(triangle, hull, ref position, worldA, worldB, renderer);
+        StandardTestSweep(triangle, compound, ref position, worldA, worldB, renderer);
+        StandardTestSweep(triangle, bigCompound, ref position, worldA, worldB, renderer);
+        StandardTestSweep(triangle, mesh, ref position, worldA, worldB, renderer);
+
+        position = new Vector3(30, 60, -75);
+        StandardTestSweep(new Cylinder(0.5f, 1), new Cylinder(.25f, 1.5f), ref position, worldA, worldB, renderer);
+        StandardTestSweep(new Cylinder(0.5f, 1), hull, ref position, worldA, worldB, renderer);
+        StandardTestSweep(new Cylinder(0.5f, 1), compound, ref position, worldA, worldB, renderer);
+        StandardTestSweep(new Cylinder(0.5f, 1), bigCompound, ref position, worldA, worldB, renderer);
+        StandardTestSweep(new Cylinder(0.5f, 1), mesh, ref position, worldA, worldB, renderer);
+
+        position = new Vector3(60, 60, -75);
+        StandardTestSweep(hull, hull, ref position, worldA, worldB, renderer);
+        StandardTestSweep(hull, compound, ref position, worldA, worldB, renderer);
+        StandardTestSweep(hull, bigCompound, ref position, worldA, worldB, renderer);
+        StandardTestSweep(hull, mesh, ref position, worldA, worldB, renderer);
+
+        position = new Vector3(90, 60, -75);
+        StandardTestSweep(compound, compound, ref position, worldA, worldB, renderer);
+        StandardTestSweep(compound, bigCompound, ref position, worldA, worldB, renderer);
+        StandardTestSweep(compound, mesh, ref position, worldA, worldB, renderer);
+
+        position = new Vector3(120, 60, -75);
+        StandardTestSweep(bigCompound, bigCompound, ref position, worldA, worldB, renderer);
+        StandardTestSweep(bigCompound, mesh, ref position, worldA, worldB, renderer);
+
+
+        //Get rid of the compound children registries so that we don't spam allocations.
+        for (int i = 0; i < compound.Children.Length; ++i)
+        {
+            Simulation.Shapes.Remove(compound.Children[i].ShapeIndex);
+        }
+        //The resources of compound are a subset of those used by the bigCompoound, so we just dispose the bigCompound.
+        bigCompound.Dispose(BufferPool);
+        mesh.Dispose(BufferPool);
+
+        //Perform simulation-wide queries against the other collidables in the scene.
+        var localOrigin = new Vector3(-25, 15, 0);
+        var localDirection = new Vector3(7, -10, 0);
+        var sweepCount = 16;
+        for (int i = 0; i < sweepCount; ++i)
+        {
+            Matrix3x3.CreateFromAxisAngle(new Vector3(0, 1, 0), i * MathHelper.TwoPi / sweepCount, out var rotation);
+            Matrix3x3.Transform(localOrigin, rotation, out var sweepOrigin);
+            Matrix3x3.Transform(localDirection, rotation, out var sweepDirection);
+
+            SceneSweepHitHandler hitHandler = default;
+            hitHandler.T = float.MaxValue;
+            var shape = new Box(1, 2, 1.5f);
+            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, 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)
             {
-                Matrix3x3.CreateFromAxisAngle(new Vector3(0, 1, 0), i * MathHelper.TwoPi / sweepCount, out var rotation);
-                Matrix3x3.Transform(localOrigin, rotation, out var sweepOrigin);
-                Matrix3x3.Transform(localDirection, rotation, out var sweepDirection);
-
-                SceneSweepHitHandler hitHandler = default;
-                hitHandler.T = float.MaxValue;
-                var shape = new Box(1, 2, 1.5f);
-                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,
-                    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)
-                {
-                    DrawImpact(renderer, ref hitHandler.HitLocation, ref hitHandler.HitNormal);
-                }
+                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/Demos/Tanks/AITank.cs b/Demos/Demos/Tanks/AITank.cs
index 4d6c1cf88..91c72fbc5 100644
--- a/Demos/Demos/Tanks/AITank.cs
+++ b/Demos/Demos/Tanks/AITank.cs
@@ -4,114 +4,113 @@
 using BepuUtilities;
 using BepuUtilities.Collections;
 
-namespace Demos.Demos.Tanks
+namespace Demos.Demos.Tanks;
+
+public struct AITank
 {
-    public struct AITank
-    {
-        /// <summary>
-        /// Controller used by the AI.
-        /// </summary>
-        public TankController Controller;
-        /// <summary>
-        /// Index of the current target of this AI.
-        /// </summary>
-        public int Target;
-        /// <summary>
-        /// Current movement target location.
-        /// </summary>
-        public Vector2 MovementTarget;
-        /// <summary>
-        /// Index of the last frame that the tank fired on.
-        /// </summary>
-        public long LastShotFrame;
+    /// <summary>
+    /// Controller used by the AI.
+    /// </summary>
+    public TankController Controller;
+    /// <summary>
+    /// Index of the current target of this AI.
+    /// </summary>
+    public int Target;
+    /// <summary>
+    /// Current movement target location.
+    /// </summary>
+    public Vector2 MovementTarget;
+    /// <summary>
+    /// Index of the last frame that the tank fired on.
+    /// </summary>
+    public long LastShotFrame;
 
-        /// <summary>
-        /// Remaining hit points of the AI. When it hits zero, the tank will fall apart.
-        /// </summary>
-        internal int HitPoints;
+    /// <summary>
+    /// Remaining hit points of the AI. When it hits zero, the tank will fall apart.
+    /// </summary>
+    internal int HitPoints;
 
-        public void Update(Simulation simulation, CollidableProperty<TankDemoBodyProperties> bodyProperties, Random random, long frameIndex, in Vector2 playAreaMin, in Vector2 playAreaMax, int aiIndex, ref QuickList<AITank> aiTanks, ref int projectileCount)
+    public void Update(Simulation simulation, CollidableProperty<TankDemoBodyProperties> bodyProperties, Random random, long frameIndex, in Vector2 playAreaMin, in Vector2 playAreaMax, int aiIndex, ref QuickList<AITank> aiTanks, ref int projectileCount)
+    {
+        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);
+        if (tankUp.Y < -0.5f)
         {
-            ref var currentPose = ref simulation.Bodies.GetBodyReference(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);
-            if (tankUp.Y < -0.5f)
-            {
-                //The tank is upside down. Don't bother doing anything.
-                Controller.UpdateMovementAndAim(simulation, 0, 0, false, false, false, new Vector3(1, 0, 0));
-                return;
-            }
-            if (Target >= aiTanks.Count || Target == aiIndex || random.NextDouble() < 1f / 250f)
+            //The tank is upside down. Don't bother doing anything.
+            Controller.UpdateMovementAndAim(simulation, 0, 0, false, false, false, new Vector3(1, 0, 0));
+            return;
+        }
+        if (Target >= aiTanks.Count || Target == aiIndex || random.NextDouble() < 1f / 250f)
+        {
+            //Change target.
+            if (aiTanks.Count > 1)
             {
-                //Change target.
-                if (aiTanks.Count > 1)
-                {
-                    do { Target = random.Next(0, aiTanks.Count); } while (Target == aiIndex);
-                }
+                do { Target = random.Next(0, aiTanks.Count); } while (Target == aiIndex);
             }
-            ref var targetTank = ref aiTanks[Target];
-            var targetTankBody = simulation.Bodies.GetBodyReference(targetTank.Controller.Tank.Body);
-            ref var targetTankPosition = ref targetTankBody.Pose.Position;
-            var currentTankPosition2D = new Vector2(currentPose.Position.X, currentPose.Position.Z);
-            var targetTankPosition2D = new Vector2(targetTankPosition.X, targetTankPosition.Z);
-            var currentToTargetTank2D = targetTankPosition2D - currentTankPosition2D;
-            if (random.NextDouble() < 1f / 250f)
+        }
+        ref var targetTank = ref aiTanks[Target];
+        var targetTankBody = simulation.Bodies[targetTank.Controller.Tank.Body];
+        ref var targetTankPosition = ref targetTankBody.Pose.Position;
+        var currentTankPosition2D = new Vector2(currentPose.Position.X, currentPose.Position.Z);
+        var targetTankPosition2D = new Vector2(targetTankPosition.X, targetTankPosition.Z);
+        var currentToTargetTank2D = targetTankPosition2D - currentTankPosition2D;
+        if (random.NextDouble() < 1f / 250f)
+        {
+            //Change movement target. Pick a random point around the target's current location.
+            //Try to avoid being excessively close, though- that tends to make all the tanks bunch up in the middle.
+            Vector2 movementTargetOffset;
+            float offsetLengthSquared;
+            do
             {
-                //Change movement target. Pick a random point around the target's current location.
-                //Try to avoid being excessively close, though- that tends to make all the tanks bunch up in the middle.
-                Vector2 movementTargetOffset;
-                float offsetLengthSquared;
-                do
-                {
-                    movementTargetOffset = new Vector2((float)(random.NextDouble() - 0.5) * 150, (float)(random.NextDouble() - 0.5) * 150);
-                    offsetLengthSquared = movementTargetOffset.LengthSquared();
+                movementTargetOffset = new Vector2((float)(random.NextDouble() - 0.5) * 150, (float)(random.NextDouble() - 0.5) * 150);
+                offsetLengthSquared = movementTargetOffset.LengthSquared();
 
-                }
-                while (offsetLengthSquared > 150 * 150 || offsetLengthSquared < 50 * 50);
-                MovementTarget = Vector2.Min(playAreaMax, Vector2.Max(playAreaMin, targetTankPosition2D + movementTargetOffset));
             }
+            while (offsetLengthSquared > 150 * 150 || offsetLengthSquared < 50 * 50);
+            MovementTarget = Vector2.Min(playAreaMax, Vector2.Max(playAreaMin, targetTankPosition2D + movementTargetOffset));
+        }
 
-            var offset = new Vector3(MovementTarget.X - currentTankPosition2D.X, 0, MovementTarget.Y - currentTankPosition2D.Y);
-            QuaternionEx.Transform(offset, QuaternionEx.Concatenate(QuaternionEx.Conjugate(currentPose.Orientation), Controller.Tank.BodyLocalOrientation), out var localMovementOffset);
+        var offset = new Vector3(MovementTarget.X - currentTankPosition2D.X, 0, MovementTarget.Y - currentTankPosition2D.Y);
+        QuaternionEx.Transform(offset, QuaternionEx.Concatenate(QuaternionEx.Conjugate(currentPose.Orientation), Controller.Tank.BodyLocalOrientation), out var localMovementOffset);
 
-            var targetHorizontalMovementDirection = new Vector2(localMovementOffset.X, -localMovementOffset.Z);
-            var targetDirectionLength = targetHorizontalMovementDirection.Length();
-            targetHorizontalMovementDirection = targetDirectionLength > 1e-10f ? targetHorizontalMovementDirection / targetDirectionLength : new Vector2(0, 1);
-            var turnWeight = targetHorizontalMovementDirection.Y > 0 ? targetHorizontalMovementDirection.X : targetHorizontalMovementDirection.X > 0 ? 1f : -1f;
-            //Set the leftTrack to 1 at turnWeight >= 0. At turnWeight -1, leftTrack should be -1.
-            var leftTrack = MathF.Min(1f, 2f * turnWeight + 1f);
-            //rightTrack = 1 at turnWeight <= 0, rightTrack = -1 at turnWeight = 1.
-            var rightTrack = MathF.Min(1f, -2f * turnWeight + 1f);
+        var targetHorizontalMovementDirection = new Vector2(localMovementOffset.X, -localMovementOffset.Z);
+        var targetDirectionLength = targetHorizontalMovementDirection.Length();
+        targetHorizontalMovementDirection = targetDirectionLength > 1e-10f ? targetHorizontalMovementDirection / targetDirectionLength : new Vector2(0, 1);
+        var turnWeight = targetHorizontalMovementDirection.Y > 0 ? targetHorizontalMovementDirection.X : targetHorizontalMovementDirection.X > 0 ? 1f : -1f;
+        //Set the leftTrack to 1 at turnWeight >= 0. At turnWeight -1, leftTrack should be -1.
+        var leftTrack = MathF.Min(1f, 2f * turnWeight + 1f);
+        //rightTrack = 1 at turnWeight <= 0, rightTrack = -1 at turnWeight = 1.
+        var rightTrack = MathF.Min(1f, -2f * turnWeight + 1f);
 
-            //If we're close to the target, slow down a bit.
-            var speedMultiplier = Math.Min(targetDirectionLength * 0.05f, 1f);
-            leftTrack *= speedMultiplier;
-            rightTrack *= speedMultiplier;
-            //If we're far away from the target but are pointing in the right direction, zoom.
-            var zoom = targetDirectionLength > 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;
-            var barrelToTarget = targetTankPosition - barrelPosition;
-            var barrelToTargetLength = barrelToTarget.Length();
-            barrelToTarget = barrelToTargetLength > 1e-10f ? barrelToTarget / barrelToTargetLength : new Vector3(0, 1, 0);
-            Controller.UpdateMovementAndAim(simulation, leftTrack, rightTrack, zoom, false, false, barrelToTarget);
-            if (frameIndex > LastShotFrame + 60)
+        //If we're close to the target, slow down a bit.
+        var speedMultiplier = Math.Min(targetDirectionLength * 0.05f, 1f);
+        leftTrack *= speedMultiplier;
+        rightTrack *= speedMultiplier;
+        //If we're far away from the target but are pointing in the right direction, zoom.
+        var zoom = targetDirectionLength > 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[Controller.Tank.Barrel].Pose.Position;
+        var barrelToTarget = targetTankPosition - barrelPosition;
+        var barrelToTargetLength = barrelToTarget.Length();
+        barrelToTarget = barrelToTargetLength > 1e-10f ? barrelToTarget / barrelToTargetLength : new Vector3(0, 1, 0);
+        Controller.UpdateMovementAndAim(simulation, leftTrack, rightTrack, zoom, false, false, barrelToTarget);
+        if (frameIndex > LastShotFrame + 60)
+        {
+            //Are we aiming reasonably close to the target?
+            if (barrelToTargetLength > 1e-10f && barrelToTargetLength < 100)
             {
-                //Are we aiming reasonably close to the target?
-                if (barrelToTargetLength > 1e-10f && barrelToTargetLength < 100)
+                Controller.Tank.ComputeBarrelDirection(simulation, out var barrelDirection);
+                var dot = Vector3.Dot(barrelDirection, barrelToTarget);
+                if (dot > 0.98f)
                 {
-                    Controller.Tank.ComputeBarrelDirection(simulation, out var barrelDirection);
-                    var dot = Vector3.Dot(barrelDirection, barrelToTarget);
-                    if (dot > 0.98f)
-                    {
-                        Controller.Tank.Fire(simulation, bodyProperties);
-                        ++projectileCount;
-                    }
+                    Controller.Tank.Fire(simulation, bodyProperties);
+                    ++projectileCount;
                 }
-                LastShotFrame = frameIndex;
             }
+            LastShotFrame = frameIndex;
         }
-
     }
+
 }
\ No newline at end of file
diff --git a/Demos/Demos/Tanks/Tank.cs b/Demos/Demos/Tanks/Tank.cs
index 984d0d024..326edcd6b 100644
--- a/Demos/Demos/Tanks/Tank.cs
+++ b/Demos/Demos/Tanks/Tank.cs
@@ -8,423 +8,422 @@
 using BepuUtilities.Collections;
 using BepuUtilities.Memory;
 
-namespace Demos.Demos.Tanks
+namespace Demos.Demos.Tanks;
+
+/// <summary>
+/// Set of handles and references to a tank instance.
+/// </summary>
+public struct Tank
 {
     /// <summary>
-    /// Set of handles and references to a tank instance.
+    /// Body handle of the tank's main body.
     /// </summary>
-    public struct Tank
-    {
-        /// <summary>
-        /// Body handle of the tank's main body.
-        /// </summary>
-        public BodyHandle Body;
-        /// <summary>
-        /// Body handle of the tank's turret.
-        /// </summary>
-        public BodyHandle Turret;
-        /// <summary>
-        /// Body handle of the tank's barrel.
-        /// </summary>
-        public BodyHandle Barrel;
-        /// <summary>
-        /// Constraint handle of the turret swivel servo.
-        /// </summary>
-        public ConstraintHandle TurretServo;
-        /// <summary>
-        /// Constraint handle of the barrel pitch servo.
-        /// </summary>
-        public ConstraintHandle BarrelServo;
-        /// <summary>
-        /// List of all wheel body handles associated with the tank.
-        /// </summary>
-        public Buffer<BodyHandle> WheelHandles;
-        /// <summary>
-        /// List of all constraint handles associated with the tank. Includes motors.
-        /// </summary>
-        public Buffer<ConstraintHandle> Constraints;
-        /// <summary>
-        /// List of constraint handles associated with the left tread's drive motors.
-        /// </summary>
-        public Buffer<ConstraintHandle> LeftMotors;
-        /// <summary>
-        /// List of constraint handles associated with the right tread's drive motors.
-        /// </summary>
-        public Buffer<ConstraintHandle> RightMotors;
+    public BodyHandle Body;
+    /// <summary>
+    /// Body handle of the tank's turret.
+    /// </summary>
+    public BodyHandle Turret;
+    /// <summary>
+    /// Body handle of the tank's barrel.
+    /// </summary>
+    public BodyHandle Barrel;
+    /// <summary>
+    /// Constraint handle of the turret swivel servo.
+    /// </summary>
+    public ConstraintHandle TurretServo;
+    /// <summary>
+    /// Constraint handle of the barrel pitch servo.
+    /// </summary>
+    public ConstraintHandle BarrelServo;
+    /// <summary>
+    /// List of all wheel body handles associated with the tank.
+    /// </summary>
+    public Buffer<BodyHandle> WheelHandles;
+    /// <summary>
+    /// List of all constraint handles associated with the tank. Includes motors.
+    /// </summary>
+    public Buffer<ConstraintHandle> Constraints;
+    /// <summary>
+    /// List of constraint handles associated with the left tread's drive motors.
+    /// </summary>
+    public Buffer<ConstraintHandle> LeftMotors;
+    /// <summary>
+    /// List of constraint handles associated with the right tread's drive motors.
+    /// </summary>
+    public Buffer<ConstraintHandle> RightMotors;
 
-        /// <summary>
-        /// Transforms directions from body local space to turret basis local space. Used for computing aiming angles.
-        /// </summary>
-        Quaternion FromBodyLocalToTurretBasisLocal;
-        /// <summary>
-        /// Orientation of the body in the tank's local space.
-        /// </summary>
-        public Quaternion BodyLocalOrientation;
-        /// <summary>
-        /// Location in the barrel body's local space where projectiles should be created.
-        /// </summary>
-        Vector3 BarrelLocalProjectileSpawn;
-        /// <summary>
-        /// Direction in the barrel body's local space along which projectiles should be fired.
-        /// </summary>
-        Vector3 BarrelLocalDirection;
-        /// <summary>
-        /// Speed of projectiles fired by the tank.
-        /// </summary>
-        float ProjectileSpeed;
-        /// <summary>
-        /// Inertia of projectiles fired by the tank.
-        /// </summary>
-        BodyInertia ProjectileInertia;
-        /// <summary>
-        /// Shape of the projectiles fired by the tank.
-        /// </summary>
-        TypedIndex ProjectileShape;
+    /// <summary>
+    /// Transforms directions from body local space to turret basis local space. Used for computing aiming angles.
+    /// </summary>
+    Quaternion FromBodyLocalToTurretBasisLocal;
+    /// <summary>
+    /// Orientation of the body in the tank's local space.
+    /// </summary>
+    public Quaternion BodyLocalOrientation;
+    /// <summary>
+    /// Location in the barrel body's local space where projectiles should be created.
+    /// </summary>
+    Vector3 BarrelLocalProjectileSpawn;
+    /// <summary>
+    /// Direction in the barrel body's local space along which projectiles should be fired.
+    /// </summary>
+    Vector3 BarrelLocalDirection;
+    /// <summary>
+    /// Speed of projectiles fired by the tank.
+    /// </summary>
+    float ProjectileSpeed;
+    /// <summary>
+    /// Inertia of projectiles fired by the tank.
+    /// </summary>
+    BodyInertia ProjectileInertia;
+    /// <summary>
+    /// Shape of the projectiles fired by the tank.
+    /// </summary>
+    TypedIndex ProjectileShape;
 
-        //We cache the motor descriptions so we don't need to recompute the bases.
-        TwistServo BarrelServoDescription;
-        TwistServo TurretServoDescription;
+    //We cache the motor descriptions so we don't need to recompute the bases.
+    TwistServo BarrelServoDescription;
+    TwistServo TurretServoDescription;
 
-        public void SetSpeed(Simulation simulation, Buffer<ConstraintHandle> motors, float speed, float maximumForce)
+    public void SetSpeed(Simulation simulation, Buffer<ConstraintHandle> motors, float speed, float maximumForce)
+    {
+        //This sets all properties of a motor at once; it's possible to create a custom description that only assigns a subset of properties if you find this to be somehow expensive.
+        var motorDescription = new AngularAxisMotor
         {
-            //This sets all properties of a motor at once; it's possible to create a custom description that only assigns a subset of properties if you find this to be somehow expensive.
-            var motorDescription = new AngularAxisMotor
-            {
-                //Assuming the wheels are cylinders oriented in the obvious way.
-                LocalAxisA = new Vector3(0, -1, 0),
-                Settings = new MotorSettings(maximumForce, 1e-6f),
-                TargetVelocity = speed
-            };
-            for (int i = 0; i < motors.Length; ++i)
-            {
-                simulation.Solver.ApplyDescription(motors[i], ref motorDescription);
-            }
+            //Assuming the wheels are cylinders oriented in the obvious way.
+            LocalAxisA = new Vector3(0, -1, 0),
+            Settings = new MotorSettings(maximumForce, 1e-6f),
+            TargetVelocity = speed
+        };
+        for (int i = 0; i < motors.Length; ++i)
+        {
+            simulation.Solver.ApplyDescription(motors[i], motorDescription);
         }
+    }
 
 
-        /// <summary>
-        /// Computes the swivel and pitch angles required to aim in a given direction based on the tank's current pose.
-        /// </summary>
-        /// <param name="simulation">Simulation containing the tank.</param>
-        /// <param name="aimDirection">Direction to aim in.</param>
-        /// <returns>Swivel and pitch angles to point in the given direction.</returns>
-        public readonly (float targetSwivelAngle, float targetPitchAngle) ComputeTurretAngles(Simulation simulation, in 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.
-            //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);
-            //-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.
-            //We'll compute the swivel angle first.
-            QuaternionEx.TransformWithoutOverlap(aimDirection, toTurretBasis, out var aimDirectionInTurretBasis);
-            var targetSwivelAngle = MathF.Atan2(aimDirectionInTurretBasis.X, -aimDirectionInTurretBasis.Z);
+    /// <summary>
+    /// Computes the swivel and pitch angles required to aim in a given direction based on the tank's current pose.
+    /// </summary>
+    /// <param name="simulation">Simulation containing the tank.</param>
+    /// <param name="aimDirection">Direction to aim in.</param>
+    /// <returns>Swivel and pitch angles to point in the given direction.</returns>
+    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.
+        //aimDirection * inverse(body.Pose.Orientation) * Tank.LocalBodyPose.Orientation * inverse(Tank.TurretBasis)
+        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.
+        //We'll compute the swivel angle first.
+        QuaternionEx.TransformWithoutOverlap(aimDirection, toTurretBasis, out var aimDirectionInTurretBasis);
+        var targetSwivelAngle = MathF.Atan2(aimDirectionInTurretBasis.X, -aimDirectionInTurretBasis.Z);
 
-            //Barrel pitching is measured against the +Y axis and an axis created from the target swivel angle.
-            var targetPitchAngle = MathF.Asin(MathF.Max(-1f, MathF.Min(1f, -aimDirectionInTurretBasis.Y)));
-            return (targetSwivelAngle, targetPitchAngle);
-        }
+        //Barrel pitching is measured against the +Y axis and an axis created from the target swivel angle.
+        var targetPitchAngle = MathF.Asin(MathF.Max(-1f, MathF.Min(1f, -aimDirectionInTurretBasis.Y)));
+        return (targetSwivelAngle, targetPitchAngle);
+    }
 
-        /// <summary>
-        /// Applies a target swivel and pitch angle to the turret's servos.
-        /// </summary>
-        /// <param name="simulation">Simulation containing the tank.</param>
-        /// <param name="targetSwivelAngle">Target swivel angle of the turret.</param>
-        /// <param name="targetPitchAngle">Target pitch angle of the barrel.</param>
-        public void SetAim(Simulation simulation, float targetSwivelAngle, float targetPitchAngle)
-        {
-            var turretDescription = TurretServoDescription;
-            turretDescription.TargetAngle = targetSwivelAngle;
-            simulation.Solver.ApplyDescription(TurretServo, ref turretDescription);
-            var barrelDescription = BarrelServoDescription;
-            barrelDescription.TargetAngle = targetPitchAngle;
-            simulation.Solver.ApplyDescription(BarrelServo, ref barrelDescription);
+    /// <summary>
+    /// Applies a target swivel and pitch angle to the turret's servos.
+    /// </summary>
+    /// <param name="simulation">Simulation containing the tank.</param>
+    /// <param name="targetSwivelAngle">Target swivel angle of the turret.</param>
+    /// <param name="targetPitchAngle">Target pitch angle of the barrel.</param>
+    public void SetAim(Simulation simulation, float targetSwivelAngle, float targetPitchAngle)
+    {
+        var turretDescription = TurretServoDescription;
+        turretDescription.TargetAngle = targetSwivelAngle;
+        simulation.Solver.ApplyDescription(TurretServo, turretDescription);
+        var barrelDescription = BarrelServoDescription;
+        barrelDescription.TargetAngle = targetPitchAngle;
+        simulation.Solver.ApplyDescription(BarrelServo, barrelDescription);
 
-        }
+    }
 
-        /// <summary>
-        /// Computes the direction along which the barrel points.
-        /// </summary>
-        /// <param name="simulation">Simulation containing the tank.</param>
-        /// <param name="barrelDirection">Direction in which the barrel points.</param>
-        public readonly void ComputeBarrelDirection(Simulation simulation, out Vector3 barrelDirection)
-        {
-            QuaternionEx.Transform(BarrelLocalDirection, simulation.Bodies.GetBodyReference(Barrel).Pose.Orientation, out barrelDirection);
-        }
+    /// <summary>
+    /// Computes the direction along which the barrel points.
+    /// </summary>
+    /// <param name="simulation">Simulation containing the tank.</param>
+    /// <param name="barrelDirection">Direction in which the barrel points.</param>
+    public readonly void ComputeBarrelDirection(Simulation simulation, out Vector3 barrelDirection)
+    {
+        QuaternionEx.Transform(BarrelLocalDirection, simulation.Bodies[Barrel].Pose.Orientation, out barrelDirection);
+    }
 
-        /// <summary>
-        /// Fires a projectile.
-        /// </summary>
-        /// <param name="simulation">Simulation that contains the tank.</param>
-        /// <param name="bodyProperties">Body properties to allocate the projectile's properties in.</param>
-        /// <returns>Handle of the created projectile body.</returns>
-        public BodyHandle Fire(Simulation simulation, CollidableProperty<TankDemoBodyProperties> bodyProperties)
-        {
-            var barrel = simulation.Bodies.GetBodyReference(Barrel);
-            ref var barrelPose = ref barrel.Pose;
-            RigidPose.Transform(BarrelLocalProjectileSpawn, barrelPose, out var projectileSpawn);
-            QuaternionEx.Transform(BarrelLocalDirection, barrelPose.Orientation, out var barrelDirection);
-            var projectileHandle = simulation.Bodies.Add(BodyDescription.CreateDynamic(projectileSpawn, new BodyVelocity(barrelDirection * ProjectileSpeed + barrel.Velocity.Linear), ProjectileInertia,
-                //The projectile moves pretty fast, so we'll use continuous collision detection.
-                new CollidableDescription(ProjectileShape, 0.1f, ContinuousDetectionSettings.Continuous(1e-3f, 1e-3f)), new BodyActivityDescription(0.01f)));
-            ref var projectileProperties = ref bodyProperties.Allocate(projectileHandle);
-            projectileProperties.Friction = 1f;
-            //Prevent the projectile from colliding with the firing tank.
-            projectileProperties.Filter = new SubgroupCollisionFilter(Body.Value);
-            projectileProperties.Filter.CollidableSubgroups = 0;
-            projectileProperties.Filter.SubgroupMembership = 0;
-            projectileProperties.Projectile = true;
+    /// <summary>
+    /// Fires a projectile.
+    /// </summary>
+    /// <param name="simulation">Simulation that contains the tank.</param>
+    /// <param name="bodyProperties">Body properties to allocate the projectile's properties in.</param>
+    /// <returns>Handle of the created projectile body.</returns>
+    public BodyHandle Fire(Simulation simulation, CollidableProperty<TankDemoBodyProperties> bodyProperties)
+    {
+        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);
+        var projectileHandle = simulation.Bodies.Add(BodyDescription.CreateDynamic(projectileSpawn, barrelDirection * ProjectileSpeed + barrel.Velocity.Linear, ProjectileInertia,
+            //The projectile moves pretty fast, so we'll use continuous collision detection.
+            new(ProjectileShape, 0.1f, ContinuousDetection.Continuous(1e-3f, 1e-3f)), 0.01f));
+        ref var projectileProperties = ref bodyProperties.Allocate(projectileHandle);
+        projectileProperties.Friction = 1f;
+        //Prevent the projectile from colliding with the firing tank.
+        projectileProperties.Filter = new SubgroupCollisionFilter(Body.Value);
+        projectileProperties.Filter.CollidableSubgroups = 0;
+        projectileProperties.Filter.SubgroupMembership = 0;
+        projectileProperties.Projectile = true;
 
-            barrel.Awake = true;
-            barrel.ApplyLinearImpulse(barrelDirection * -ProjectileSpeed / ProjectileInertia.InverseMass);
-            return projectileHandle;
-        }
+        barrel.Awake = true;
+        barrel.ApplyLinearImpulse(barrelDirection * -ProjectileSpeed / ProjectileInertia.InverseMass);
+        return projectileHandle;
+    }
+
+    static BodyHandle CreateWheel(Simulation simulation, CollidableProperty<TankDemoBodyProperties> properties, in RigidPose tankPose, in RigidPose bodyLocalPose,
+        TypedIndex wheelShape, BodyInertia wheelInertia, float wheelFriction, BodyHandle bodyHandle, ref SubgroupCollisionFilter bodyFilter, Vector3 bodyToWheelSuspension, float suspensionLength,
+        in SpringSettings suspensionSettings, Quaternion localWheelOrientation,
+        ref QuickList<BodyHandle> wheelHandles, ref QuickList<ConstraintHandle> constraints, ref QuickList<ConstraintHandle> motors)
+    {
+        RigidPose wheelPose;
+        QuaternionEx.TransformUnitX(localWheelOrientation, out var suspensionDirection);
+        RigidPose.Transform(bodyToWheelSuspension + suspensionDirection * suspensionLength, tankPose, out wheelPose.Position);
+        QuaternionEx.ConcatenateWithoutOverlap(localWheelOrientation, tankPose.Orientation, out wheelPose.Orientation);
+
+        var wheelHandle = simulation.Bodies.Add(BodyDescription.CreateDynamic(wheelPose, wheelInertia, wheelShape, 0.01f));
+        wheelHandles.AllocateUnsafely() = wheelHandle;
 
-        static BodyHandle CreateWheel(Simulation simulation, CollidableProperty<TankDemoBodyProperties> 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,
-            ref QuickList<BodyHandle> wheelHandles, ref QuickList<ConstraintHandle> constraints, ref QuickList<ConstraintHandle> motors)
+        //We need a LinearAxisServo to act as the suspension spring, pushing the wheel down.
+        constraints.AllocateUnsafely() = simulation.Solver.Add(bodyHandle, wheelHandle, new LinearAxisServo
+        {
+            LocalPlaneNormal = suspensionDirection,
+            TargetOffset = suspensionLength,
+            LocalOffsetA = bodyToWheelSuspension,
+            LocalOffsetB = default,
+            ServoSettings = ServoSettings.Default,
+            SpringSettings = suspensionSettings
+        });
+        //A PointOnLineServo keeps the wheel on a fixed track. Note that it does not constrain the angular behavior of the wheel at all.
+        constraints.AllocateUnsafely() = simulation.Solver.Add(bodyHandle, wheelHandle, new PointOnLineServo
         {
-            RigidPose wheelPose;
-            QuaternionEx.TransformUnitX(localWheelOrientation, out var suspensionDirection);
-            RigidPose.Transform(bodyToWheelSuspension + suspensionDirection * suspensionLength, tankPose, out wheelPose.Position);
-            QuaternionEx.ConcatenateWithoutOverlap(localWheelOrientation, tankPose.Orientation, out wheelPose.Orientation);
+            LocalDirection = suspensionDirection,
+            LocalOffsetA = bodyToWheelSuspension,
+            LocalOffsetB = default,
+            ServoSettings = ServoSettings.Default,
+            SpringSettings = new SpringSettings(30, 1)
+        });
+        //The angular component is handled by a hinge. Note that we only use the angular component of a hinge constraint here- the PointOnLineServo handles the linear degrees of freedom.
+        //We're assuming the wheels will be cylinders. Pretty safe bet. A cylinder rolls around its local Y axis, so the motor will act along that axis.
+        QuaternionEx.TransformUnitY(localWheelOrientation, out var wheelRotationAxis);
+        constraints.AllocateUnsafely() = simulation.Solver.Add(bodyHandle, wheelHandle, new AngularHinge
+        {
+            LocalHingeAxisA = QuaternionEx.Transform(wheelRotationAxis, QuaternionEx.Conjugate(bodyLocalPose.Orientation)),
+            LocalHingeAxisB = new Vector3(0, 1, 0),
+            SpringSettings = new SpringSettings(30, 1)
+        });
+        //We'll need a velocity motor to actually make the tank move.
+        var motorHandle = simulation.Solver.Add(wheelHandle, bodyHandle, new AngularAxisMotor
+        {
+            //(All these are technically set on the fly during the update right now, but a custom constraint description could set only the Settings and TargetVelocity,
+            //leaving the LocalAxisA unchanged, so we'll go ahead and set it to a reasonable value.)
+            LocalAxisA = new Vector3(0, 1, 0),
+            Settings = default,
+            TargetVelocity = default
+        });
+        motors.AllocateUnsafely() = motorHandle;
+        constraints.AllocateUnsafely() = motorHandle;
+        ref var wheelProperties = ref properties.Allocate(wheelHandle);
+        wheelProperties = new TankDemoBodyProperties { Filter = new SubgroupCollisionFilter(bodyHandle.Value, 3), Friction = wheelFriction, TankPart = true };
+        //The wheels don't need to be tested against the body or each other.
+        SubgroupCollisionFilter.DisableCollision(ref wheelProperties.Filter, ref bodyFilter);
+        SubgroupCollisionFilter.DisableCollision(ref wheelProperties.Filter, ref wheelProperties.Filter);
+        return wheelHandle;
+    }
 
-            var wheelHandle = simulation.Bodies.Add(BodyDescription.CreateDynamic(wheelPose, wheelInertia, new CollidableDescription(wheelShape, 0.1f), new BodyActivityDescription(0.01f)));
-            wheelHandles.AllocateUnsafely() = wheelHandle;
+    static ref SubgroupCollisionFilter CreatePart(Simulation simulation, in TankPartDescription part, RigidPose pose, CollidableProperty<TankDemoBodyProperties> properties, out BodyHandle handle)
+    {
+        RigidPose.MultiplyWithoutOverlap(part.Pose, pose, out var bodyPose);
+        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;
+    }
 
-            //We need a LinearAxisServo to act as the suspension spring, pushing the wheel down.
-            constraints.AllocateUnsafely() = simulation.Solver.Add(bodyHandle, wheelHandle, new LinearAxisServo
-            {
-                LocalPlaneNormal = suspensionDirection,
-                TargetOffset = suspensionLength,
-                LocalOffsetA = bodyToWheelSuspension,
-                LocalOffsetB = default,
-                ServoSettings = ServoSettings.Default,
-                SpringSettings = suspensionSettings
-            });
-            //A PointOnLineServo keeps the wheel on a fixed track. Note that it does not constrain the angular behavior of the wheel at all.
-            constraints.AllocateUnsafely() = simulation.Solver.Add(bodyHandle, wheelHandle, new PointOnLineServo
+    /// <summary>
+    /// Creates a tank from a provided tank description.
+    /// </summary>
+    /// <param name="simulation">Simulation to add the tank to.</param>
+    /// <param name="properties">Property set to store per-body information into.</param>
+    /// <param name="pool">Buffer pool to allocate tank resources from.</param>
+    /// <param name="pose">Pose of the tank.</param>
+    /// <param name="description">Description of the tank.</param>
+    /// <returns>Tank instance containing references to the simulation tank parts.</returns>
+    public static Tank Create(Simulation simulation, CollidableProperty<TankDemoBodyProperties> properties, BufferPool pool, in RigidPose pose, in TankDescription description)
+    {
+        var wheelHandles = new QuickList<BodyHandle>(description.WheelCountPerTread * 2, pool);
+        var constraints = new QuickList<ConstraintHandle>(description.WheelCountPerTread * 2 * 6 + 4, pool);
+        var leftMotors = new QuickList<ConstraintHandle>(description.WheelCountPerTread, pool);
+        var rightMotors = new QuickList<ConstraintHandle>(description.WheelCountPerTread, pool);
+        Tank tank;
+        ref var bodyFilter = ref CreatePart(simulation, description.Body, pose, properties, out tank.Body);
+        ref var turretFilter = ref CreatePart(simulation, description.Turret, pose, properties, out tank.Turret);
+        ref var barrelFilter = ref CreatePart(simulation, description.Barrel, pose, properties, out tank.Barrel);
+        //Use the tank's body handle as the group id for collision filters.
+        bodyFilter = new SubgroupCollisionFilter(tank.Body.Value, 0);
+        turretFilter = new SubgroupCollisionFilter(tank.Body.Value, 1);
+        barrelFilter = new SubgroupCollisionFilter(tank.Body.Value, 2);
+        SubgroupCollisionFilter.DisableCollision(ref bodyFilter, ref turretFilter);
+        SubgroupCollisionFilter.DisableCollision(ref turretFilter, ref barrelFilter);
+
+        Matrix3x3.CreateFromQuaternion(description.TurretBasis, out var turretBasis);
+
+        //Attach the turret to the body.
+        QuaternionEx.Transform(turretBasis.Y, QuaternionEx.Conjugate(description.Body.Pose.Orientation), out var bodyLocalSwivelAxis);
+        QuaternionEx.Transform(turretBasis.Y, QuaternionEx.Conjugate(description.Turret.Pose.Orientation), out var turretLocalSwivelAxis);
+        RigidPose.TransformByInverse(description.TurretAnchor, description.Body.Pose, out var bodyLocalTurretAnchor);
+        RigidPose.TransformByInverse(description.TurretAnchor, description.Turret.Pose, out var turretLocalTurretAnchor);
+        constraints.AllocateUnsafely() = simulation.Solver.Add(tank.Body, tank.Turret,
+            new Hinge
             {
-                LocalDirection = suspensionDirection,
-                LocalOffsetA = bodyToWheelSuspension,
-                LocalOffsetB = default,
-                ServoSettings = ServoSettings.Default,
+                LocalHingeAxisA = bodyLocalSwivelAxis,
+                LocalHingeAxisB = turretLocalSwivelAxis,
+                LocalOffsetA = bodyLocalTurretAnchor,
+                LocalOffsetB = turretLocalTurretAnchor,
                 SpringSettings = new SpringSettings(30, 1)
             });
-            //The angular component is handled by a hinge. Note that we only use the angular component of a hinge constraint here- the PointOnLineServo handles the linear degrees of freedom.
-            //We're assuming the wheels will be cylinders. Pretty safe bet. A cylinder rolls around its local Y axis, so the motor will act along that axis.
-            QuaternionEx.TransformUnitY(localWheelOrientation, out var wheelRotationAxis);
-            constraints.AllocateUnsafely() = simulation.Solver.Add(bodyHandle, wheelHandle, new AngularHinge
+        //The twist servo might seem like an odd choice to control 1 angular degree of freedom, but servo-like control over 1DOF requires a measurement basis to interpret the target angle.
+        //Hence the apparent complexity.
+        Matrix3x3 turretSwivelBasis;
+        turretSwivelBasis.Z = -turretBasis.Y;
+        turretSwivelBasis.X = -turretBasis.Z;
+        turretSwivelBasis.Y = turretBasis.X;
+        Debug.Assert(turretSwivelBasis.Determinant() > 0.999f && turretSwivelBasis.Determinant() < 1.0001f, "The turret swivel axis and forward axis should be perpendicular and unit length.");
+        QuaternionEx.CreateFromRotationMatrix(turretSwivelBasis, out var turretSwivelBasisQuaternion);
+        QuaternionEx.ConcatenateWithoutOverlap(turretSwivelBasisQuaternion, QuaternionEx.Conjugate(description.Body.Pose.Orientation), out var bodyLocalTurretBasis);
+        QuaternionEx.ConcatenateWithoutOverlap(turretSwivelBasisQuaternion, QuaternionEx.Conjugate(description.Turret.Pose.Orientation), out var turretLocalTurretBasis);
+        tank.TurretServoDescription = new TwistServo
+        {
+            LocalBasisA = bodyLocalTurretBasis,
+            LocalBasisB = turretLocalTurretBasis,
+            SpringSettings = description.TurretSpring,
+            ServoSettings = description.TurretServo
+        };
+        tank.TurretServo = simulation.Solver.Add(tank.Body, tank.Turret, tank.TurretServoDescription);
+        constraints.AllocateUnsafely() = tank.TurretServo;
+
+        //Attach the barrel to the turret.
+        QuaternionEx.Transform(turretBasis.X, QuaternionEx.Conjugate(description.Turret.Pose.Orientation), out var turretLocalPitchAxis);
+        QuaternionEx.Transform(turretBasis.X, QuaternionEx.Conjugate(description.Barrel.Pose.Orientation), out var barrelLocalPitchAxis);
+        RigidPose.TransformByInverse(description.BarrelAnchor, description.Turret.Pose, out var turretLocalBarrelAnchor);
+        RigidPose.TransformByInverse(description.BarrelAnchor, description.Barrel.Pose, out var barrelLocalBarrelAnchor);
+        constraints.AllocateUnsafely() = simulation.Solver.Add(tank.Turret, tank.Barrel,
+            new Hinge
             {
-                LocalHingeAxisA = QuaternionEx.Transform(wheelRotationAxis, QuaternionEx.Conjugate(bodyLocalPose.Orientation)),
-                LocalHingeAxisB = new Vector3(0, 1, 0),
+                LocalHingeAxisA = turretLocalPitchAxis,
+                LocalHingeAxisB = barrelLocalPitchAxis,
+                LocalOffsetA = turretLocalBarrelAnchor,
+                LocalOffsetB = barrelLocalBarrelAnchor,
                 SpringSettings = new SpringSettings(30, 1)
             });
-            //We'll need a velocity motor to actually make the tank move.
-            var motorHandle = simulation.Solver.Add(wheelHandle, bodyHandle, new AngularAxisMotor
-            {
-                //(All these are technically set on the fly during the update right now, but a custom constraint description could set only the Settings and TargetVelocity,
-                //leaving the LocalAxisA unchanged, so we'll go ahead and set it to a reasonable value.)
-                LocalAxisA = new Vector3(0, 1, 0),
-                Settings = default,
-                TargetVelocity = default
-            });
-            motors.AllocateUnsafely() = motorHandle;
-            constraints.AllocateUnsafely() = motorHandle;
-            ref var wheelProperties = ref properties.Allocate(wheelHandle);
-            wheelProperties = new TankDemoBodyProperties { Filter = new SubgroupCollisionFilter(bodyHandle.Value, 3), Friction = wheelFriction, TankPart = true };
-            //The wheels don't need to be tested against the body or each other.
-            SubgroupCollisionFilter.DisableCollision(ref wheelProperties.Filter, ref bodyFilter);
-            SubgroupCollisionFilter.DisableCollision(ref wheelProperties.Filter, ref wheelProperties.Filter);
-            return wheelHandle;
-        }
-
-        static ref SubgroupCollisionFilter CreatePart(Simulation simulation, in TankPartDescription part, RigidPose pose, CollidableProperty<TankDemoBodyProperties> properties, out BodyHandle handle)
+        //The twist servo might seem like an odd choice to control 1 angular degree of freedom, but servo-like control over 1DOF requires a measurement basis to interpret the target angle.
+        //Hence the apparent complexity.
+        Matrix3x3 barrelPitchBasis;
+        barrelPitchBasis.Z = -turretBasis.X;
+        barrelPitchBasis.X = -turretBasis.Z;
+        barrelPitchBasis.Y = -turretBasis.Y;
+        Debug.Assert(barrelPitchBasis.Determinant() > 0.999f && barrelPitchBasis.Determinant() < 1.0001f, "The barrel axis and forward axis should be perpendicular and unit length.");
+        QuaternionEx.CreateFromRotationMatrix(barrelPitchBasis, out var barrelPitchBasisQuaternion);
+        QuaternionEx.ConcatenateWithoutOverlap(barrelPitchBasisQuaternion, QuaternionEx.Conjugate(description.Turret.Pose.Orientation), out var turretLocalBarrelBasis);
+        QuaternionEx.ConcatenateWithoutOverlap(barrelPitchBasisQuaternion, QuaternionEx.Conjugate(description.Barrel.Pose.Orientation), out var barrelLocalBarrelBasis);
+        tank.BarrelServoDescription = new TwistServo
         {
-            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)));
-            ref var partProperties = ref properties.Allocate(handle);
-            partProperties = new TankDemoBodyProperties { Friction = part.Friction, TankPart = true };
-            return ref partProperties.Filter;
-        }
+            LocalBasisA = turretLocalBarrelBasis,
+            LocalBasisB = barrelLocalBarrelBasis,
+            SpringSettings = description.BarrelSpring,
+            ServoSettings = description.BarrelServo
+        };
+        tank.BarrelServo = simulation.Solver.Add(tank.Turret, tank.Barrel, tank.BarrelServoDescription);
+        constraints.AllocateUnsafely() = tank.BarrelServo;
 
-        /// <summary>
-        /// Creates a tank from a provided tank description.
-        /// </summary>
-        /// <param name="simulation">Simulation to add the tank to.</param>
-        /// <param name="properties">Property set to store per-body information into.</param>
-        /// <param name="pool">Buffer pool to allocate tank resources from.</param>
-        /// <param name="pose">Pose of the tank.</param>
-        /// <param name="description">Description of the tank.</param>
-        /// <returns>Tank instance containing references to the simulation tank parts.</returns>
-        public static Tank Create(Simulation simulation, CollidableProperty<TankDemoBodyProperties> properties, BufferPool pool, in RigidPose pose, in TankDescription description)
+        QuaternionEx.TransformUnitY(description.WheelOrientation, out var wheelAxis);
+        QuaternionEx.TransformUnitZ(description.WheelOrientation, out var treadDirection);
+        var treadStart = description.TreadSpacing * (description.WheelCountPerTread - 1) * -0.5f;
+        BodyHandle previousLeftWheelHandle = default, previousRightWheelHandle = default;
+        for (int i = 0; i < description.WheelCountPerTread; ++i)
         {
-            var wheelHandles = new QuickList<BodyHandle>(description.WheelCountPerTread * 2, pool);
-            var constraints = new QuickList<ConstraintHandle>(description.WheelCountPerTread * 2 * 6 + 4, pool);
-            var leftMotors = new QuickList<ConstraintHandle>(description.WheelCountPerTread, pool);
-            var rightMotors = new QuickList<ConstraintHandle>(description.WheelCountPerTread, pool);
-            Tank tank;
-            ref var bodyFilter = ref CreatePart(simulation, description.Body, pose, properties, out tank.Body);
-            ref var turretFilter = ref CreatePart(simulation, description.Turret, pose, properties, out tank.Turret);
-            ref var barrelFilter = ref CreatePart(simulation, description.Barrel, pose, properties, out tank.Barrel);
-            //Use the tank's body handle as the group id for collision filters.
-            bodyFilter = new SubgroupCollisionFilter(tank.Body.Value, 0);
-            turretFilter = new SubgroupCollisionFilter(tank.Body.Value, 1);
-            barrelFilter = new SubgroupCollisionFilter(tank.Body.Value, 2);
-            SubgroupCollisionFilter.DisableCollision(ref bodyFilter, ref turretFilter);
-            SubgroupCollisionFilter.DisableCollision(ref turretFilter, ref barrelFilter);
-
-            Matrix3x3.CreateFromQuaternion(description.TurretBasis, out var turretBasis);
+            var wheelOffsetFromTread = treadDirection * (treadStart + i * description.TreadSpacing);
+            var rightWheelHandle = CreateWheel(simulation, properties, pose, description.Body.Pose,
+                description.WheelShape, description.WheelInertia, description.WheelFriction, tank.Body, ref properties[tank.Body].Filter,
+                description.RightTreadOffset + wheelOffsetFromTread - description.Body.Pose.Position,
+                description.SuspensionLength, description.SuspensionSettings, description.WheelOrientation,
+                ref wheelHandles, ref constraints, ref rightMotors);
+            var leftWheelHandle = CreateWheel(simulation, properties, pose, description.Body.Pose,
+                description.WheelShape, description.WheelInertia, description.WheelFriction, tank.Body, ref properties[tank.Body].Filter,
+                description.LeftTreadOffset + wheelOffsetFromTread - description.Body.Pose.Position,
+                description.SuspensionLength, description.SuspensionSettings, description.WheelOrientation,
+                ref wheelHandles, ref constraints, ref leftMotors);
 
-            //Attach the turret to the body.
-            QuaternionEx.Transform(turretBasis.Y, QuaternionEx.Conjugate(description.Body.Pose.Orientation), out var bodyLocalSwivelAxis);
-            QuaternionEx.Transform(turretBasis.Y, QuaternionEx.Conjugate(description.Turret.Pose.Orientation), out var turretLocalSwivelAxis);
-            RigidPose.TransformByInverse(description.TurretAnchor, description.Body.Pose, out var bodyLocalTurretAnchor);
-            RigidPose.TransformByInverse(description.TurretAnchor, description.Turret.Pose, out var turretLocalTurretAnchor);
-            constraints.AllocateUnsafely() = simulation.Solver.Add(tank.Body, tank.Turret,
-                new Hinge
-                {
-                    LocalHingeAxisA = bodyLocalSwivelAxis,
-                    LocalHingeAxisB = turretLocalSwivelAxis,
-                    LocalOffsetA = bodyLocalTurretAnchor,
-                    LocalOffsetB = turretLocalTurretAnchor,
-                    SpringSettings = new SpringSettings(30, 1)
-                });
-            //The twist servo might seem like an odd choice to control 1 angular degree of freedom, but servo-like control over 1DOF requires a measurement basis to interpret the target angle.
-            //Hence the apparent complexity.
-            Matrix3x3 turretSwivelBasis;
-            turretSwivelBasis.Z = -turretBasis.Y;
-            turretSwivelBasis.X = -turretBasis.Z;
-            turretSwivelBasis.Y = turretBasis.X;
-            Debug.Assert(turretSwivelBasis.Determinant() > 0.999f && turretSwivelBasis.Determinant() < 1.0001f, "The turret swivel axis and forward axis should be perpendicular and unit length.");
-            QuaternionEx.CreateFromRotationMatrix(turretSwivelBasis, out var turretSwivelBasisQuaternion);
-            QuaternionEx.ConcatenateWithoutOverlap(turretSwivelBasisQuaternion, QuaternionEx.Conjugate(description.Body.Pose.Orientation), out var bodyLocalTurretBasis);
-            QuaternionEx.ConcatenateWithoutOverlap(turretSwivelBasisQuaternion, QuaternionEx.Conjugate(description.Turret.Pose.Orientation), out var turretLocalTurretBasis);
-            tank.TurretServoDescription = new TwistServo
+            if (i >= 1)
             {
-                LocalBasisA = bodyLocalTurretBasis,
-                LocalBasisB = turretLocalTurretBasis,
-                SpringSettings = description.TurretSpring,
-                ServoSettings = description.TurretServo
-            };
-            tank.TurretServo = simulation.Solver.Add(tank.Body, tank.Turret, tank.TurretServoDescription);
-            constraints.AllocateUnsafely() = tank.TurretServo;
-
-            //Attach the barrel to the turret.
-            QuaternionEx.Transform(turretBasis.X, QuaternionEx.Conjugate(description.Turret.Pose.Orientation), out var turretLocalPitchAxis);
-            QuaternionEx.Transform(turretBasis.X, QuaternionEx.Conjugate(description.Barrel.Pose.Orientation), out var barrelLocalPitchAxis);
-            RigidPose.TransformByInverse(description.BarrelAnchor, description.Turret.Pose, out var turretLocalBarrelAnchor);
-            RigidPose.TransformByInverse(description.BarrelAnchor, description.Barrel.Pose, out var barrelLocalBarrelAnchor);
-            constraints.AllocateUnsafely() = simulation.Solver.Add(tank.Turret, tank.Barrel,
-                new Hinge
-                {
-                    LocalHingeAxisA = turretLocalPitchAxis,
-                    LocalHingeAxisB = barrelLocalPitchAxis,
-                    LocalOffsetA = turretLocalBarrelAnchor,
-                    LocalOffsetB = barrelLocalBarrelAnchor,
-                    SpringSettings = new SpringSettings(30, 1)
-                });
-            //The twist servo might seem like an odd choice to control 1 angular degree of freedom, but servo-like control over 1DOF requires a measurement basis to interpret the target angle.
-            //Hence the apparent complexity.
-            Matrix3x3 barrelPitchBasis;
-            barrelPitchBasis.Z = -turretBasis.X;
-            barrelPitchBasis.X = -turretBasis.Z;
-            barrelPitchBasis.Y = -turretBasis.Y;
-            Debug.Assert(barrelPitchBasis.Determinant() > 0.999f && barrelPitchBasis.Determinant() < 1.0001f, "The barrel axis and forward axis should be perpendicular and unit length.");
-            QuaternionEx.CreateFromRotationMatrix(barrelPitchBasis, out var barrelPitchBasisQuaternion);
-            QuaternionEx.ConcatenateWithoutOverlap(barrelPitchBasisQuaternion, QuaternionEx.Conjugate(description.Turret.Pose.Orientation), out var turretLocalBarrelBasis);
-            QuaternionEx.ConcatenateWithoutOverlap(barrelPitchBasisQuaternion, QuaternionEx.Conjugate(description.Barrel.Pose.Orientation), out var barrelLocalBarrelBasis);
-            tank.BarrelServoDescription = new TwistServo
-            {
-                LocalBasisA = turretLocalBarrelBasis,
-                LocalBasisB = barrelLocalBarrelBasis,
-                SpringSettings = description.BarrelSpring,
-                ServoSettings = description.BarrelServo
-            };
-            tank.BarrelServo = simulation.Solver.Add(tank.Turret, tank.Barrel, tank.BarrelServoDescription);
-            constraints.AllocateUnsafely() = tank.BarrelServo;
-
-            QuaternionEx.TransformUnitY(description.WheelOrientation, out var wheelAxis);
-            QuaternionEx.TransformUnitZ(description.WheelOrientation, out var treadDirection);
-            var treadStart = description.TreadSpacing * (description.WheelCountPerTread - 1) * -0.5f;
-            BodyHandle previousLeftWheelHandle = default, previousRightWheelHandle = default;
-            for (int i = 0; i < description.WheelCountPerTread; ++i)
-            {
-                var wheelOffsetFromTread = treadDirection * (treadStart + i * description.TreadSpacing);
-                var rightWheelHandle = CreateWheel(simulation, properties, pose, description.Body.Pose,
-                    description.WheelShape, description.WheelInertia, description.WheelFriction, tank.Body, ref properties[tank.Body].Filter,
-                    description.RightTreadOffset + wheelOffsetFromTread - description.Body.Pose.Position,
-                    description.SuspensionLength, description.SuspensionSettings, description.WheelOrientation,
-                    ref wheelHandles, ref constraints, ref rightMotors);
-                var leftWheelHandle = CreateWheel(simulation, properties, pose, description.Body.Pose,
-                    description.WheelShape, description.WheelInertia, description.WheelFriction, tank.Body, ref properties[tank.Body].Filter,
-                    description.LeftTreadOffset + wheelOffsetFromTread - description.Body.Pose.Position,
-                    description.SuspensionLength, description.SuspensionSettings, description.WheelOrientation,
-                    ref wheelHandles, ref constraints, ref leftMotors);
+                //Connect wheels in a tread to each other to distribute the drive forces.
+                //The motor will always just target 0 velocity. The wheel orientations will be allowed to drift from each other.
+                //(If you didn't want to allow drift, you could use an AngularServo or TwistServo. AngularServo constrains all 3 degrees of freedom, but for these purposes, that'd be fine.)
+                var motorDescription = new AngularAxisMotor { LocalAxisA = new Vector3(0, 1, 0), Settings = new MotorSettings(float.MaxValue, 1e-4f) };
+                constraints.AllocateUnsafely() = simulation.Solver.Add(previousLeftWheelHandle, leftWheelHandle, motorDescription);
+                constraints.AllocateUnsafely() = simulation.Solver.Add(previousRightWheelHandle, rightWheelHandle, motorDescription);
+            }
+            previousLeftWheelHandle = leftWheelHandle;
+            previousRightWheelHandle = rightWheelHandle;
 
-                if (i >= 1)
-                {
-                    //Connect wheels in a tread to each other to distribute the drive forces.
-                    //The motor will always just target 0 velocity. The wheel orientations will be allowed to drift from each other.
-                    //(If you didn't want to allow drift, you could use an AngularServo or TwistServo. AngularServo constrains all 3 degrees of freedom, but for these purposes, that'd be fine.)
-                    var motorDescription = new AngularAxisMotor { LocalAxisA = new Vector3(0, 1, 0), Settings = new MotorSettings(float.MaxValue, 1e-4f) };
-                    constraints.AllocateUnsafely() = simulation.Solver.Add(previousLeftWheelHandle, leftWheelHandle, ref motorDescription);
-                    constraints.AllocateUnsafely() = simulation.Solver.Add(previousRightWheelHandle, rightWheelHandle, ref motorDescription);
-                }
-                previousLeftWheelHandle = leftWheelHandle;
-                previousRightWheelHandle = rightWheelHandle;
+        }
 
-            }
+        tank.WheelHandles = wheelHandles.Span.Slice(wheelHandles.Count);
+        tank.Constraints = constraints.Span.Slice(constraints.Count);
+        tank.LeftMotors = leftMotors.Span.Slice(leftMotors.Count);
+        tank.RightMotors = rightMotors.Span.Slice(rightMotors.Count);
 
-            tank.WheelHandles = wheelHandles.Span.Slice(wheelHandles.Count);
-            tank.Constraints = constraints.Span.Slice(constraints.Count);
-            tank.LeftMotors = leftMotors.Span.Slice(leftMotors.Count);
-            tank.RightMotors = rightMotors.Span.Slice(rightMotors.Count);
+        //To aim, we transform the aim direction into the turret basis. 
+        //aimDirectionInTurretBasis = worldAimDirection * inverse(body.Pose.Orientation) * description.Body.Pose.Orientation * inverse(description.TurretBasis), so we precompute and cache:
+        //FromBodyLocalToTurretBasisLocal = description.Body.Pose.Orientation * inverse(description.TurretBasis)
+        QuaternionEx.ConcatenateWithoutOverlap(description.Body.Pose.Orientation, QuaternionEx.Conjugate(description.TurretBasis), out tank.FromBodyLocalToTurretBasisLocal);
+        tank.BodyLocalOrientation = description.Body.Pose.Orientation;
+        tank.BarrelLocalProjectileSpawn = description.BarrelLocalProjectileSpawn;
+        QuaternionEx.Transform(-turretBasis.Z, QuaternionEx.Conjugate(description.Barrel.Pose.Orientation), out tank.BarrelLocalDirection);
+        tank.ProjectileInertia = description.ProjectileInertia;
+        tank.ProjectileShape = description.ProjectileShape;
+        tank.ProjectileSpeed = description.ProjectileSpeed;
+        return tank;
+    }
 
-            //To aim, we transform the aim direction into the turret basis. 
-            //aimDirectionInTurretBasis = worldAimDirection * inverse(body.Pose.Orientation) * description.Body.Pose.Orientation * inverse(description.TurretBasis), so we precompute and cache:
-            //FromBodyLocalToTurretBasisLocal = description.Body.Pose.Orientation * inverse(description.TurretBasis)
-            QuaternionEx.ConcatenateWithoutOverlap(description.Body.Pose.Orientation, QuaternionEx.Conjugate(description.TurretBasis), out tank.FromBodyLocalToTurretBasisLocal);
-            tank.BodyLocalOrientation = description.Body.Pose.Orientation;
-            tank.BarrelLocalProjectileSpawn = description.BarrelLocalProjectileSpawn;
-            QuaternionEx.Transform(-turretBasis.Z, QuaternionEx.Conjugate(description.Barrel.Pose.Orientation), out tank.BarrelLocalDirection);
-            tank.ProjectileInertia = description.ProjectileInertia;
-            tank.ProjectileShape = description.ProjectileShape;
-            tank.ProjectileSpeed = description.ProjectileSpeed;
-            return tank;
-        }
+    static void ClearBodyProperties(ref TankDemoBodyProperties properties)
+    {
+        //After blowing up, all tank parts will collide with each other, and we should no longer flag the pieces as part of a living tank.
+        properties.Filter = new SubgroupCollisionFilter(properties.Filter.GroupId);
+        properties.TankPart = false;
+    }
 
-        static void ClearBodyProperties(ref TankDemoBodyProperties properties)
+    public void Explode(Simulation simulation, CollidableProperty<TankDemoBodyProperties> 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)
         {
-            //After blowing up, all tank parts will collide with each other, and we should no longer flag the pieces as part of a living tank.
-            properties.Filter = new SubgroupCollisionFilter(properties.Filter.GroupId);
-            properties.TankPart = false;
+            ClearBodyProperties(ref properties[WheelHandles[i]]);
         }
-
-        public void Explode(Simulation simulation, CollidableProperty<TankDemoBodyProperties> properties, BufferPool pool)
+        ClearBodyProperties(ref properties[Body]);
+        ClearBodyProperties(ref properties[Turret]);
+        ClearBodyProperties(ref properties[Barrel]);
+        var turret = simulation.Bodies[Turret];
+        turret.Awake = true;
+        turret.Velocity.Linear += new Vector3(0, 10, 0);
+        for (int i = 0; i < Constraints.Length; ++i)
         {
-            //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)
-            {
-                ClearBodyProperties(ref properties[WheelHandles[i]]);
-            }
-            ClearBodyProperties(ref properties[Body]);
-            ClearBodyProperties(ref properties[Turret]);
-            ClearBodyProperties(ref properties[Barrel]);
-            var turret = simulation.Bodies.GetBodyReference(Turret);
-            turret.Awake = true;
-            turret.Velocity.Linear += new Vector3(0, 10, 0);
-            for (int i =0; i < Constraints.Length; ++i)
-            {
-                simulation.Solver.Remove(Constraints[i]);
-            }
-            pool.Return(ref WheelHandles);
-            pool.Return(ref Constraints);
-            pool.Return(ref LeftMotors);
-            pool.Return(ref RightMotors);
+            simulation.Solver.Remove(Constraints[i]);
         }
-
+        pool.Return(ref WheelHandles);
+        pool.Return(ref Constraints);
+        pool.Return(ref LeftMotors);
+        pool.Return(ref RightMotors);
     }
+
 }
\ No newline at end of file
diff --git a/Demos/Demos/Tanks/TankCallbacks.cs b/Demos/Demos/Tanks/TankCallbacks.cs
index 34860db1f..c351c3a34 100644
--- a/Demos/Demos/Tanks/TankCallbacks.cs
+++ b/Demos/Demos/Tanks/TankCallbacks.cs
@@ -1,176 +1,172 @@
-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
+namespace Demos.Demos.Tanks;
+
+/// <summary>
+/// Stores properties about a body in the tank demo.
+/// </summary>
+public struct TankDemoBodyProperties
 {
     /// <summary>
-    /// Stores properties about a body in the tank demo.
+    /// Controls which collidables the body can collide with.
     /// </summary>
-    public struct TankDemoBodyProperties
-    {
-        /// <summary>
-        /// Controls which collidables the body can collide with.
-        /// </summary>
-        public SubgroupCollisionFilter Filter;
-        /// <summary>
-        /// Friction coefficient to use for the body.
-        /// </summary>
-        public float Friction;
-        /// <summary>
-        /// True if the body is a projectile and should explode on contact.
-        /// </summary>
-        public bool Projectile;
-        /// <summary>
-        /// True if the body is part of a tank.
-        /// </summary>
-        public bool TankPart;
-    }
+    public SubgroupCollisionFilter Filter;
+    /// <summary>
+    /// Friction coefficient to use for the body.
+    /// </summary>
+    public float Friction;
+    /// <summary>
+    /// True if the body is a projectile and should explode on contact.
+    /// </summary>
+    public bool Projectile;
+    /// <summary>
+    /// True if the body is part of a tank.
+    /// </summary>
+    public bool TankPart;
+}
 
-    public struct ProjectileImpact
+public struct ProjectileImpact
+{
+    /// <summary>
+    /// Handle of the projectile body associated with this impact.
+    /// </summary>
+    public BodyHandle ProjectileHandle;
+    /// <summary>
+    /// Handle of the tank body associated with whatever the projectile hit. If the projectile didn't hit a tank, this is -1.
+    /// </summary>
+    public BodyHandle ImpactedTankBodyHandle;
+}
+
+/// <summary>
+/// For the tank demo, we want both wheel-body collision filtering and different friction for wheels versus the tank body.
+/// </summary>
+struct TankCallbacks : INarrowPhaseCallbacks
+{
+    public CollidableProperty<TankDemoBodyProperties> Properties;
+    public SpinLock ProjectileLock;
+    public QuickList<ProjectileImpact> ProjectileImpacts;
+    public void Initialize(Simulation simulation)
     {
-        /// <summary>
-        /// Handle of the projectile body associated with this impact.
-        /// </summary>
-        public BodyHandle ProjectileHandle;
-        /// <summary>
-        /// Handle of the tank body associated with whatever the projectile hit. If the projectile didn't hit a tank, this is -1.
-        /// </summary>
-        public BodyHandle ImpactedTankBodyHandle;
+        Properties.Initialize(simulation);
     }
 
-    /// <summary>
-    /// For the tank demo, we want both wheel-body collision filtering and different friction for wheels versus the tank body.
-    /// </summary>
-    struct TankCallbacks : INarrowPhaseCallbacks
+    [MethodImpl(MethodImplOptions.AggressiveInlining)]
+    public bool AllowContactGeneration(int workerIndex, CollidableReference a, CollidableReference b, ref float speculativeMargin)
     {
-        public CollidableProperty<TankDemoBodyProperties> Properties;
-        public SpinLock ProjectileLock;
-        public QuickList<ProjectileImpact> ProjectileImpacts;
-        public void Initialize(Simulation simulation)
+        //It's impossible for two statics to collide, and pairs are sorted such that bodies always come before statics.
+        if (b.Mobility != CollidableMobility.Static)
         {
-            Properties.Initialize(simulation);
+            return SubgroupCollisionFilter.AllowCollision(Properties[a.BodyHandle].Filter, Properties[b.BodyHandle].Filter);
         }
+        return a.Mobility == CollidableMobility.Dynamic || b.Mobility == CollidableMobility.Dynamic;
+    }
 
-        [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public bool AllowContactGeneration(int workerIndex, CollidableReference a, CollidableReference b)
+    [MethodImpl(MethodImplOptions.AggressiveInlining)]
+    public bool AllowContactGeneration(int workerIndex, CollidablePair pair, int childIndexA, int childIndexB)
+    {
+        //This function is called for children of compounds, triangles in meshes, and similar cases, but we don't perform any child-level filtering in the tank demo.
+        //The top level filter will always run before this function has a chance to, so we don't have to do anything here.
+        return true;
+    }
+
+    [MethodImpl(MethodImplOptions.AggressiveInlining)]
+    void TryAddProjectileImpact(BodyHandle projectileHandle, CollidableReference impactedCollidable)
+    {
+        bool lockTaken = false;
+        ProjectileLock.Enter(ref lockTaken);
+        try
         {
-            //It's impossible for two statics to collide, and pairs are sorted such that bodies always come before statics.
-            if (b.Mobility != CollidableMobility.Static)
+            //Note that we have to protect against redundant adds- a projectile might hit multiple things in the same frame. Wouldn't want it to explode multiple times.
+            for (int i = 0; i < ProjectileImpacts.Count; ++i)
             {
-                return SubgroupCollisionFilter.AllowCollision(Properties[a.BodyHandle].Filter, Properties[b.BodyHandle].Filter);
+                ref var impact = ref ProjectileImpacts[i];
+                //If the projectile has already been handled, ignore it.
+                if (impact.ProjectileHandle.Value == projectileHandle.Value)
+                    return;
             }
-            return a.Mobility == CollidableMobility.Dynamic || b.Mobility == CollidableMobility.Dynamic;
-        }
-
-        [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public bool AllowContactGeneration(int workerIndex, CollidablePair pair, int childIndexA, int childIndexB)
-        {
-            //This function is called for children of compounds, triangles in meshes, and similar cases, but we don't perform any child-level filtering in the tank demo.
-            //The top level filter will always run before this function has a chance to, so we don't have to do anything here.
-            return true;
-        }
-
-        [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        void TryAddProjectileImpact(BodyHandle projectileHandle, CollidableReference impactedCollidable)
-        {
-            bool lockTaken = false;
-            ProjectileLock.Enter(ref lockTaken);
-            try
+            //The exploding projectiles list should have been sized ahead of time to hold all projectiles, so no dynamic allocations should be required.
+            ref var newImpact = ref ProjectileImpacts.AllocateUnsafely();
+            newImpact.ProjectileHandle = projectileHandle;
+            if (impactedCollidable.Mobility != CollidableMobility.Static)
             {
-                //Note that we have to protect against redundant adds- a projectile might hit multiple things in the same frame. Wouldn't want it to explode multiple times.
-                for (int i = 0; i < ProjectileImpacts.Count; ++i)
-                {
-                    ref var impact = ref ProjectileImpacts[i];
-                    //If the projectile has already been handled, ignore it.
-                    if (impact.ProjectileHandle.Value == projectileHandle.Value)
-                        return;
-                }
-                //The exploding projectiles list should have been sized ahead of time to hold all projectiles, so no dynamic allocations should be required.
-                ref var newImpact = ref ProjectileImpacts.AllocateUnsafely();
-                newImpact.ProjectileHandle = projectileHandle;
-                if (impactedCollidable.Mobility != CollidableMobility.Static)
-                {
-                    //The filter's group id is the tank's main body handle. We use that to find the tank (if this body is related to a tank at all).
-                    ref var properties = ref Properties[impactedCollidable.BodyHandle];
-                    newImpact.ImpactedTankBodyHandle = new BodyHandle(properties.TankPart ? properties.Filter.GroupId : -1);
-                }
-                else
-                {
-                    //It hit a static; tank's aren't static.
-                    newImpact.ImpactedTankBodyHandle = new BodyHandle(-1);
-                }
+                //The filter's group id is the tank's main body handle. We use that to find the tank (if this body is related to a tank at all).
+                ref var properties = ref Properties[impactedCollidable.BodyHandle];
+                newImpact.ImpactedTankBodyHandle = new BodyHandle(properties.TankPart ? properties.Filter.GroupId : -1);
             }
-            finally
+            else
             {
-                if (lockTaken)
-                    ProjectileLock.Exit();
+                //It hit a static; tank's aren't static.
+                newImpact.ImpactedTankBodyHandle = new BodyHandle(-1);
             }
         }
+        finally
+        {
+            if (lockTaken)
+                ProjectileLock.Exit();
+        }
+    }
 
-        [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public unsafe bool ConfigureContactManifold<TManifold>(int workerIndex, CollidablePair pair, ref TManifold manifold, out PairMaterialProperties pairMaterial) where TManifold : unmanaged, IContactManifold<TManifold>
+    [MethodImpl(MethodImplOptions.AggressiveInlining)]
+    public bool ConfigureContactManifold<TManifold>(int workerIndex, CollidablePair pair, ref TManifold manifold, out PairMaterialProperties pairMaterial) where TManifold : unmanaged, IContactManifold<TManifold>
+    {
+        //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];
+        pairMaterial.FrictionCoefficient = propertiesA.Friction;
+        if (pair.B.Mobility != CollidableMobility.Static)
         {
-            //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];
-            pairMaterial.FrictionCoefficient = propertiesA.Friction;
-            if (pair.B.Mobility != CollidableMobility.Static)
-            {
-                //If two bodies collide, just average the friction. Other options include min(a, b) or a * b.
-                ref var propertiesB = ref Properties[pair.B.BodyHandle];
-                pairMaterial.FrictionCoefficient = (pairMaterial.FrictionCoefficient + propertiesB.Friction) * 0.5f;
-            }
-            //These are just some nice standard values. Higher maximum velocities can result in more energy being introduced during deep contact.
-            //Finite spring stiffness helps the solver converge to a solution in difficult cases. Try to keep the spring frequency at around half of the timestep frequency or less.
-            pairMaterial.MaximumRecoveryVelocity = 2f;
-            pairMaterial.SpringSettings = new SpringSettings(30, 1);
+            //If two bodies collide, just average the friction. Other options include min(a, b) or a * b.
+            ref var propertiesB = ref Properties[pair.B.BodyHandle];
+            pairMaterial.FrictionCoefficient = (pairMaterial.FrictionCoefficient + propertiesB.Friction) * 0.5f;
+        }
+        //These are just some nice standard values. Higher maximum velocities can result in more energy being introduced during deep contact.
+        //Finite spring stiffness helps the solver converge to a solution in difficult cases. Try to keep the spring frequency at around half of the timestep frequency or less.
+        pairMaterial.MaximumRecoveryVelocity = 2f;
+        pairMaterial.SpringSettings = new SpringSettings(30, 1);
 
-            if (propertiesA.Projectile || (pair.B.Mobility != CollidableMobility.Static && Properties[pair.B.BodyHandle].Projectile))
+        if (propertiesA.Projectile || (pair.B.Mobility != CollidableMobility.Static && Properties[pair.B.BodyHandle].Projectile))
+        {
+            for (int i = 0; i < manifold.Count; ++i)
             {
-                for (int i = 0; i < manifold.Count; ++i)
+                //This probably looks a bit odd. You can't return refs to the this instance in structs, and interfaces can't require static functions...
+                //so we use this redundant construction to get a direct reference to a contact's depth with near zero overhead.
+                //There's a more typical out parameter overload for contact properties too. And there's always the option of using the manifold pointers directly.
+                //Note the use of a nonzero negative threshold: speculative contacts will bring incoming objects to a stop at the surface, but in some cases integrator/numerical issues can mean that they don't quite reach.
+                //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(i) >= -1e-3f)
                 {
-                    //This probably looks a bit odd. You can't return refs to the this instance in structs, and interfaces can't require static functions...
-                    //so we use this redundant construction to get a direct reference to a contact's depth with near zero overhead.
-                    //There's a more typical out parameter overload for contact properties too. And there's always the option of using the manifold pointers directly.
-                    //Note the use of a nonzero negative threshold: speculative contacts will bring incoming objects to a stop at the surface, but in some cases integrator/numerical issues can mean that they don't quite reach.
-                    //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)
+                    //An actual collision was found. 
+                    if (propertiesA.Projectile)
                     {
-                        //An actual collision was found. 
-                        if (propertiesA.Projectile)
-                        {
-                            TryAddProjectileImpact(pair.A.BodyHandle, pair.B);
-                        }
-                        if (pair.B.Mobility != CollidableMobility.Static && Properties[pair.B.BodyHandle].Projectile)
-                        {
-                            //Could technically combine the locks in the case that both bodies are projectiles, but that's not exactly common.
-                            TryAddProjectileImpact(pair.B.BodyHandle, pair.A);
-                        }
-                        break;
+                        TryAddProjectileImpact(pair.A.BodyHandle, pair.B);
                     }
+                    if (pair.B.Mobility != CollidableMobility.Static && Properties[pair.B.BodyHandle].Projectile)
+                    {
+                        //Could technically combine the locks in the case that both bodies are projectiles, but that's not exactly common.
+                        TryAddProjectileImpact(pair.B.BodyHandle, pair.A);
+                    }
+                    break;
                 }
             }
-            return true;
         }
+        return true;
+    }
 
-        [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public unsafe 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()
-        {
-            Properties.Dispose();
-        }
+    public void Dispose()
+    {
+        Properties.Dispose();
     }
 }
\ No newline at end of file
diff --git a/Demos/Demos/Tanks/TankController.cs b/Demos/Demos/Tanks/TankController.cs
index b402606b0..ddc2d0ea4 100644
--- a/Demos/Demos/Tanks/TankController.cs
+++ b/Demos/Demos/Tanks/TankController.cs
@@ -1,86 +1,83 @@
-using System;
-using System.Numerics;
+using System.Numerics;
 using BepuPhysics;
-using BepuPhysics.Collidables;
 
-namespace Demos.Demos.Tanks
+namespace Demos.Demos.Tanks;
+
+/// <summary>
+/// Applies control inputs to a tank instance.
+/// </summary>
+public struct TankController
 {
+    public Tank Tank;
+
+    //While the Tank instance contains references to all the simulation-contained stuff, none of it actually defines how fast or strong the tank is.
+    //We store that here in the controller so it can be modified on the fly.
+    public float Speed;
+    public float Force;
+    public float ZoomMultiplier;
+    public float IdleForce;
+    public float BrakeForce;
+    
+    //Track the previous state to force wakeups if the constraint targets have changed.
+    private float previousLeftTargetSpeed;
+    private float previousLeftForce;
+    private float previousRightTargetSpeed;
+    private float previousRightForce;
+    private float previousTurretSwivel;
+    private float previousBarrelPitch;
+
+    public TankController(Tank tank,
+        float speed, float force, float zoomMultiplier, float idleForce, float brakeForce) : this()
+    {
+        Tank = tank;
+        Speed = speed;
+        Force = force;
+        ZoomMultiplier = zoomMultiplier;
+        IdleForce = idleForce;
+        BrakeForce = brakeForce;
+    }
+
     /// <summary>
-    /// Applies control inputs to a tank instance.
+    /// Updates constraint targets for an input state.
     /// </summary>
-    public struct TankController
+    /// <param name="simulation">Simulation containing the tank.</param>
+    /// <param name="leftTargetSpeedFraction">Target speed fraction of the maximum speed for the left tread.</param>
+    /// <param name="rightTargetSpeedFraction">Target speed fraction of the maximum speed for the right tread.</param>
+    /// <param name="zoom">Whether or not to use the boost mulitplier.</param>
+    /// <param name="brakeLeft">Whether the left tread should brake.</param>
+    /// <param name="brakeRight">Whether the right tread should brake.</param>
+    /// <param name="aimDirection">Direction that the tank's barrel should point.</param>
+    public void UpdateMovementAndAim(Simulation simulation, float leftTargetSpeedFraction, float rightTargetSpeedFraction, bool zoom, bool brakeLeft, bool brakeRight, Vector3 aimDirection)
     {
-        public Tank Tank;
-
-        //While the Tank instance contains references to all the simulation-contained stuff, none of it actually defines how fast or strong the tank is.
-        //We store that here in the controller so it can be modified on the fly.
-        public float Speed;
-        public float Force;
-        public float ZoomMultiplier;
-        public float IdleForce;
-        public float BrakeForce;
-        
-        //Track the previous state to force wakeups if the constraint targets have changed.
-        private float previousLeftTargetSpeed;
-        private float previousLeftForce;
-        private float previousRightTargetSpeed;
-        private float previousRightForce;
-        private float previousTurretSwivel;
-        private float previousBarrelPitch;
-
-        public TankController(Tank tank,
-            float speed, float force, float zoomMultiplier, float idleForce, float brakeForce) : this()
+        var leftTargetSpeed = brakeLeft ? 0 : leftTargetSpeedFraction * Speed;
+        var rightTargetSpeed = brakeRight ? 0 : rightTargetSpeedFraction * Speed;
+        if (zoom)
         {
-            Tank = tank;
-            Speed = speed;
-            Force = force;
-            ZoomMultiplier = zoomMultiplier;
-            IdleForce = idleForce;
-            BrakeForce = brakeForce;
+            leftTargetSpeed *= ZoomMultiplier;
+            rightTargetSpeed *= ZoomMultiplier;
         }
+        var leftForce = brakeLeft ? BrakeForce : leftTargetSpeedFraction == 0 ? IdleForce : Force;
+        var rightForce = brakeRight ? BrakeForce : rightTargetSpeedFraction == 0 ? IdleForce : Force;
 
-        /// <summary>
-        /// Updates constraint targets for an input state.
-        /// </summary>
-        /// <param name="simulation">Simulation containing the tank.</param>
-        /// <param name="leftTargetSpeedFraction">Target speed fraction of the maximum speed for the left tread.</param>
-        /// <param name="rightTargetSpeedFraction">Target speed fraction of the maximum speed for the right tread.</param>
-        /// <param name="zoom">Whether or not to use the boost mulitplier.</param>
-        /// <param name="brakeLeft">Whether the left tread should brake.</param>
-        /// <param name="brakeRight">Whether the right tread should brake.</param>
-        /// <param name="aimDirection">Direction that the tank's barrel should point.</param>
-        public void UpdateMovementAndAim(Simulation simulation, float leftTargetSpeedFraction, float rightTargetSpeedFraction, bool zoom, bool brakeLeft, bool brakeRight, in Vector3 aimDirection)
-        {
-            var leftTargetSpeed = brakeLeft ? 0 : leftTargetSpeedFraction * Speed;
-            var rightTargetSpeed = brakeRight ? 0 : rightTargetSpeedFraction * Speed;
-            if (zoom)
-            {
-                leftTargetSpeed *= ZoomMultiplier;
-                rightTargetSpeed *= ZoomMultiplier;
-            }
-            var leftForce = brakeLeft ? BrakeForce : leftTargetSpeedFraction == 0 ? IdleForce : Force;
-            var rightForce = brakeRight ? BrakeForce : rightTargetSpeedFraction == 0 ? IdleForce : Force;
-
-            var (targetSwivelAngle, targetPitchAngle) = Tank.ComputeTurretAngles(simulation, aimDirection);
+        var (targetSwivelAngle, targetPitchAngle) = Tank.ComputeTurretAngles(simulation, aimDirection);
 
-            if (leftTargetSpeed != previousLeftTargetSpeed || rightTargetSpeed != previousRightTargetSpeed ||
-                leftForce != previousLeftForce || rightForce != previousRightForce ||
-                targetSwivelAngle != previousTurretSwivel || targetPitchAngle != previousBarrelPitch)
-            {
-                //By guarding the constraint modifications behind a state test, we avoid waking up the tank every single frame.
-                //(We could have also used the ApplyDescriptionWithoutWaking function and then explicitly woke the tank up when changes occur.)
-                Tank.SetSpeed(simulation, Tank.LeftMotors, leftTargetSpeed, leftForce);
-                Tank.SetSpeed(simulation, Tank.RightMotors, rightTargetSpeed, rightForce);
-                previousLeftTargetSpeed = leftTargetSpeed;
-                previousRightTargetSpeed = rightTargetSpeed;
-                previousLeftForce = leftForce;
-                previousRightForce = rightForce;
-                Tank.SetAim(simulation, targetSwivelAngle, targetPitchAngle);
-                previousTurretSwivel = targetSwivelAngle;
-                previousBarrelPitch = targetPitchAngle;
-            }
+        if (leftTargetSpeed != previousLeftTargetSpeed || rightTargetSpeed != previousRightTargetSpeed ||
+            leftForce != previousLeftForce || rightForce != previousRightForce ||
+            targetSwivelAngle != previousTurretSwivel || targetPitchAngle != previousBarrelPitch)
+        {
+            //By guarding the constraint modifications behind a state test, we avoid waking up the tank every single frame.
+            //(We could have also used the ApplyDescriptionWithoutWaking function and then explicitly woke the tank up when changes occur.)
+            Tank.SetSpeed(simulation, Tank.LeftMotors, leftTargetSpeed, leftForce);
+            Tank.SetSpeed(simulation, Tank.RightMotors, rightTargetSpeed, rightForce);
+            previousLeftTargetSpeed = leftTargetSpeed;
+            previousRightTargetSpeed = rightTargetSpeed;
+            previousLeftForce = leftForce;
+            previousRightForce = rightForce;
+            Tank.SetAim(simulation, targetSwivelAngle, targetPitchAngle);
+            previousTurretSwivel = targetSwivelAngle;
+            previousBarrelPitch = targetPitchAngle;
+        }
 
 
-        }
     }
 }
\ No newline at end of file
diff --git a/Demos/Demos/Tanks/TankDemo.cs b/Demos/Demos/Tanks/TankDemo.cs
index 5b5655a0d..9be7778a8 100644
--- a/Demos/Demos/Tanks/TankDemo.cs
+++ b/Demos/Demos/Tanks/TankDemo.cs
@@ -12,352 +12,352 @@
 using DemoUtilities;
 using OpenTK.Input;
 
-namespace Demos.Demos.Tanks
+namespace Demos.Demos.Tanks;
+
+public class TankDemo : Demo
 {
-    public class TankDemo : Demo
-    {
-        CollidableProperty<TankDemoBodyProperties> bodyProperties;
-        TankController playerController;
+    CollidableProperty<TankDemoBodyProperties> bodyProperties;
+    TankController playerController;
 
-        QuickList<AITank> aiTanks;
-        Random random;
-        Vector2 playAreaMin, playAreaMax;
+    QuickList<AITank> 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<Explosion> 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<Explosion> 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<TankDemoBodyProperties>();
-            //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());
+        bodyProperties = new CollidableProperty<TankDemoBodyProperties>();
+        //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 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);
+        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);
-            projectileShape.ComputeInertia(0.2f, out var projectileInertia);
-            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 RigidPose(new Vector3(0, 0.85f, 0.4f)), 0.5f, Simulation.Shapes),
-                Barrel = TankPartDescription.Create(0.5f, new Box(0.2f, 0.2f, 3f), new RigidPose(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 RigidPose(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 + (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());
-                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)));
-            }
+        //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),
-                new CollidableDescription(Simulation.Shapes.Add(planeMesh), 0.1f)));
+        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, ThreadDispatcher);
+        Simulation.Statics.Add(new StaticDescription(new Vector3(0, 0, 0), Simulation.Shapes.Add(planeMesh)));
 
-            explosions = new QuickList<Explosion>(32, BufferPool);
+        explosions = new QuickList<Explosion>(32, BufferPool);
 
-            //Create the AI tanks.
-            const int aiTankCount = 100;
-            aiTanks = new QuickList<AITank>(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 = 100;
+        aiTanks = new QuickList<AITank>(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((float)random.NextDouble(), (float)random.NextDouble()) * 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)),
-                            tankDescription), 20, 5, 2, 1, 3.5f),
-                    HitPoints = 5
-                };
-            }
+                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
+            };
         }
+    }
 
-        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<int> 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<TankCallbacks>)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<int> 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<TankCallbacks>)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.GetBodyReference(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.GetBodyReference(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, ref 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), 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);
+        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/Demos/Tanks/TankDescription.cs b/Demos/Demos/Tanks/TankDescription.cs
index f008abf15..f776fab6b 100644
--- a/Demos/Demos/Tanks/TankDescription.cs
+++ b/Demos/Demos/Tanks/TankDescription.cs
@@ -3,114 +3,113 @@
 using BepuPhysics.Collidables;
 using BepuPhysics.Constraints;
 
-namespace Demos.Demos.Tanks
+namespace Demos.Demos.Tanks;
+
+/// <summary>
+/// Describes a tank's construction.
+/// </summary>
+public struct TankDescription
 {
     /// <summary>
-    /// Describes a tank's construction.
-    /// </summary>
-    public struct TankDescription
-    {
-        /// <summary>
-        /// Description of the tank's turret body.
-        /// </summary>
-        public TankPartDescription Turret;
-        /// <summary>
-        /// Description of the tank's barrel body.
-        /// </summary>
-        public TankPartDescription Barrel;
-        /// <summary>
-        /// Description of the tank's main body.
-        /// </summary>
-        public TankPartDescription Body;
-        /// <summary>
-        /// Location of the barrel's anchor in the tank's local space. The barrel will connect to the turret at this location.
-        /// </summary>
-        public Vector3 BarrelAnchor;
-        /// <summary>
-        /// Location of the turret's anchor in the tank's local space. The turret will connect to the main body at this location.
-        /// </summary>
-        public Vector3 TurretAnchor;
-        /// <summary>
-        /// 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.
-        /// </summary>
-        public Quaternion TurretBasis;
-        /// <summary>
-        /// Servo properties for the tank's swivel constraint.
-        /// </summary>
-        public ServoSettings TurretServo;
-        /// <summary>
-        /// Spring properties for the tank's swivel constraint.
-        /// </summary>
-        public SpringSettings TurretSpring;
-        /// <summary>
-        /// Servo properties for the tank's barrel pitching constraint.
-        /// </summary>
-        public ServoSettings BarrelServo;
-        /// <summary>
-        /// Spring properties for the tank's barrel pitching constraint.
-        /// </summary>
-        public SpringSettings BarrelSpring;
+    /// Description of the tank's turret body.
+    /// </summary>
+    public TankPartDescription Turret;
+    /// <summary>
+    /// Description of the tank's barrel body.
+    /// </summary>
+    public TankPartDescription Barrel;
+    /// <summary>
+    /// Description of the tank's main body.
+    /// </summary>
+    public TankPartDescription Body;
+    /// <summary>
+    /// Location of the barrel's anchor in the tank's local space. The barrel will connect to the turret at this location.
+    /// </summary>
+    public Vector3 BarrelAnchor;
+    /// <summary>
+    /// Location of the turret's anchor in the tank's local space. The turret will connect to the main body at this location.
+    /// </summary>
+    public Vector3 TurretAnchor;
+    /// <summary>
+    /// 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.
+    /// </summary>
+    public Quaternion TurretBasis;
+    /// <summary>
+    /// Servo properties for the tank's swivel constraint.
+    /// </summary>
+    public ServoSettings TurretServo;
+    /// <summary>
+    /// Spring properties for the tank's swivel constraint.
+    /// </summary>
+    public SpringSettings TurretSpring;
+    /// <summary>
+    /// Servo properties for the tank's barrel pitching constraint.
+    /// </summary>
+    public ServoSettings BarrelServo;
+    /// <summary>
+    /// Spring properties for the tank's barrel pitching constraint.
+    /// </summary>
+    public SpringSettings BarrelSpring;
 
-        /// <summary>
-        /// Location in the barrel body's local space where projectiles should be created.
-        /// </summary>
-        public Vector3 BarrelLocalProjectileSpawn;
-        /// <summary>
-        /// Inertia of fired projectiles.
-        /// </summary>
-        public BodyInertia ProjectileInertia;
-        /// <summary>
-        /// Shape of fired projectiles.
-        /// </summary>
-        public TypedIndex ProjectileShape;
-        /// <summary>
-        /// Speed of fired projectiles.
-        /// </summary>
-        public float ProjectileSpeed;
+    /// <summary>
+    /// Location in the barrel body's local space where projectiles should be created.
+    /// </summary>
+    public Vector3 BarrelLocalProjectileSpawn;
+    /// <summary>
+    /// Inertia of fired projectiles.
+    /// </summary>
+    public BodyInertia ProjectileInertia;
+    /// <summary>
+    /// Shape of fired projectiles.
+    /// </summary>
+    public TypedIndex ProjectileShape;
+    /// <summary>
+    /// Speed of fired projectiles.
+    /// </summary>
+    public float ProjectileSpeed;
 
 
-        /// <summary>
-        /// Shape used for all wheels.
-        /// </summary>
-        public TypedIndex WheelShape;
-        /// <summary>
-        /// Inertia of each wheel body.
-        /// </summary>
-        public BodyInertia WheelInertia;
+    /// <summary>
+    /// Shape used for all wheels.
+    /// </summary>
+    public TypedIndex WheelShape;
+    /// <summary>
+    /// Inertia of each wheel body.
+    /// </summary>
+    public BodyInertia WheelInertia;
 
-        /// <summary>
-        /// 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.
-        /// </summary>
-        public Quaternion WheelOrientation;
-        /// <summary>
-        /// Offset from the tank's local space origin to the left tread's center. The tread will be aligned along (0,0,1) * WheelOrientation.
-        /// </summary>
-        public Vector3 LeftTreadOffset;
-        /// <summary>
-        /// Offset from the tank's local space origin to the right tread's center. The tread will be aligned along (0,0,1) * WheelOrientation.
-        /// </summary>
-        public Vector3 RightTreadOffset;
-        /// <summary>
-        /// Number of wheels in each tread.
-        /// </summary>
-        public int WheelCountPerTread;
-        /// <summary>
-        /// How much space to put in between wheels in the tread.
-        /// </summary>
-        public float TreadSpacing;
-        /// <summary>
-        /// Resting length of the suspension for each wheel.
-        /// </summary>
-        public float SuspensionLength;
-        /// <summary>
-        /// Spring settings for the wheel suspension.
-        /// </summary>
-        public SpringSettings SuspensionSettings;
-        /// <summary>
-        /// Friction for the wheel bodies.
-        /// </summary>
-        public float WheelFriction;
-    }
+    /// <summary>
+    /// 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.
+    /// </summary>
+    public Quaternion WheelOrientation;
+    /// <summary>
+    /// Offset from the tank's local space origin to the left tread's center. The tread will be aligned along (0,0,1) * WheelOrientation.
+    /// </summary>
+    public Vector3 LeftTreadOffset;
+    /// <summary>
+    /// Offset from the tank's local space origin to the right tread's center. The tread will be aligned along (0,0,1) * WheelOrientation.
+    /// </summary>
+    public Vector3 RightTreadOffset;
+    /// <summary>
+    /// Number of wheels in each tread.
+    /// </summary>
+    public int WheelCountPerTread;
+    /// <summary>
+    /// How much space to put in between wheels in the tread.
+    /// </summary>
+    public float TreadSpacing;
+    /// <summary>
+    /// Resting length of the suspension for each wheel.
+    /// </summary>
+    public float SuspensionLength;
+    /// <summary>
+    /// Spring settings for the wheel suspension.
+    /// </summary>
+    public SpringSettings SuspensionSettings;
+    /// <summary>
+    /// Friction for the wheel bodies.
+    /// </summary>
+    public float WheelFriction;
 }
\ No newline at end of file
diff --git a/Demos/Demos/Tanks/TankPartDescription.cs b/Demos/Demos/Tanks/TankPartDescription.cs
index db0e93256..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;
+
+/// <summary>
+/// Describes properties of a piece of a tank.
+/// </summary>
+public struct TankPartDescription
 {
     /// <summary>
-    /// Describes properties of a piece of a tank.
+    /// Shape index used by this part's collidable.
     /// </summary>
-    public struct TankPartDescription
-    {
-        /// <summary>
-        /// Shape index used by this part's collidable.
-        /// </summary>
-        public TypedIndex Shape;
-        /// <summary>
-        /// Inertia of this part's body.
-        /// </summary>
-        public BodyInertia Inertia;
-        /// <summary>
-        /// Pose of the part in the tank's local space.
-        /// </summary>
-        public RigidPose Pose;
-        /// <summary>
-        /// Friction of the body to be used in pair material calculations.
-        /// </summary>
-        public float Friction;
+    public TypedIndex Shape;
+    /// <summary>
+    /// Inertia of this part's body.
+    /// </summary>
+    public BodyInertia Inertia;
+    /// <summary>
+    /// Pose of the part in the tank's local space.
+    /// </summary>
+    public RigidPose Pose;
+    /// <summary>
+    /// Friction of the body to be used in pair material calculations.
+    /// </summary>
+    public float Friction;
 
-        public static TankPartDescription Create<TShape>(float mass, in TShape shape, in RigidPose pose, float friction, Shapes shapes) where TShape : unmanaged, IConvexShape
-        {
-            TankPartDescription description;
-            description.Shape = shapes.Add(shape);
-            shape.ComputeInertia(mass, out description.Inertia);
-            description.Pose = pose;
-            description.Friction = friction;
-            return description;
-        }
+    public static TankPartDescription Create<TShape>(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 2a347f478..ddedab484 100644
--- a/Demos/GameLoop.cs
+++ b/Demos/GameLoop.cs
@@ -1,80 +1,76 @@
 using DemoRenderer;
 using DemoUtilities;
 using System;
-using System.Collections.Generic;
-using System.Text;
 using BepuUtilities;
-using OpenTK;
 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 01153bb6f..ea29003dd 100644
--- a/Demos/Grabber.cs
+++ b/Demos/Grabber.cs
@@ -3,135 +3,133 @@
 using BepuPhysics.Constraints;
 using BepuPhysics.Trees;
 using BepuUtilities;
+using BepuUtilities.Memory;
 using DemoRenderer;
 using DemoRenderer.Constraints;
 using System;
 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, in 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(in 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, 360 / 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, in 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.GetBodyReference(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, ref linearDescription);
-                    if (!Bodies.HasLockedInertia(body.LocalInertia.InverseInertiaTensor))
-                        angularMotorHandle = simulation.Solver.Add(body.Handle, ref 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, ref linearDescription);
+    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;
+        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, ref 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, pool, 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 189962aa9..aaa4f3ca5 100644
--- a/Demos/Program.cs
+++ b/Demos/Program.cs
@@ -3,24 +3,24 @@
 using DemoUtilities;
 using OpenTK;
 
-namespace Demos
+namespace Demos;
+
+class Program
 {
-    class Program
+    static void Main()
     {
-        static void Main(string[] args)
+        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);
-            }
-            var demo = new DemoHarness(loop, content);
-            loop.Run(demo);
-            loop.Dispose();
-            window.Dispose();
+            content = ContentArchive.Load(stream);
         }
+        //HeadlessTest.Test<ShapePileTestDemo>(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 25d3db346..c606fe9ef 100644
--- a/Demos/RolloverInfo.cs
+++ b/Demos/RolloverInfo.cs
@@ -1,73 +1,70 @@
-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
+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<RolloverDescription> descriptions;
+    List<RolloverDescription> descriptions;
 
-        public RolloverInfo()
-        {
-            descriptions = new List<RolloverDescription>();
-        }
+    public RolloverInfo()
+    {
+        descriptions = new List<RolloverDescription>();
+    }
 
-        public void Add(in 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/SimpleThreadDispatcher.cs b/Demos/SimpleThreadDispatcher.cs
deleted file mode 100644
index 15936558a..000000000
--- a/Demos/SimpleThreadDispatcher.cs
+++ /dev/null
@@ -1,116 +0,0 @@
-using BepuPhysics;
-using System;
-using System.Diagnostics;
-using System.Threading;
-using BepuUtilities.Memory;
-using BepuUtilities;
-
-namespace Demos
-{
-    public class SimpleThreadDispatcher : IThreadDispatcher, IDisposable
-    {
-        int threadCount;
-        public int ThreadCount => threadCount;
-        struct Worker
-        {
-            public Thread Thread;
-            public AutoResetEvent Signal;
-        }
-
-        Worker[] workers;
-        AutoResetEvent finished;
-
-        BufferPool[] bufferPools;
-
-        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.Increment(ref completedWorkerCounter) == threadCount)
-            {
-                finished.Set();
-            }
-        }
-
-        volatile Action<int> workerBody;
-        int workerIndex;
-        int completedWorkerCounter;
-
-        void WorkerLoop(object untypedSignal)
-        {
-            var signal = (AutoResetEvent)untypedSignal;
-            while (true)
-            {
-                signal.WaitOne();
-                if (disposed)
-                    return;
-                DispatchThread(Interlocked.Increment(ref workerIndex) - 1);
-            }
-        }
-
-        void SignalThreads()
-        {
-            for (int i = 0; i < workers.Length; ++i)
-            {
-                workers[i].Signal.Set();
-            }
-        }
-
-        public void DispatchWorkers(Action<int> workerBody)
-        {
-            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;
-        }
-
-        volatile bool disposed;
-        public void Dispose()
-        {
-            if (!disposed)
-            {
-                disposed = true;
-                SignalThreads();
-                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/Demos/SimulationTimeSamples.cs b/Demos/SimulationTimeSamples.cs
index 74f41ff05..8a31e45cf 100644
--- a/Demos/SimulationTimeSamples.cs
+++ b/Demos/SimulationTimeSamples.cs
@@ -1,77 +1,65 @@
 using BepuPhysics;
 using BepuUtilities.Memory;
-using System;
-using System.Collections.Generic;
-using System.Text;
 
-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;
+}
+
+/// <summary>
+/// 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.
+/// </summary>
+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);
     }
-    
-    /// <summary>
-    /// 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.
-    /// </summary>
-    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 BodyOptimizer;
-        public TimingsRingBuffer ConstraintOptimizer;
-        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);
-            BodyOptimizer = new TimingsRingBuffer(frameCapacity, pool);
-            ConstraintOptimizer = 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]);
-            BodyOptimizer.Add(simulation.Profiler[simulation.BodyLayoutOptimizer]);
-            ConstraintOptimizer.Add(simulation.Profiler[simulation.ConstraintLayoutOptimizer]);
-            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();
-            BodyOptimizer.Dispose();
-            ConstraintOptimizer.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 fee374dd2..55179bad5 100644
--- a/Demos/SpecializedTests/BatchedCollisionTests.cs
+++ b/Demos/SpecializedTests/BatchedCollisionTests.cs
@@ -3,221 +3,294 @@
 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;
 
-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 unsafe void OnPairCompleted<TManifold>(int pairId, ref TManifold manifold) where TManifold : unmanaged, IContactManifold<TManifold>
+        public void OnPairCompleted<TManifold>(int pairId, ref TManifold manifold) where TManifold : unmanaged, IContactManifold<TManifold>
+        {
+            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 unsafe void OnChildPairCompleted(int pairId, int childA, int childB, ref ConvexContactManifold manifold)
+            else
             {
+                ++*Count;
             }
+        }
 
-            public bool AllowCollisionTesting(int pairId, int childA, int childB)
-            {
-                return true;
-            }
+        public void OnChildPairCompleted(int pairId, int childA, int childB, ref ConvexContactManifold manifold)
+        {
+        }
 
+        public bool AllowCollisionTesting(int pairId, int childA, int childB)
+        {
+            return true;
         }
 
+    }
+
 
-        static void TestPair<TA, TB>(ref TA a, ref TB b, ref Buffer<RigidPose> posesA, ref Buffer<RigidPose> posesB,
-            ref TestCollisionCallbacks callbacks, BufferPool pool, Shapes shapes, CollisionTaskRegistry registry, int iterationCount)
-            where TA : struct, IShape where TB : struct, IShape
+    static void TestPair<TA, TB>(ref TA a, ref TB b, ref Buffer<RigidPose> posesA, ref Buffer<RigidPose> posesB,
+        ref TestCollisionCallbacks callbacks, BufferPool pool, Shapes shapes, CollisionTaskRegistry registry, int iterationCount)
+        where TA : struct, IShape where TB : struct, IShape
+    {
+        var batcher = new CollisionBatcher<TestCollisionCallbacks>(pool, shapes, registry, Demo.TimestepDuration, callbacks);
+        for (int i = 0; i < iterationCount; ++i)
         {
-            var batcher = new CollisionBatcher<TestCollisionCallbacks>(pool, shapes, registry, 1 / 60f, 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();
+            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<TA, TB>(ref TA a, ref TB b, ref Buffer<RigidPose> posesA, ref Buffer<RigidPose> posesB,
-            BufferPool pool, Shapes shapes, CollisionTaskRegistry registry, int iterationCount)
-                        where TA : struct, IShape where TB : struct, IShape
+    unsafe static void Test<TA, TB>(ref TA a, ref TB b, ref Buffer<RigidPose> posesA, ref Buffer<RigidPose> 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}");
+    }
+
+    [MethodImpl(MethodImplOptions.NoInlining)]
+    static float TestPair<TA, TAWide, TB, TBWide, TDistanceTester>(ref TAWide a, ref TBWide b, ref TDistanceTester tester,
+        ref Buffer<RigidPose> posesA, ref Buffer<RigidPose> posesB, int iterationCount)
+        where TA : IShape where TB : IShape
+        where TAWide : struct, IShapeWide<TA> where TBWide : struct, IShapeWide<TB>
+        where TDistanceTester : IPairDistanceTester<TAWide, TBWide>
+    {
+        var distanceSum = Vector<float>.Zero;
+        for (int i = 0; i < iterationCount; ++i)
         {
-            int count = 0;
-            var callbacks = new TestCollisionCallbacks { Count = &count };
-            TestPair(ref a, ref b, ref posesA, ref posesB, ref callbacks, pool, shapes, registry, 64);
-            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}");
+            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<int>.Zero, out var intersected, out var distance, out var closestA, out var normal);
+            distanceSum += distance;
         }
+        return distanceSum[0];
+    }
+
+    static void Test<TA, TAWide, TB, TBWide, TDistanceTester>(in TA a, in TB b,
+        ref Buffer<RigidPose> posesA, ref Buffer<RigidPose> posesB, int iterationCount)
+        where TA : unmanaged, IShape where TB : unmanaged, IShape
+        where TAWide : unmanaged, IShapeWide<TA> where TBWide : unmanaged, IShapeWide<TB>
+        where TDistanceTester : struct, IPairDistanceTester<TAWide, TBWide>
+    {
+        TAWide aWide = default;
+        aWide.Broadcast(a);
+        TBWide bWide = default;
+        bWide.Broadcast(b);
+        var tester = default(TDistanceTester);
+        TestPair<TA, TAWide, TB, TBWide, TDistanceTester>(ref aWide, ref bWide, ref tester, ref posesA, ref posesB, 64);
+        var start = Stopwatch.GetTimestamp();
+        TestPair<TA, TAWide, TB, TBWide, TDistanceTester>(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<float>.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}");
+    }
+
+    static void GetRandomPose(Random random, out RigidPose pose)
+    {
+        pose.Position = new Vector3(random.NextSingle(), random.NextSingle(), random.NextSingle());
 
-        [MethodImpl(MethodImplOptions.NoInlining)]
-        static float TestPair<TA, TAWide, TB, TBWide, TDistanceTester>(ref TAWide a, ref TBWide b, ref TDistanceTester tester,
-            ref Buffer<RigidPose> posesA, ref Buffer<RigidPose> posesB, int iterationCount)
-            where TA : IShape where TB : IShape
-            where TAWide : struct, IShapeWide<TA> where TBWide : struct, IShapeWide<TB>
-            where TDistanceTester : IPairDistanceTester<TAWide, TBWide>
+        float orientationLengthSquared;
+        do
         {
-            var distanceSum = Vector<float>.Zero;
-            for (int i = 0; i < iterationCount; ++i)
+            pose.Orientation = new Quaternion
             {
-                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<int>.Zero, out var intersected, out var distance, out var closestA, out var normal);
-                distanceSum += distance;
-            }
-            return distanceSum[0];
+                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;
+    }
 
-        unsafe static void Test<TA, TAWide, TB, TBWide, TDistanceTester>(in TA a, in TB b,
-            ref Buffer<RigidPose> posesA, ref Buffer<RigidPose> posesB, int iterationCount)
-            where TA : unmanaged, IShape where TB : unmanaged, IShape
-            where TAWide : unmanaged, IShapeWide<TA> where TBWide : unmanaged, IShapeWide<TB>
-            where TDistanceTester : struct, IPairDistanceTester<TAWide, TBWide>
+    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<Vector3>(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)
         {
-            TAWide aWide = default;
-            aWide.Broadcast(a);
-            TBWide bWide = default;
-            bWide.Broadcast(b);
-            var tester = default(TDistanceTester);
-            TestPair<TA, TAWide, TB, TBWide, TDistanceTester>(ref aWide, ref bWide, ref tester, ref posesA, ref posesB, 64);
-            var start = Stopwatch.GetTimestamp();
-            TestPair<TA, TAWide, TB, TBWide, TDistanceTester>(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<float>.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}");
+            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();
         }
 
-        static void GetRandomPose(Random random, out RigidPose pose)
-        {
-            pose.Position = new Vector3((float)random.NextDouble(), (float)random.NextDouble(), (float)random.NextDouble());
+        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);
+
+        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
             {
-                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
-                };
+                localPose.Orientation = new Quaternion(random.NextSingle(), random.NextSingle(), random.NextSingle(), random.NextSingle());
+                orientationLengthSquared = QuaternionEx.LengthSquared(ref localPose.Orientation);
             }
-            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;
+            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);
 
-        public static void Test()
+        int iterationCount = 1 << 20;
+        pool.Take<RigidPose>(iterationCount, out var posesA);
+        pool.Take<RigidPose>(iterationCount, out var posesB);
+        for (int i = 0; i < iterationCount; ++i)
         {
-            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 = 8192;
-            var points = new QuickList<Vector3>(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((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);
-
-            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);
-
-            int iterationCount = 1 << 22;
-            pool.Take<RigidPose>(iterationCount, out var posesA);
-            pool.Take<RigidPose>(iterationCount, out var posesB);
-            for (int i = 0; i < iterationCount; ++i)
-            {
-                GetRandomPose(random, out posesA[i]);
-                GetRandomPose(random, out posesB[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 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, SphereWide, Sphere, SphereWide, SpherePairDistanceTester>(sphere, sphere, ref posesA, ref posesB, iterationCount);
-            Test<Sphere, SphereWide, Capsule, CapsuleWide, SphereCapsuleDistanceTester>(sphere, capsule, ref posesA, ref posesB, iterationCount);
-            Test<Sphere, SphereWide, Box, BoxWide, SphereBoxDistanceTester>(sphere, box, ref posesA, ref posesB, iterationCount);
-            Test<Sphere, SphereWide, Triangle, TriangleWide, SphereTriangleDistanceTester>(sphere, triangle, ref posesA, ref posesB, iterationCount);
-            Test<Sphere, SphereWide, Cylinder, CylinderWide, SphereCylinderDistanceTester>(sphere, cylinder, ref posesA, ref posesB, iterationCount);
-            Test<Capsule, CapsuleWide, Capsule, CapsuleWide, CapsulePairDistanceTester>(capsule, capsule, ref posesA, ref posesB, iterationCount);
-            Test<Capsule, CapsuleWide, Box, BoxWide, CapsuleBoxDistanceTester>(capsule, box, ref posesA, ref posesB, iterationCount);
-            Test<Capsule, CapsuleWide, Triangle, TriangleWide, GJKDistanceTester<Capsule, CapsuleWide, CapsuleSupportFinder, Triangle, TriangleWide, TriangleSupportFinder>>(capsule, triangle, ref posesA, ref posesB, iterationCount);
-            Test<Capsule, CapsuleWide, Cylinder, CylinderWide, GJKDistanceTester<Capsule, CapsuleWide, CapsuleSupportFinder, Cylinder, CylinderWide, CylinderSupportFinder>>(capsule, cylinder, ref posesA, ref posesB, iterationCount);
-            Test<Cylinder, CylinderWide, Box, BoxWide, GJKDistanceTester<Cylinder, CylinderWide, CylinderSupportFinder, Box, BoxWide, BoxSupportFinder>>(cylinder, box, ref posesA, ref posesB, iterationCount);
-            Test<Cylinder, CylinderWide, Triangle, TriangleWide, GJKDistanceTester<Cylinder, CylinderWide, CylinderSupportFinder, Triangle, TriangleWide, TriangleSupportFinder>>(cylinder, triangle, ref posesA, ref posesB, iterationCount);
-            Test<Cylinder, CylinderWide, Cylinder, CylinderWide, GJKDistanceTester<Cylinder, CylinderWide, CylinderSupportFinder, Cylinder, CylinderWide, CylinderSupportFinder>>(cylinder, cylinder, ref posesA, ref posesB, iterationCount);
-            Test<Box, BoxWide, Box, BoxWide, GJKDistanceTester<Box, BoxWide, BoxSupportFinder, Box, BoxWide, BoxSupportFinder>>(box, box, ref posesA, ref posesB, iterationCount);
-            Test<Box, BoxWide, Triangle, TriangleWide, GJKDistanceTester<Box, BoxWide, BoxSupportFinder, Triangle, TriangleWide, TriangleSupportFinder>>(box, triangle, ref posesA, ref posesB, iterationCount);
-            Test<Triangle, TriangleWide, Triangle, TriangleWide, GJKDistanceTester<Triangle, TriangleWide, TriangleSupportFinder, Triangle, TriangleWide, TriangleSupportFinder>>(triangle, triangle, ref posesA, ref posesB, iterationCount);
-            Console.WriteLine($"Done. Hit enter to exit.");
-            Console.ReadLine();
-        }
+        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, SphereWide, Sphere, SphereWide, SpherePairDistanceTester>(sphere, sphere, ref posesA, ref posesB, iterationCount);
+        Test<Sphere, SphereWide, Capsule, CapsuleWide, SphereCapsuleDistanceTester>(sphere, capsule, ref posesA, ref posesB, iterationCount);
+        Test<Sphere, SphereWide, Box, BoxWide, SphereBoxDistanceTester>(sphere, box, ref posesA, ref posesB, iterationCount);
+        Test<Sphere, SphereWide, Triangle, TriangleWide, SphereTriangleDistanceTester>(sphere, triangle, ref posesA, ref posesB, iterationCount);
+        Test<Sphere, SphereWide, Cylinder, CylinderWide, SphereCylinderDistanceTester>(sphere, cylinder, ref posesA, ref posesB, iterationCount);
+        Test<Capsule, CapsuleWide, Capsule, CapsuleWide, CapsulePairDistanceTester>(capsule, capsule, ref posesA, ref posesB, iterationCount);
+        Test<Capsule, CapsuleWide, Box, BoxWide, CapsuleBoxDistanceTester>(capsule, box, ref posesA, ref posesB, iterationCount);
+        Test<Capsule, CapsuleWide, Triangle, TriangleWide, GJKDistanceTester<Capsule, CapsuleWide, CapsuleSupportFinder, Triangle, TriangleWide, TriangleSupportFinder>>(capsule, triangle, ref posesA, ref posesB, iterationCount);
+        Test<Capsule, CapsuleWide, Cylinder, CylinderWide, GJKDistanceTester<Capsule, CapsuleWide, CapsuleSupportFinder, Cylinder, CylinderWide, CylinderSupportFinder>>(capsule, cylinder, ref posesA, ref posesB, iterationCount);
+        Test<Cylinder, CylinderWide, Box, BoxWide, GJKDistanceTester<Cylinder, CylinderWide, CylinderSupportFinder, Box, BoxWide, BoxSupportFinder>>(cylinder, box, ref posesA, ref posesB, iterationCount);
+        Test<Cylinder, CylinderWide, Triangle, TriangleWide, GJKDistanceTester<Cylinder, CylinderWide, CylinderSupportFinder, Triangle, TriangleWide, TriangleSupportFinder>>(cylinder, triangle, ref posesA, ref posesB, iterationCount);
+        Test<Cylinder, CylinderWide, Cylinder, CylinderWide, GJKDistanceTester<Cylinder, CylinderWide, CylinderSupportFinder, Cylinder, CylinderWide, CylinderSupportFinder>>(cylinder, cylinder, ref posesA, ref posesB, iterationCount);
+        Test<Box, BoxWide, Box, BoxWide, GJKDistanceTester<Box, BoxWide, BoxSupportFinder, Box, BoxWide, BoxSupportFinder>>(box, box, ref posesA, ref posesB, iterationCount);
+        Test<Box, BoxWide, Triangle, TriangleWide, GJKDistanceTester<Box, BoxWide, BoxSupportFinder, Triangle, TriangleWide, TriangleSupportFinder>>(box, triangle, ref posesA, ref posesB, iterationCount);
+        Test<Triangle, TriangleWide, Triangle, TriangleWide, GJKDistanceTester<Triangle, TriangleWide, TriangleSupportFinder, Triangle, TriangleWide, TriangleSupportFinder>>(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 d96b28565..fccb45b96 100644
--- a/Demos/SpecializedTests/BroadPhaseStressTestDemo.cs
+++ b/Demos/SpecializedTests/BroadPhaseStressTestDemo.cs
@@ -1,110 +1,293 @@
-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 BepuUtilities.Memory;
-using BepuUtilities.Collections;
-using System.Runtime.CompilerServices;
-using DemoContentLoader;
+using static Demos.SpecializedTests.TreeFiddlingTestDemo;
+
+namespace Demos.SpecializedTests;
 
-namespace Demos.SpecializedTests
+public class BroadPhaseStressTestDemo : Demo
 {
-    public class BroadPhaseStressTestDemo : Demo
+
+    public 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 bool ConfigureContactManifold<TManifold>(int workerIndex, CollidablePair pair, ref TManifold manifold, out PairMaterialProperties pairMaterial) where TManifold : unmanaged, IContactManifold<TManifold>
+        {
+            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 override void Initialize(ContentArchive content, Camera camera)
     {
-        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());
-
-            var shape = new Sphere(0.5f);
-            shape.ComputeInertia(1, out var sphereInertia);
-            var shapeIndex = Simulation.Shapes.Add(shape);
-            const int width = 64;
-            const int height = 64;
-            const int length = 64;
-            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;
-            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 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)
                     {
-                        var r = new Vector3((float)random.NextDouble(), (float)random.NextDouble(), (float)random.NextDouble());
-                        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);
-                        }
+                        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
-                        {
-                            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.Bodies.Add(BodyDescription.CreateDynamic(location, sphereInertia, shapeIndex, -1));
+                    }
+                    else
+                    {
+                        Simulation.Statics.Add(new StaticDescription(location, shapeIndex));
                     }
                 }
             }
-            refineTimes = new TimingsRingBuffer(sampleCount, BufferPool);
-            testTimes = new TimingsRingBuffer(sampleCount, BufferPool);
         }
+        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))));
+        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);
+    }
+    StaticHandle groundStatic;
 
-        const int sampleCount = 128;
-        TimingsRingBuffer refineTimes;
-        TimingsRingBuffer testTimes;
-        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;
+
+    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)
         {
-            base.Update(window, camera, input, dt);
-            refineTimes.Add(Simulation.Profiler[Simulation.BroadPhase]);
-            testTimes.Add(Simulation.Profiler[Simulation.BroadPhaseOverlapFinder]);
-            if (frameCount++ % sampleCount == 0)
+            ref var node = ref Simulation.BroadPhase.StaticTree.Metanodes[nodeIndex];
+            Console.Write($"{nodeIndex}, ");
+            if (node.Parent >= 0)
             {
-                var refineStats = refineTimes.ComputeStats();
-                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");
+                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 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();
+            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);
+    }
+
+    // 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);
+        }
+    }
+
 }
diff --git a/Demos/SpecializedTests/CacheBlaster.cs b/Demos/SpecializedTests/CacheBlaster.cs
index 628445c21..8b8231157 100644
--- a/Demos/SpecializedTests/CacheBlaster.cs
+++ b/Demos/SpecializedTests/CacheBlaster.cs
@@ -1,45 +1,39 @@
-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
+namespace Demos.SpecializedTests;
+
+public static class CacheBlaster
 {
-    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 intCount = byteCount / 4;
-        static int vectorCount = intCount / Vector<int>.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<int>.Count;
+    static int vectorMask = vectorCount - 1;
+    static int[] readblob = new int[intCount];
+    static int[] writeblob = new int[intCount];
 
-        /// <summary>
-        /// 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.
-        /// </summary>
-        [MethodImpl(MethodImplOptions.NoOptimization)]
-        public static void Blast()
+    /// <summary>
+    /// 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.
+    /// </summary>
+    [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<int>.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<int>.Count;
-            Parallel.For(0, intCount / intsPerJob, jobIndex =>
-            {
-                var baseIndex = jobIndex * intsPerJob;
-                ref Vector<int> read = ref Unsafe.As<int, Vector<int>>(ref readblob[0]);
-                ref Vector<int> write = ref Unsafe.As<int, Vector<int>>(ref writeblob[baseIndex]);
+            var baseIndex = jobIndex * intsPerJob;
+            ref Vector<int> read = ref Unsafe.As<int, Vector<int>>(ref readblob[0]);
+            ref Vector<int> write = ref Unsafe.As<int, Vector<int>>(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 452144035..251c82b74 100644
--- a/Demos/SpecializedTests/CapsuleTestDemo.cs
+++ b/Demos/SpecializedTests/CapsuleTestDemo.cs
@@ -5,76 +5,52 @@
 using BepuPhysics.Collidables;
 using System;
 using System.Numerics;
-using System.Diagnostics;
-using BepuUtilities.Memory;
-using BepuUtilities.Collections;
 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 unsafe 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 DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new PositionFirstTimestepper());
+        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);
-            shape.ComputeInertia(1, out var localInertia);
-            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 = 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 }
-                        };
-                        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);
-            boxShape.ComputeInertia(1, out var boxLocalInertia);
-            var boxDescription = new BodyDescription
-            {
-                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)
-                },
-                Collidable = new CollidableDescription { SpeculativeMargin = 50.1f, Shape = Simulation.Shapes.Add(boxShape) }
-            };
-            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), new CollidableDescription { SpeculativeMargin = 0.1f, Shape = 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 bc1b306f1..2564359ff 100644
--- a/Demos/SpecializedTests/CharacterTestDemo.cs
+++ b/Demos/SpecializedTests/CharacterTestDemo.cs
@@ -9,130 +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 unsafe 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<ulong>();
-            characters = new CharacterControllers(BufferPool);
-            Simulation = Simulation.Create(BufferPool, new CharacterNarrowphaseCallbacks(characters), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new PositionFirstTimestepper());
+        camera.Position = new Vector3(20, 10, 20);
+        camera.Yaw = MathHelper.Pi * -1f / 4;
+        camera.Pitch = MathHelper.Pi * 0.05f;
+        var masks = new CollidableProperty<ulong>();
+        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 * (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 BodyActivityDescription(-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((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 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;
-            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);
-            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)));
+        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<CharacterController>(characters.CharacterCount, BufferPool);
-        }
+        removedCharacters = new QuickQueue<CharacterController>(characters.CharacterCount, BufferPool);
+    }
 
-        QuickQueue<CharacterController> removedCharacters;
-        int frameIndex;
-        public override void Update(Window window, Camera camera, Input input, float dt)
+    QuickQueue<CharacterController> 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 ae95288f7..65154c8fd 100644
--- a/Demos/SpecializedTests/ClothLatticeDemo.cs
+++ b/Demos/SpecializedTests/ClothLatticeDemo.cs
@@ -1,143 +1,93 @@
 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;
 
-namespace Demos.SpecializedTests
+namespace Demos.SpecializedTests;
+
+public class ClothLatticeDemo : Demo
 {
-    public class ClothLatticeDemo : Demo
+    public override void Initialize(ContentArchive content, Camera camera)
     {
-        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());
+        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);
-            clothNodeShape.ComputeInertia(1, out var clothNodeInertia);
-            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 = 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
-                    };
-                    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], ref left);
-                    if (j >= 1)
-                        Simulation.Solver.Add(nodeHandles[i][j], nodeHandles[i][j - 1], ref up);
-                    if (i >= 1 && j >= 1)
-                        Simulation.Solver.Add(nodeHandles[i][j], nodeHandles[i - 1][j - 1], ref leftUp);
-                    if (i < width - 1 && j >= 1)
-                        Simulation.Solver.Add(nodeHandles[i][j], nodeHandles[i + 1][j - 1], ref 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
-            {
-                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
-                }
-            };
-            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
-            {
-                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
-                }
-            };
-            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 d838407f8..f09195f59 100644
--- a/Demos/SpecializedTests/CompoundBoundTests.cs
+++ b/Demos/SpecializedTests/CompoundBoundTests.cs
@@ -5,234 +5,233 @@
 using BepuPhysics.Collidables;
 using System;
 using System.Numerics;
-using System.Diagnostics;
 using DemoContentLoader;
 using DemoRenderer.UI;
 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 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());
-
-
-        }
-
-        void GetArcExpansion(in Vector3 offset, in 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(in Vector3 localPoseA, in Vector3 angularVelocityA, in Vector3 offsetB, in 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((float)random.NextDouble(), (float)random.NextDouble(), (float)random.NextDouble()) * 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<float>(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 unsafe 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<float>(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 eda0e3b23..7e82278b3 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;
@@ -11,81 +9,80 @@
 using BepuPhysics.CollisionDetection;
 using BepuPhysics.Constraints;
 
-namespace Demos.SpecializedTests
+namespace Demos.SpecializedTests;
+
+public struct IndexReportingNarrowPhaseCallbacks : INarrowPhaseCallbacks
 {
-    public unsafe 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)
-        {
-            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 unsafe bool ConfigureContactManifold<TManifold>(int workerIndex, CollidablePair pair, ref TManifold manifold, out PairMaterialProperties pairMaterial) where TManifold : unmanaged, IContactManifold<TManifold>
+    [MethodImpl(MethodImplOptions.AggressiveInlining)]
+    public bool ConfigureContactManifold<TManifold>(int workerIndex, CollidablePair pair, ref TManifold manifold, out PairMaterialProperties pairMaterial) where TManifold : unmanaged, IContactManifold<TManifold>
+    {
+        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 PositionFirstTimestepper());
+        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 RigidPose(new Vector3(-1, 0, 0)), 1);
-            builder.Add(new Capsule(0.5f, 1f), new RigidPose(new Vector3(0, 0, 0)), 1);
-            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 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 compoundCollidable = new CollidableDescription(Simulation.Shapes.Add(new Compound(children)), 0.1f);
+        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)), 0.1f)));
-        }
+        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
new file mode 100644
index 000000000..f0681c2d6
--- /dev/null
+++ b/Demos/SpecializedTests/ConstrainedKinematicIntegrationTest.cs
@@ -0,0 +1,55 @@
+using DemoRenderer;
+using BepuPhysics;
+using BepuPhysics.Collidables;
+using System.Numerics;
+using DemoContentLoader;
+using BepuPhysics.Constraints;
+
+namespace Demos.SpecializedTests;
+
+public class ConstrainedKinematicIntegrationTest : Demo
+{
+    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)
+        {
+            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 ab7ffca24..c78388063 100644
--- a/Demos/SpecializedTests/ConstraintTestDemo.cs
+++ b/Demos/SpecializedTests/ConstraintTestDemo.cs
@@ -8,416 +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 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());
-
-            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);
-            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 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 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)
-                });
-            }
+                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 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),
+                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 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) });
-            }
+                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.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)),
+                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 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) });
-            }
+                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 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) });
-            }
+                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 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)));
-            }
+                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 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),
+                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 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),
+                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), 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)));
-            }
+                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, 0.01f),
-                });
-            }
+                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)), 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)));
+                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/ConvexHullTestDemo.cs b/Demos/SpecializedTests/ConvexHullTestDemo.cs
index 928a0c816..1771eb615 100644
--- a/Demos/SpecializedTests/ConvexHullTestDemo.cs
+++ b/Demos/SpecializedTests/ConvexHullTestDemo.cs
@@ -1,222 +1,933 @@
-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;
-
-namespace Demos.SpecializedTests
-{
-    public class ConvexHullTestDemo : Demo
-    {
-        QuickList<Vector3> points;
-        //List<DebugStep> debugSteps;
-        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 DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new PositionFirstTimestepper());
-
-            //var meshContent = content.Load<MeshContent>("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<Vector3>(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);
-            //}
-
-            const int pointCount = 50;
-            points = new QuickList<Vector3>(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 * (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 pointsBuffer = points.Span.Slice(points.Count);
-            CreateShape(pointsBuffer, BufferPool, out _, out var hullShape);
-            //ConvexHullHelper.ComputeHull(pointsBuffer, BufferPool, out _, out debugSteps);
-
-            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(pointsBuffer, 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);
-            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.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)));
-
-            var spacing = new Vector3(3f, 3f, 3);
-            int width = 16;
-            int height = 16;
-            int length = 16;
-            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(
-                            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)));
-                    }
-                }
-            }
-            Simulation.Statics.Add(new StaticDescription(new Vector3(0, -10, 0), new CollidableDescription(Simulation.Shapes.Add(new Box(1000, 1, 1000)), 0.1f)));
-        }
-
-        void TestConvexHullCreation()
-        {
-            var random = new Random(5);
-            for (int iterationIndex = 0; iterationIndex < 100000; ++iterationIndex)
-            {
-                const int pointCount = 32;
-                var points = new QuickList<Vector3>(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());
-                }
-
-                var pointsBuffer = points.Span.Slice(points.Count);
-                CreateShape(pointsBuffer, BufferPool, out _, out var hullShape);
-
-                hullShape.Dispose(BufferPool);
-            }
-        }
-
-        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 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));
-
-            //    }
-            //}
-            //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);
-        }
-    }
-}
+// 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;
+using BepuPhysics.Collidables;
+using BepuUtilities.Collections;
+using DemoContentLoader;
+using DemoRenderer;
+using BepuPhysics;
+using static BepuPhysics.Collidables.ConvexHullHelper;
+using BepuUtilities;
+using BepuPhysics.Constraints;
+using BepuUtilities.Memory;
+using System.Text.Json;
+using System.IO;
+using DemoRenderer.Constraints;
+using DemoUtilities;
+using DemoRenderer.UI;
+using System.Diagnostics;
+
+
+
+namespace Demos.SpecializedTests;
+
+public class ConvexHullTestDemo : Demo
+{
+    Vector3[] CreateRandomConvexHullPoints()
+    {
+        var points = new Vector3[50];
+        var random = new Random(5);
+        for (int i = 0; i < points.Length; ++i)
+        {
+            points[i] = new(3 * random.NextSingle(), 1 * random.NextSingle(), 3 * random.NextSingle());
+        }
+
+        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;
+    }
+
+    Vector3[] CreatePlaneish()
+    {
+        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;
+    }
+
+    Vector3[] CreateDistantPlane()
+    {
+        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;
+    }
+
+    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.
+        var points = new Vector3[meshContent.Triangles.Length * 3];
+        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;
+    }
+
+    Vector3[] CreateBoxConvexHull(float 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
+    Vector3[] CreateTestConvexHull()
+    {
+        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;
+    }
+
+    Vector3[] CreateTestConvexHull2()
+    {
+        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;
+    }
+
+
+    Vector3[] CreateTestConvexHull3()
+    {
+        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;
+    }
+
+    Vector3[] CreateJSONSourcedConvexHull(string filePath)
+    {
+        //ChatGPT wrote this, of course.
+        List<Vector3> points = new List<Vector3>();
+        if (File.Exists(filePath))
+        {
+            string jsonContent = File.ReadAllText(filePath);
+            List<List<double>> rawPoints = JsonSerializer.Deserialize<List<List<double>>>(jsonContent);
+            foreach (List<double> 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);
+        }       
+        return points.ToArray();
+    }
+
+    void CreateHellCubeFace(int widthInPoints, float step, Span<Vector3> 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;
+            }
+        }
+    }
+    Vector3[] CreateHellCube(int widthInPoints)
+    {
+        var facePointCount = widthInPoints * widthInPoints;
+        var buffer = new Vector3[facePointCount * 6];
+        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.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;
+    }
+
+    struct HullTestData
+    {
+        public Vector3[] Points;
+        public HullData HullData;
+#if DEBUG_STEPS
+        public List<DebugStep> 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);
+        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 hullPointSets = new Vector3[][]
+        {
+            CreateRandomConvexHullPoints(),
+            CreateMeshConvexHull(content.Load<MeshContent>(@"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);
+
+            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 _);
+
+
+
+
+        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 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)
+            {
+                for (int k = 0; k < length; ++k)
+                {
+                    Simulation.Bodies.Add(BodyDescription.CreateDynamic(
+                        (origin + pileSpacing * new Vector3(i, j, k), QuaternionEx.CreateFromAxisAngle(new Vector3(0, 1, 0), MathHelper.Pi * 0.05f)),
+                        pileInertia, pileShape, 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(64, 0, 0), Simulation.Shapes.Add(mesh)));
+
+#if DEBUG_STEPS
+        stepIndices = new int[hullTests.Length];
+#endif
+    }
+
+    void TestConvexHullCreation()
+    {
+        var random = new Random(5);
+        for (int iterationIndex = 0; iterationIndex < 100000; ++iterationIndex)
+        {
+            const int pointCount = 32;
+            var points = new QuickList<Vector3>(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);
+        }
+    }
+
+#if DEBUG_STEPS
+    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, 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;
+        }
+        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 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);
+
+        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.Length; ++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.Length; ++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));
+            }
+        }
+
+        void DrawFace(DebugStep step, int[] reduced, Vector3 color, float offsetScale)
+        {
+            var offset = step.FaceNormal * offsetScale;
+            if (showWireframe)
+            {
+                var previousIndex = reduced.Length - 1;
+                for (int q = 0; q < reduced.Length; ++q)
+                {
+                    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
+            {
+                for (int k = 2; k < reduced.Length; ++k)
+                {
+                    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, renderOffset, color);
+                }
+            }
+        }
+
+        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
+            {
+                // Draw filled triangles for the face
+                var baseIndex = step.FaceIndices[startIndex];
+                var basePoint = points[baseIndex] * scale;
+                for (int vertexIndex = startIndex + 2; vertexIndex < endIndex; ++vertexIndex)
+                {
+                    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}");
+
+        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("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);
+        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);
+    }
+#endif
+}
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;
+
+/// <summary>
+/// Pure forwarding wrapper around <see cref="Mesh"/>. Has its own TypeId so the narrow phase treats it as a distinct shape,
+/// which lets us verify that <see cref="MeshReduction"/>'s boundary smoothing works for any <see cref="IHomogeneousCompoundShape{Triangle, TriangleWide}">, not just the built-in Mesh type.
+/// </summary>
+public struct WrappedMesh : IHomogeneousCompoundShape<Triangle, TriangleWide>
+{
+    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<WrappedMesh, Triangle, TriangleWide>(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<TRayHitHandler>(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<TRayHitHandler>(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<TOverlaps, TSubpairOverlaps>(ref Buffer<OverlapQueryForPair> pairs, BufferPool pool, Shapes shapes, ref TOverlaps overlaps)
+        where TOverlaps : struct, ICollisionTaskOverlaps<TSubpairOverlaps>
+        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<TSubpairOverlaps> enumerator;
+        enumerator.Pool = pool;
+        for (int i = 0; i < pairs.Length; ++i)
+        {
+            ref var pair = ref pairs[i];
+            ref var wrapped = ref Unsafe.AsRef<WrappedMesh>(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<TOverlaps>(Vector3 min, Vector3 max, Vector3 sweep, float maximumT, BufferPool pool, Shapes shapes, void* overlaps)
+        where TOverlaps : ICollisionTaskSubpairOverlaps
+    {
+        Inner.FindLocalOverlaps<TOverlaps>(min, max, sweep, maximumT, pool, shapes, overlaps);
+    }
+
+    public readonly void FindLocalOverlaps<TEnumerator>(Vector3 min, Vector3 max, BufferPool pool, Shapes shapes, ref TEnumerator enumerator)
+        where TEnumerator : IBreakableForEach<int>
+    {
+        Inner.FindLocalOverlaps(min, max, pool, shapes, ref enumerator);
+    }
+
+    public void Dispose(BufferPool pool)
+    {
+        Inner.Dispose(pool);
+    }
+}
+
+/// <summary>
+/// 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 <see cref="MeshReduction.ReduceManifolds"/> for a non-<see cref="Mesh"/>
+/// IHomogeneousCompoundShape so boundary smoothing can be validated on the type-erased path.
+/// </summary>
+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<WrappedMesh> is used instead of MeshReductionThunks<Mesh>.
+        var collisionTasks = Simulation.NarrowPhase.CollisionTaskRegistry;
+        collisionTasks.Register(new ConvexCompoundCollisionTask<Sphere, WrappedMesh, ConvexCompoundOverlapFinder<Sphere, SphereWide, WrappedMesh>, ConvexMeshContinuations<WrappedMesh>, MeshReduction>());
+        collisionTasks.Register(new ConvexCompoundCollisionTask<Capsule, WrappedMesh, ConvexCompoundOverlapFinder<Capsule, CapsuleWide, WrappedMesh>, ConvexMeshContinuations<WrappedMesh>, MeshReduction>());
+        collisionTasks.Register(new ConvexCompoundCollisionTask<Box, WrappedMesh, ConvexCompoundOverlapFinder<Box, BoxWide, WrappedMesh>, ConvexMeshContinuations<WrappedMesh>, MeshReduction>());
+        collisionTasks.Register(new ConvexCompoundCollisionTask<Triangle, WrappedMesh, ConvexCompoundOverlapFinder<Triangle, TriangleWide, WrappedMesh>, ConvexMeshContinuations<WrappedMesh>, MeshReduction>());
+        collisionTasks.Register(new ConvexCompoundCollisionTask<Cylinder, WrappedMesh, ConvexCompoundOverlapFinder<Cylinder, CylinderWide, WrappedMesh>, ConvexMeshContinuations<WrappedMesh>, MeshReduction>());
+        collisionTasks.Register(new ConvexCompoundCollisionTask<ConvexHull, WrappedMesh, ConvexCompoundOverlapFinder<ConvexHull, ConvexHullWide, WrappedMesh>, ConvexMeshContinuations<WrappedMesh>, MeshReduction>());
+
+        //Compound-vs-WrappedMesh uses a separate continuation type (CompoundMeshReduction), but it plugs into MeshReductionThunks<WrappedMesh> the same way.
+        collisionTasks.Register(new CompoundPairCollisionTask<Compound, WrappedMesh, CompoundPairOverlapFinder<Compound, WrappedMesh>, CompoundMeshContinuations<Compound, WrappedMesh>, CompoundMeshReduction>());
+
+        //Sweep tasks matching the convex set, so swept queries keep working too.
+        var sweepTasks = Simulation.NarrowPhase.SweepTaskRegistry;
+        sweepTasks.Register(new ConvexHomogeneousCompoundSweepTask<Sphere, SphereWide, WrappedMesh, Triangle, TriangleWide, ConvexCompoundSweepOverlapFinder<Sphere, WrappedMesh>>());
+        sweepTasks.Register(new ConvexHomogeneousCompoundSweepTask<Capsule, CapsuleWide, WrappedMesh, Triangle, TriangleWide, ConvexCompoundSweepOverlapFinder<Capsule, WrappedMesh>>());
+        sweepTasks.Register(new ConvexHomogeneousCompoundSweepTask<Box, BoxWide, WrappedMesh, Triangle, TriangleWide, ConvexCompoundSweepOverlapFinder<Box, WrappedMesh>>());
+        sweepTasks.Register(new ConvexHomogeneousCompoundSweepTask<Triangle, TriangleWide, WrappedMesh, Triangle, TriangleWide, ConvexCompoundSweepOverlapFinder<Triangle, WrappedMesh>>());
+        sweepTasks.Register(new ConvexHomogeneousCompoundSweepTask<Cylinder, CylinderWide, WrappedMesh, Triangle, TriangleWide, ConvexCompoundSweepOverlapFinder<Cylinder, WrappedMesh>>());
+        sweepTasks.Register(new ConvexHomogeneousCompoundSweepTask<ConvexHull, ConvexHullWide, WrappedMesh, Triangle, TriangleWide, ConvexCompoundSweepOverlapFinder<ConvexHull, WrappedMesh>>());
+        sweepTasks.Register(new CompoundHomogeneousCompoundSweepTask<Compound, WrappedMesh, Triangle, TriangleWide, CompoundPairSweepOverlapFinder<Compound, WrappedMesh>>());
+
+        //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<WrappedMesh>, 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<Vector3>(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<Mesh> (rather than AddShape<WrappedMesh>) 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);
+    }
+}
diff --git a/Demos/SpecializedTests/CylinderTestDemo.cs b/Demos/SpecializedTests/CylinderTestDemo.cs
index e5ed8e794..517104447 100644
--- a/Demos/SpecializedTests/CylinderTestDemo.cs
+++ b/Demos/SpecializedTests/CylinderTestDemo.cs
@@ -7,356 +7,341 @@
 using BepuPhysics.Collidables;
 using BepuPhysics.CollisionDetection.CollisionTasks;
 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(in Vector3 lineOrigin, in 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 * (float)random.NextDouble(), 0.2f + 0.8f * (float)random.NextDouble());
-                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);
-                    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((float)random.NextDouble(), (float)random.NextDouble(), (float)random.NextDouble()) * 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<float>(capsule.HalfLength), cylinderWide, out var t, out var min, out var max, out var offsetFromCylindertoLineSegment, out var iterationsRequired);
+            //CapsuleCylinderTester.GetClosestPointBetweenLineSegmentAndCylinder(capsuleOrigin, capsuleY, new Vector<float>(capsule.HalfLength), cylinderWide, out var t, out var min, out var max, out var offsetFromCylindertoLineSegment, out var iterationsRequired);
 
-                //CapsuleCylinderTester.GetClosestPointBetweenLineSegmentAndCylinder(capsuleOrigin, capsuleY, new Vector<float>(capsule.HalfLength), cylinderWide, out var t, out var offsetFromCylindertoLineSegment);
-                Vector<float> t = default;
-                Vector3Wide offsetFromCylinderToLineSegment = default;
-                var innerStart = Stopwatch.GetTimestamp();
-                for (int j = 0; j < innerIterations; ++j)
-                {
-                    CapsuleCylinderTester.GetClosestPointBetweenLineSegmentAndCylinder(capsuleOrigin, capsuleY, new Vector<float>(capsule.HalfLength), cylinderWide, Vector<int>.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<float>(capsule.HalfLength), cylinderWide, out var t, out var offsetFromCylindertoLineSegment);
+            Vector<float> t = default;
+            Vector3Wide offsetFromCylinderToLineSegment = default;
+            var innerStart = Stopwatch.GetTimestamp();
+            for (int j = 0; j < innerIterations; ++j)
+            {
+                CapsuleCylinderTester.GetClosestPointBetweenLineSegmentAndCylinder(capsuleOrigin, capsuleY, new Vector<float>(capsule.HalfLength), cylinderWide, Vector<int>.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 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 CollidableDescription(Simulation.Shapes.Add(cylinderShape), 1000), 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)));
-            Simulation.Bodies.Add(BodyDescription.CreateConvexKinematic(new RigidPose(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 RigidPose(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 RigidPose(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);
-            cylinderShape.ComputeInertia(1, out cylinderInertia);
-            //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);
-            box.ComputeInertia(1, out var boxInertia);
-            capsule.ComputeInertia(1, out var capsuleInertia);
-            sphere.ComputeInertia(1, out var sphereInertia);
-            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 = 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
-                            {
-                                Continuity = new ContinuousDetectionSettings { Mode = ContinuousDetectionMode.Discrete },
-                                SpeculativeMargin = 0.1f
-                            }
-                        };
-                        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;
-            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, 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)));
-
-            //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<float>(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<float>.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<Cylinder, CylinderWide, CylinderSupportFinder, Cylinder, CylinderWide, CylinderSupportFinder>.Refine(a, b, localOffsetB, localOrientationB,
-            //        ref supportFinderA, ref supportFinderB, initialGuess, new Vector<float>(-0.1f), new Vector<float>(1e-4f), 1500, Vector<int>.Zero, out var localNormal, out var depthBelowThreshold);
-
-            //    GJKDistanceTester<Cylinder, CylinderWide, CylinderSupportFinder, Cylinder, CylinderWide, CylinderSupportFinder> 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<Cylinder, CylinderWide, CylinderSupportFinder, Cylinder, CylinderWide, CylinderSupportFinder>.Test(a, b, localOffsetB, localOrientationB, ref supportFinderA, ref supportFinderB, new Vector<float>(1e-5f), Vector<int>.Zero, out var intersecting, out var localNormal);
-            //    for (int i = 0; i < 5; ++i)
-            //    {
-            //        MPR<Cylinder, CylinderWide, CylinderSupportFinder, Cylinder, CylinderWide, CylinderSupportFinder>.LocalSurfaceCast(a, b, localOffsetB, localOrientationB, ref supportFinderA, ref supportFinderB, localNormal, new Vector<float>(1e-3f), Vector<int>.Zero,
-            //            out var t, out localNormal);
-            //    }
-            //    Vector3Wide.Normalize(localNormal, out var test);
-
-            //    GJKDistanceTester<Cylinder, CylinderWide, CylinderSupportFinder, Cylinder, CylinderWide, CylinderSupportFinder> 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<Cylinder, CylinderWide, CylinderSupportFinder, Cylinder, CylinderWide, CylinderSupportFinder>.Test(a, b, localOffsetB, localOrientationB, ref supportFinderA, ref supportFinderB, new Vector<float>(1e-5f), Vector<int>.Zero, out var intersecting, out var localNormal);
-            //    Vector3Wide.Normalize(localNormal, out var test);
-            //    GJKDistanceTester<Cylinder, CylinderWide, CylinderSupportFinder, Cylinder, CylinderWide, CylinderSupportFinder> 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<float>(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<float>.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<Cylinder, CylinderWide, CylinderSupportFinder, Cylinder, CylinderWide, CylinderSupportFinder>.Refine(a, b, localOffsetB, localOrientationB,
+        //        ref supportFinderA, ref supportFinderB, initialGuess, new Vector<float>(-0.1f), new Vector<float>(1e-4f), 1500, Vector<int>.Zero, out var localNormal, out var depthBelowThreshold);
+
+        //    GJKDistanceTester<Cylinder, CylinderWide, CylinderSupportFinder, Cylinder, CylinderWide, CylinderSupportFinder> 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<Cylinder, CylinderWide, CylinderSupportFinder, Cylinder, CylinderWide, CylinderSupportFinder>.Test(a, b, localOffsetB, localOrientationB, ref supportFinderA, ref supportFinderB, new Vector<float>(1e-5f), Vector<int>.Zero, out var intersecting, out var localNormal);
+        //    for (int i = 0; i < 5; ++i)
+        //    {
+        //        MPR<Cylinder, CylinderWide, CylinderSupportFinder, Cylinder, CylinderWide, CylinderSupportFinder>.LocalSurfaceCast(a, b, localOffsetB, localOrientationB, ref supportFinderA, ref supportFinderB, localNormal, new Vector<float>(1e-3f), Vector<int>.Zero,
+        //            out var t, out localNormal);
+        //    }
+        //    Vector3Wide.Normalize(localNormal, out var test);
+
+        //    GJKDistanceTester<Cylinder, CylinderWide, CylinderSupportFinder, Cylinder, CylinderWide, CylinderSupportFinder> 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<Cylinder, CylinderWide, CylinderSupportFinder, Cylinder, CylinderWide, CylinderSupportFinder>.Test(a, b, localOffsetB, localOrientationB, ref supportFinderA, ref supportFinderB, new Vector<float>(1e-5f), Vector<int>.Zero, out var intersecting, out var localNormal);
+        //    Vector3Wide.Normalize(localNormal, out var test);
+        //    GJKDistanceTester<Cylinder, CylinderWide, CylinderSupportFinder, Cylinder, CylinderWide, CylinderSupportFinder> 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/DepthRefinerTestDemo.cs b/Demos/SpecializedTests/DepthRefinerTestDemo.cs
index a1d822742..d18f58300 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,124 +32,55 @@
 //            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)));
-//            //{
-//            //    //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<float>(0.007193089f);
-
-//            //    var convergenceThreshold = new Vector<float>(4e-7f);
-
-//            //    var minimumDepthThreshold = new Vector<float>(-0.1f);
-
-//            //    //var simplex = new DepthRefiner<Cylinder, CylinderWide, CylinderSupportFinder, Triangle, TriangleWide, PretransformedTriangleSupportFinder>.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<int>(-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<int>(-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<int>(-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<Cylinder, CylinderWide, CylinderSupportFinder, Triangle, TriangleWide, PretransformedTriangleSupportFinder>(
-//            //        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<DepthRefinerStep>();
-//            //    //DepthRefiner<Cylinder, CylinderWide, CylinderSupportFinder, Triangle, TriangleWide, PretransformedTriangleSupportFinder>.FindMinimumDepth(
-//            //    //    aWide, bWide, localOffsetB, localOrientationB, ref cylinderSupportFinder, ref triangleSupportFinder, ref simplex, initialNormal, initialDepth, new Vector<int>(), convergenceThreshold, minimumDepthThreshold,
-//            //    //    out var depthWide, out var localNormalWide, out var witnessOnA, steps, 50);
-//            //    DepthRefiner<Cylinder, CylinderWide, CylinderSupportFinder, Triangle, TriangleWide, PretransformedTriangleSupportFinder>.FindMinimumDepth(
-//            //        aWide, bWide, localOffsetB, localOrientationB, ref cylinderSupportFinder, ref triangleSupportFinder, initialNormal, new Vector<int>(), convergenceThreshold, minimumDepthThreshold,
-//            //        out var depthWide, out var localNormalWide, out var witnessOnA, steps, 50);
-
-//            //}
-
+//            Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, 0, 0)), new SolveDescription(8, 1));
 //            {
-//                const int pointCount = 16;
-//                var randomizedPoints = new QuickList<Vector3>(pointCount * 2, BufferPool);
-//                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());
-//                }
-//                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<float>(0.007193089f);
+
+//                var convergenceThreshold = new Vector<float>(4e-7f);
+
+//                var minimumDepthThreshold = new Vector<float>(-0.1f);
 
-//                //Vector3Wide.Normalize(localOffsetB, out var initialNormal);
-//                Vector3Wide.Broadcast(new Vector3(1f, 0, 0), out var initialNormal);
-//                var initialDepth = new Vector<float>(2.674457f);
+//                //var simplex = new DepthRefiner<Cylinder, CylinderWide, CylinderSupportFinder, Triangle, TriangleWide, PretransformedTriangleSupportFinder>.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<int>(-1);
 
-//                var convergenceThreshold = new Vector<float>(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<int>(-1);
 
-//                var minimumDepthThreshold = new Vector<float>(-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<int>(-1);
 
 //                basePosition = default;
 //                var poseA = RigidPose.Identity;
 //                Matrix3x3Wide.ReadSlot(ref localOrientationB, 0, out var orientationBNarrow);
 //                RigidPose poseB;
-//                Quaternion.CreateFromRotationMatrix(orientationBNarrow, out poseB.Orientation);
+//                QuaternionEx.CreateFromRotationMatrix(orientationBNarrow, out poseB.Orientation);
 //                Vector3Wide.ReadSlot(ref localOffsetB, 0, out poseB.Position);
-//                shapeLines = MinkowskiShapeVisualizer.CreateLines<ConvexHull, ConvexHullWide, ConvexHullSupportFinder, Triangle, TriangleWide, PretransformedTriangleSupportFinder>(
+//                shapeLines = MinkowskiShapeVisualizer.CreateLines<Cylinder, CylinderWide, CylinderSupportFinder, Triangle, TriangleWide, PretransformedTriangleSupportFinder>(
 //                    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<ConvexHull>() * Vector<float>.Count;
-//                var memory = stackalloc byte[memoryLength];
-//                aWide.Initialize(new RawBuffer(memory, memoryLength));
+//                var aWide = default(CylinderWide);
 //                var bWide = default(TriangleWide);
 //                aWide.Broadcast(shapeA);
 //                bWide.Broadcast(shapeB);
@@ -160,23 +90,7 @@
 //                //Vector3Wide.Broadcast(localOffsetB, out var localOffsetBWide);
 //                //Matrix3x3Wide.Broadcast(localOrientationB, out var localOrientationBWide);
 //                var triangleSupportFinder = default(PretransformedTriangleSupportFinder);
-//                var convexHullSupportFinder = default(ConvexHullSupportFinder);
-
-
-//                DepthRefiner<ConvexHull, ConvexHullWide, ConvexHullSupportFinder, Triangle, TriangleWide, PretransformedTriangleSupportFinder>.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<int>(-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<int>(-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<int>(-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<int>(-1);
+//                var cylinderSupportFinder = default(CylinderSupportFinder);
 
 //                //var initialNormal = Vector3.Normalize(localOffsetB);
 //                //Vector3Wide.Broadcast(initialNormal, out var initialNormalWide);
@@ -184,23 +98,112 @@
 //                //DepthRefiner<Cylinder, CylinderWide, CylinderSupportFinder, Triangle, TriangleWide, PretransformedTriangleSupportFinder>.FindMinimumDepth(
 //                //    aWide, bWide, localOffsetB, localOrientationB, ref cylinderSupportFinder, ref triangleSupportFinder, ref simplex, initialNormal, initialDepth, new Vector<int>(), convergenceThreshold, minimumDepthThreshold,
 //                //    out var depthWide, out var localNormalWide, out var witnessOnA, steps, 50);
-//                var inactiveLanes = new Vector<int>(-1);
-//                Unsafe.As<Vector<int>, int>(ref inactiveLanes) = 0;
-//                //DepthRefiner<ConvexHull, ConvexHullWide, ConvexHullSupportFinder, Triangle, TriangleWide, PretransformedTriangleSupportFinder>.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<ConvexHull, ConvexHullWide, ConvexHullSupportFinder, Triangle, TriangleWide, PretransformedTriangleSupportFinder>.FindMinimumDepth(
-//                    aWide, bWide, localOffsetB, localOrientationB, ref convexHullSupportFinder, ref triangleSupportFinder, initialNormal, inactiveLanes, convergenceThreshold, minimumDepthThreshold,
+//                DepthRefiner<Cylinder, CylinderWide, CylinderSupportFinder, Triangle, TriangleWide, PretransformedTriangleSupportFinder>.FindMinimumDepth(
+//                    aWide, bWide, localOffsetB, localOrientationB, ref cylinderSupportFinder, ref triangleSupportFinder, initialNormal, new Vector<int>(), 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)));
 
-//                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)));
 //            }
 
+//            //{
+//            //    const int pointCount = 16;
+//            //    var randomizedPoints = new QuickList<Vector3>(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<float>(2.674457f);
+
+//            //    var convergenceThreshold = new Vector<float>(1e-5f * 4.187582f);
+
+//            //    var minimumDepthThreshold = new Vector<float>(-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<ConvexHull, ConvexHullWide, ConvexHullSupportFinder, Triangle, TriangleWide, PretransformedTriangleSupportFinder>(
+//            //        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<ConvexHull>() * Vector<float>.Count;
+//            //    var memory = stackalloc byte[memoryLength];
+//            //    aWide.Initialize(new Buffer<byte>(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<ConvexHull, ConvexHullWide, ConvexHullSupportFinder, Triangle, TriangleWide, PretransformedTriangleSupportFinder>.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<int>(-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<int>(-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<int>(-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<int>(-1);
+
+//            //    //var initialNormal = Vector3.Normalize(localOffsetB);
+//            //    //Vector3Wide.Broadcast(initialNormal, out var initialNormalWide);
+//            //    steps = new List<DepthRefinerStep>();
+//            //    //DepthRefiner<Cylinder, CylinderWide, CylinderSupportFinder, Triangle, TriangleWide, PretransformedTriangleSupportFinder>.FindMinimumDepth(
+//            //    //    aWide, bWide, localOffsetB, localOrientationB, ref cylinderSupportFinder, ref triangleSupportFinder, ref simplex, initialNormal, initialDepth, new Vector<int>(), convergenceThreshold, minimumDepthThreshold,
+//            //    //    out var depthWide, out var localNormalWide, out var witnessOnA, steps, 50);
+//            //    var inactiveLanes = new Vector<int>(-1);
+//            //    Unsafe.As<Vector<int>, int>(ref inactiveLanes) = 0;
+//            //    //DepthRefiner<ConvexHull, ConvexHullWide, ConvexHullSupportFinder, Triangle, TriangleWide, PretransformedTriangleSupportFinder>.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<ConvexHull, ConvexHullWide, ConvexHullSupportFinder, Triangle, TriangleWide, PretransformedTriangleSupportFinder>.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);
@@ -231,12 +234,12 @@
 //            //    var aWide = default(CapsuleWide);
 //            //    var bWide = default(ConvexHullWide);
 //            //    aWide.Broadcast(shapeA);
-//            //    BufferPool.Take(bWide.InternalAllocationSize, out var memory);
+//            //    BufferPool.Take<byte>(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);
@@ -252,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);
@@ -294,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<Cylinder, CylinderWide, CylinderSupportFinder, Cylinder, CylinderWide, CylinderSupportFinder>(
@@ -308,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);
@@ -358,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);
diff --git a/Demos/SpecializedTests/DeterminismTest.cs b/Demos/SpecializedTests/DeterminismTest.cs
index 77da433a0..7064a12bc 100644
--- a/Demos/SpecializedTests/DeterminismTest.cs
+++ b/Demos/SpecializedTests/DeterminismTest.cs
@@ -1,79 +1,73 @@
-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
+namespace Demos.SpecializedTests;
+
+public static class DeterminismTest<T> where T : Demo, new()
 {
-    public static class DeterminismTest<T> where T : Demo, new()
+    static Dictionary<int, MotionState> ExecuteSimulation(ContentArchive content, int frameCount)
     {
-        struct MotionState
+        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)
         {
-            public RigidPose Pose;
-            public BodyVelocity Velocity;
+            demo.Update(null, null, null, Demo.TimestepDuration);
+            //InvasiveHashDiagnostics.Instance.MoveToNextHashFrame();
+            if ((i + 1) % 32 == 0)
+                Console.Write($"{i + 1}, ");
         }
-        static Dictionary<int, MotionState> ExecuteSimulation(ContentArchive content, int frameCount)
+        var motionStates = new Dictionary<int, MotionState>();
+        for (int setIndex = 0; setIndex < demo.Simulation.Bodies.Sets.Length; ++setIndex)
         {
-            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, 1 / 60f);
-                if ((i + 1) % 32 == 0)
-                    Console.Write($"{i + 1}, ");
-            }
-            var motionStates = new Dictionary<int, MotionState>();
-            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, new MotionState { Pose = set.Poses[bodyIndex], Velocity = set.Velocities[bodyIndex] });
-                    }
+                    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)
         {
-            var initialStates = ExecuteSimulation(archive, frameCount);
-            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.");
+            foreach (var state in states)
             {
-                var states = ExecuteSimulation(archive, frameCount);
-                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
new file mode 100644
index 000000000..46a4a5a41
--- /dev/null
+++ b/Demos/SpecializedTests/FountainStressTestDemo.cs
@@ -0,0 +1,413 @@
+using BepuUtilities;
+using DemoRenderer;
+using DemoUtilities;
+using BepuPhysics;
+using BepuPhysics.Collidables;
+using System;
+using System.Numerics;
+using BepuUtilities.Memory;
+using BepuUtilities.Collections;
+using System.Diagnostics;
+using DemoContentLoader;
+using BepuPhysics.Constraints;
+
+namespace Demos.SpecializedTests;
+
+public class FountainStressTestDemo : Demo
+{
+    QuickQueue<StaticDescription> removedStatics;
+    QuickQueue<BodyHandle> 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
+        {
+            Bodies = 1,
+            ConstraintCountPerBodyEstimate = 1,
+            Constraints = 1,
+            ConstraintsPerTypeBatch = 1,
+            Islands = 1,
+            ShapesPerType = 1,
+            Statics = 1
+        });
+
+        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)
+        {
+            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<BodyHandle>(65536, BufferPool);
+        removedStatics = new QuickQueue<StaticDescription>(512, BufferPool);
+        random = new Random(5);
+    }
+
+    double time;
+    double t;
+    BodyHandle[] kinematicHandles;
+
+    void AddConvexShape<TConvex>(in TConvex convex, out TypedIndex shapeIndex, out BodyInertia inertia) where TConvex : unmanaged, IConvexShape
+    {
+        shapeIndex = Simulation.Shapes.Add(convex);
+        inertia = convex.ComputeInertia(1);
+    }
+
+    ConvexHull CreateRandomHull()
+    {
+        const int pointCount = 16;
+        var points = new QuickList<Vector3>(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;
+    }
+
+    void CreateRandomCompound(out Buffer<CompoundChild> children, out BodyInertia inertia)
+    {
+        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);
+        }
+    }
+
+    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<Vector3> 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<Triangle> 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);
+        inertia = mesh.ComputeClosedInertia(1);
+    }
+
+    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))
+            {
+                default:
+                    {
+                        AddConvexShape(new Sphere(0.35f + 0.35f * random.NextSingle()), out shapeIndex, out inertia);
+                    }
+                    break;
+                case 1:
+                    {
+                        AddConvexShape(new Capsule(
+                            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 * 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;
+            }
+        }
+
+        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;
+        }
+    }
+
+    public override void Update(Window window, Camera camera, Input input, float dt)
+    {
+        var timestepDuration = 1f / 60f;
+        time += timestepDuration;
+
+        //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;
+
+        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 (bodyLocation.SetIndex > 0)
+                {
+                    //We're requesting a nonzero velocity, so it must be active.
+                    Simulation.Awakener.AwakenSet(bodyLocation.SetIndex);
+                }
+                if (distance > maxDisplacement)
+                {
+                    correction *= maxDisplacement / distance;
+                }
+                Debug.Assert(bodyLocation.SetIndex == 0);
+                Simulation.Bodies.ActiveSet.DynamicsState[bodyLocation.Index].Motion.Velocity.Linear = correction * inverseDt;
+            }
+            else
+            {
+                if (bodyLocation.SetIndex == 0)
+                {
+                    Simulation.Bodies.ActiveSet.DynamicsState[bodyLocation.Index].Motion.Velocity.Linear = new Vector3();
+                }
+            }
+        }
+
+        //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);
+        }
+
+        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 77a05cebf..ea2166d18 100644
--- a/Demos/SpecializedTests/GyroscopeTestDemo.cs
+++ b/Demos/SpecializedTests/GyroscopeTestDemo.cs
@@ -1,77 +1,74 @@
-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
+namespace Demos.Demos;
+
+struct GyroscopicIntegratorCallbacks : IPoseIntegratorCallbacks
 {
-    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.ConserveMomentumWithGyroscopicTorque;
+    //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);
+    }
 
-        [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public void PrepareForIntegration(float dt)
-        {
-            innerCallbacks.PrepareForIntegration(dt);
-        }
+    public GyroscopicIntegratorCallbacks(Vector3 gravity, float linearDamping, float angularDamping)
+    {
+        innerCallbacks = new DemoPoseIntegratorCallbacks(gravity, linearDamping, angularDamping);
+    }
 
-        [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);
-        }
+    public void PrepareForIntegration(float dt)
+    {
+        innerCallbacks.PrepareForIntegration(dt);
+    }
 
+    public void IntegrateVelocity(Vector<int> bodyIndices, Vector3Wide position, QuaternionWide orientation, BodyInertiaWide localInertia, Vector<int> integrationMask, int workerIndex, Vector<float> 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 GyroscopicIntegratorCallbacks(new Vector3(0, -10, 0), 0f, 0f), new SubsteppingTimestepper(4), 2);
+        //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(), new CollidableDescription(Simulation.Shapes.Add(new Box(100, 1, 100)), 0.1f)));
+        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)));
-            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 RigidPose(new Vector3(-0.5f, 0, 0)), 1);
-            builder.Add(new Box(0.3f, 2f, 0.3f), new RigidPose(new Vector3(0.15f, 0, 0)), 2);
-            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 BodyVelocity(new Vector3(0, 0, 1), new Vector3(5, .001f, .001f)), inertia, new CollidableDescription(dzhanibekovShape, 0.1f), 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) });
-        }
+        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/HeadlessDemo.cs b/Demos/SpecializedTests/HeadlessDemo.cs
index 08c56cfb4..8aee7df8c 100644
--- a/Demos/SpecializedTests/HeadlessDemo.cs
+++ b/Demos/SpecializedTests/HeadlessDemo.cs
@@ -1,65 +1,73 @@
-using BepuPhysics.Collidables;
-using BepuPhysics.CollisionDetection;
-using DemoContentLoader;
-using DemoUtilities;
+using DemoContentLoader;
 using System;
-using System.Collections.Generic;
 using System.Diagnostics;
-using System.Text;
 
-namespace Demos.SpecializedTests
+namespace Demos.SpecializedTests;
+
+static class HeadlessTest
 {
-    static class HeadlessTest
+    public static void Test<T>(ContentArchive content, int runCount, int warmUpFrames, int frameCount) where T : Demo, new()
     {
-        public static void Test<T>(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, 1 / 60f);
-                }
-                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, 1 / 60f);
-                    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.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/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<float> BiasImpulse;
+    //And once again, CFM becomes CFM * EffectiveMass- massively cancels out due to the derivation of CFM. (See prestep notes.)
+    public Vector<float> 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<float> maximumRecoveryVelocity,
+        ref Vector<float> 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<float>.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
+
+
+    /// <summary>
+    /// Transforms an impulse from constraint space to world space, uses it to modify the cached world space velocities of the bodies.
+    /// </summary>
+    [MethodImpl(MethodImplOptions.AggressiveInlining)]
+    public static void ApplyImpulse(ref Projection2Body1DOF data, ref Vector<float> 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<float> 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<float> accumulatedImpulse,
+        out Vector<float> 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<float>.Zero, accumulatedImpulse + csi);
+
+        correctiveCSI = accumulatedImpulse - previousAccumulated;
+    }
+
+    [MethodImpl(MethodImplOptions.AggressiveInlining)]
+    public static void Solve(ref Projection2Body1DOF projection, ref Vector<float> 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/IntertreeThreadingTests.cs b/Demos/SpecializedTests/IntertreeThreadingTests.cs
index eb6b3064a..621ad439c 100644
--- a/Demos/SpecializedTests/IntertreeThreadingTests.cs
+++ b/Demos/SpecializedTests/IntertreeThreadingTests.cs
@@ -1,193 +1,189 @@
-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
+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((float)random.NextDouble(), (float)random.NextDouble(), (float)random.NextDouble()) + 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);
         }
-
-        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(ref 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);
-            }
-            
-            {
-                var indexToRemove = 1;
-                GetBoundsForLeaf(treeB, indexToRemove, out var removedBounds);
-                treeB.RemoveAt(indexToRemove);
-                treeA.Add(ref 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<OverlapHandler>(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.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<OverlapHandler>(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, pool, 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<int>
+        treeA.Dispose(pool);
+        treeB.Dispose(pool);
+    }
+
+    struct BruteForceResultsEnumerator : IBreakableForEach<int>
+    {
+        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 SimpleThreadDispatcher(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/ManifoldReductionScaleTestDemo.cs b/Demos/SpecializedTests/ManifoldReductionScaleTestDemo.cs
new file mode 100644
index 000000000..1a7d9b92f
--- /dev/null
+++ b/Demos/SpecializedTests/ManifoldReductionScaleTestDemo.cs
@@ -0,0 +1,75 @@
+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;
+
+/// <summary>
+/// Stress tests contact manifold reduction heuristics at a variety of scales using box-box tests.
+/// </summary>
+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<float>(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));
+
+                    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
diff --git a/Demos/SpecializedTests/Media/2.0/BedsheetDemo.cs b/Demos/SpecializedTests/Media/2.0/BedsheetDemo.cs
new file mode 100644
index 000000000..5bbbaa425
--- /dev/null
+++ b/Demos/SpecializedTests/Media/2.0/BedsheetDemo.cs
@@ -0,0 +1,159 @@
+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;
+
+
+/// <summary>
+/// Shows a few different examples of cloth-ish constraint lattices.
+/// </summary>
+public class BedsheetDemo : Demo
+{
+    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<ClothCollisionFilter> 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 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<ClothCollisionFilter>();
+        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..219b74084
--- /dev/null
+++ b/Demos/SpecializedTests/Media/2.0/ColosseumVideoDemo.cs
@@ -0,0 +1,160 @@
+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;
+
+/// <summary>
+/// Version of the colosseum demo for video purposes.
+/// </summary>
+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 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), 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);
+    }
+
+
+    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..88cada604
--- /dev/null
+++ b/Demos/SpecializedTests/Media/2.0/NewtDemandingSacrificeVideoDemo.cs
@@ -0,0 +1,77 @@
+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<SubgroupCollisionFilter> 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;
+
+        filters = new CollidableProperty<SubgroupCollisionFilter>(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;
+
+        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;
+
+    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)
+    {
+        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..9ebfdb1e0
--- /dev/null
+++ b/Demos/SpecializedTests/Media/2.0/NewtVideoDemo.cs
@@ -0,0 +1,65 @@
+using BepuPhysics;
+using BepuPhysics.Collidables;
+using BepuPhysics.Constraints;
+using BepuUtilities;
+using DemoContentLoader;
+using DemoRenderer;
+using Demos.Demos;
+using DemoUtilities;
+using System;
+using System.Numerics;
+
+namespace Demos.SpecializedTests.Media;
+
+public class NewtVideoDemo : Demo
+{
+    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;
+
+        var filters = new CollidableProperty<DeformableCollisionFilter>();
+        Simulation = Simulation.Create(BufferPool, new DeformableCallbacks(filters), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(8, 1));
+
+        var meshContent = content.Load<MeshContent>("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);
+
+        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))
+        {
+            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..833e5e1a5
--- /dev/null
+++ b/Demos/SpecializedTests/Media/2.0/PyramidVideoDemo.cs
@@ -0,0 +1,81 @@
+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;
+
+/// <summary>
+/// A pyramid of boxes, because you can't have a physics engine without pyramids of boxes.
+/// </summary>
+public class PyramidVideoDemo : Demo
+{
+    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));
+
+        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..60ccddb87
--- /dev/null
+++ b/Demos/SpecializedTests/Media/2.0/ShrinkwrappedNewtsVideoDemo.cs
@@ -0,0 +1,70 @@
+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 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<MeshContent>("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<Vector3>(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))));
+
+
+
+        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;
+
+    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/Colosseum24VideoDemo.cs b/Demos/SpecializedTests/Media/2.4/Colosseum24VideoDemo.cs
new file mode 100644
index 000000000..a0b7d36c2
--- /dev/null
+++ b/Demos/SpecializedTests/Media/2.4/Colosseum24VideoDemo.cs
@@ -0,0 +1,121 @@
+using BepuPhysics;
+using BepuPhysics.Collidables;
+using BepuPhysics.Constraints;
+using BepuUtilities;
+using DemoContentLoader;
+using DemoRenderer;
+using DemoUtilities;
+using System;
+using System.Numerics;
+
+namespace Demos.SpecializedTests.Media;
+
+/// <summary>
+/// A colosseum made out of boxes that is sometimes hit by large purple hail.
+/// </summary>
+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 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);
+    }
+
+}
diff --git a/Demos/SpecializedTests/Media/2.4/ExcessivePyramidVideoDemo.cs b/Demos/SpecializedTests/Media/2.4/ExcessivePyramidVideoDemo.cs
new file mode 100644
index 000000000..e61d9227d
--- /dev/null
+++ b/Demos/SpecializedTests/Media/2.4/ExcessivePyramidVideoDemo.cs
@@ -0,0 +1,63 @@
+using BepuPhysics;
+using BepuPhysics.Collidables;
+using BepuPhysics.Constraints;
+using BepuUtilities;
+using DemoContentLoader;
+using DemoRenderer;
+using DemoUtilities;
+using System;
+using System.Numerics;
+
+namespace Demos.SpecializedTests.Media;
+
+/// <summary>
+/// A pyramid of boxes, because you can't have a physics engine without pyramids of boxes.
+/// </summary>
+public class ExcessivePyramidVideoDemo : Demo
+{
+    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));
+
+        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)))
+        {
+            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
new file mode 100644
index 000000000..2e8861c59
--- /dev/null
+++ b/Demos/SpecializedTests/Media/2.4/NewtTyrannyDemo.cs
@@ -0,0 +1,110 @@
+using DemoContentLoader;
+using DemoRenderer;
+using System;
+using System.Numerics;
+using BepuPhysics;
+using DemoUtilities;
+using BepuUtilities.Collections;
+using BepuPhysics.Collidables;
+using Demos.Demos.Characters;
+
+namespace Demos.Demos.Sponsors;
+
+public class NewtTyrannyDemo : Demo
+{
+    QuickList<SponsorNewt> newts;
+
+    Vector2 newtArenaMin, newtArenaMax;
+    Random random;
+    CharacterControllers characterControllers;
+    QuickList<SponsorCharacterAI> 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;
+
+        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<SponsorNewt>(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<SponsorCharacterAI>(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, 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
new file mode 100644
index 000000000..63eb4e670
--- /dev/null
+++ b/Demos/SpecializedTests/Media/2.4/RagdollTubeVideoDemo.cs
@@ -0,0 +1,103 @@
+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;
+
+/// <summary>
+/// Subjects a bunch of unfortunate ragdolls to a tumble dry cycle.
+/// </summary>
+public class RagdollTubeVideoDemo : Demo
+{
+    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<SubgroupCollisionFilter>();
+        //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);
+
+        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;
+
+    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/RopeTwistVideoDemo.cs b/Demos/SpecializedTests/Media/2.4/RopeTwistVideoDemo.cs
new file mode 100644
index 000000000..fbc788785
--- /dev/null
+++ b/Demos/SpecializedTests/Media/2.4/RopeTwistVideoDemo.cs
@@ -0,0 +1,104 @@
+using BepuPhysics;
+using BepuPhysics.Collidables;
+using BepuPhysics.CollisionDetection;
+using BepuPhysics.Constraints;
+using DemoContentLoader;
+using DemoRenderer;
+using Demos.Demos;
+using System;
+using System.Numerics;
+
+namespace Demos.SpecializedTests.Media;
+
+/// <summary>
+/// Shows a bundle of ropes being tangled up by spinning weights.
+/// </summary>
+public class RopeTwistVideoDemo : Demo
+{
+    public override void Initialize(ContentArchive content, Camera camera)
+    {
+        camera.Position = new Vector3(0, 20, 20);
+        camera.Yaw = 0;
+        camera.Pitch = 0;
+
+        var filters = new CollidableProperty<RopeFilter>();
+        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))));
+    }
+}
diff --git a/Demos/SpecializedTests/Media/2.4/TankSwarmDemo.cs b/Demos/SpecializedTests/Media/2.4/TankSwarmDemo.cs
new file mode 100644
index 000000000..929a8eb19
--- /dev/null
+++ b/Demos/SpecializedTests/Media/2.4/TankSwarmDemo.cs
@@ -0,0 +1,365 @@
+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<TankDemoBodyProperties> bodyProperties;
+    TankController playerController;
+
+    QuickList<AITank> 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<Explosion> 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<TankDemoBodyProperties>();
+        //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)));
+        }
+
+        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<Explosion>(32, BufferPool);
+
+        //Create the AI tanks.
+        const int aiTankCount = 3072;
+        aiTanks = new QuickList<AITank>(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<int> 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<TankCallbacks>)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), 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);
+
+        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
new file mode 100644
index 000000000..4f9c78a63
--- /dev/null
+++ b/Demos/SpecializedTests/Media/2.4/VideoDancerDemo.cs
@@ -0,0 +1,197 @@
+using BepuPhysics;
+using BepuPhysics.Collidables;
+using BepuPhysics.Constraints;
+using BepuUtilities;
+using DemoContentLoader;
+using DemoRenderer;
+using DemoRenderer.UI;
+using Demos.Demos;
+using Demos.Demos.Dancers;
+using DemoUtilities;
+using System;
+using System.Numerics;
+
+namespace Demos.SpecializedTests.Media;
+
+/// <summary>
+/// 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 <see cref="DemoDancers"/> implementation, this provides a starting point for cosmetic cloth attached to characters. 
+/// </summary>
+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<ClothCollisionFilter> 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<ClothCollisionFilter> 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<Capsule>(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 override void Initialize(ContentArchive content, Camera camera)
+    {
+        camera.Position = new Vector3(0, 2, 10);
+        camera.Yaw = 0;
+        camera.Pitch = 0;
+
+        var collisionFilters = new CollidableProperty<SubgroupCollisionFilter>();
+        Simulation = Simulation.Create(BufferPool, new SubgroupFilteredCallbacks(collisionFilters), new DemoPoseIntegratorCallbacks(new Vector3(0, 0, 0)), new SolveDescription(8, 1));
+
+        dancers = new DemoDancers().Initialize<ClothCallbacks, ClothCollisionFilter>(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 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..9ca6f38b9
--- /dev/null
+++ b/Demos/SpecializedTests/Media/2.4/VideoPlumpDancerDemo.cs
@@ -0,0 +1,253 @@
+using BepuPhysics;
+using BepuPhysics.Collidables;
+using BepuPhysics.Constraints;
+using BepuUtilities;
+using DemoContentLoader;
+using DemoRenderer;
+using DemoRenderer.UI;
+using Demos.Demos;
+using Demos.Demos.Dancers;
+using DemoUtilities;
+using System;
+using System.Diagnostics;
+using System.Numerics;
+
+namespace Demos.SpecializedTests.Media;
+
+/// <summary>
+/// A bunch of somewhat overweight background dancers struggle to keep up with the masterful purple prancer.
+/// Combined with the <see cref="DemoDancers"/> implementation, this shows an example of how cosmetic deformable physics could be applied to characters.
+/// </summary>
+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<Capsule>(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<DeformableCollisionFilter> 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<TestCapsule> 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<DeformableCollisionFilter> 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<Capsule>(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<SubgroupCollisionFilter>();
+        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<DeformableCallbacks, DeformableCollisionFilter>(32, 32, Simulation, collisionFilters, ThreadDispatcher, BufferPool, new SolveDescription(1, 1), CreateFatSuit, new DeformableCollisionFilter(0, 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 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/Media/BedsheetDemo.cs b/Demos/SpecializedTests/Media/BedsheetDemo.cs
deleted file mode 100644
index 55191425e..000000000
--- a/Demos/SpecializedTests/Media/BedsheetDemo.cs
+++ /dev/null
@@ -1,166 +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
-{
-
-    /// <summary>
-    /// Shows a few different examples of cloth-ish constraint lattices.
-    /// </summary>
-    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<ClothCollisionFilter> filters, KinematicDecider isKinematic)
-        {
-            var description = new BodyDescription
-            {
-                Activity = new BodyActivityDescription(0.01f),
-                Collidable = new CollidableDescription(Simulation.Shapes.Add(new Sphere(bodyRadius)), 0.1f),
-                LocalInertia = default,
-                Pose = new RigidPose(default, orientation)
-            };
-            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)
-                {
-                    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]);
-                }
-            }
-        }
-
-        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<ClothCollisionFilter>();
-            //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());
-            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), 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(65.5f, 8f, 20), new CollidableDescription(Simulation.Shapes.Add(new Cylinder(15, 15)), 0.1f)));
-
-
-            {
-                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), new CollidableDescription(Simulation.Shapes.Add(new Box(400, 1, 400)), 0.1f)));
-
-        }
-
-        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 3b900e5ca..000000000
--- a/Demos/SpecializedTests/Media/ColosseumVideoDemo.cs
+++ /dev/null
@@ -1,169 +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
-{
-    /// <summary>
-    /// Version of the colosseum demo for video purposes.
-    /// </summary>
-    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 = new RigidPose(
-                        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 = new RigidPose(
-                    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 PositionFirstTimestepper());
-
-            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 BodyActivityDescription(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), new CollidableDescription(Simulation.Shapes.Add(new Box(500, 1, 500)), 0.1f)));
-
-            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));
-
-            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));
-        }
-
-        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, 1 / 60f);
-                }
-
-                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 3454d1aec..000000000
--- a/Demos/SpecializedTests/Media/NewtDemandingSacrificeVideoDemo.cs
+++ /dev/null
@@ -1,77 +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<SubgroupCollisionFilter> 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<SubgroupCollisionFilter>(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)));
-
-            //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)));
-        }
-
-        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((float)random.NextDouble(), (float)random.NextDouble(), (float)random.NextDouble()) };
-        }
-
-        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 * (float)random.NextDouble(), 31 + 4 * (float)random.NextDouble(), 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);
-            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);
-
-            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 6d713144f..000000000
--- a/Demos/SpecializedTests/Media/NewtVideoDemo.cs
+++ /dev/null
@@ -1,70 +0,0 @@
-using BepuPhysics;
-using BepuPhysics.Collidables;
-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 DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new PositionFirstTimestepper());
-
-            var meshContent = content.Load<MeshContent>("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<Vector3>(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), 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)));
-
-
-
-            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)));
-        }
-
-        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), new CollidableDescription(Simulation.Shapes.Add(mesh), 0.1f)));
-            }
-            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 7bd17fb34..000000000
--- a/Demos/SpecializedTests/Media/PyramidVideoDemo.cs
+++ /dev/null
@@ -1,85 +0,0 @@
-using BepuPhysics;
-using BepuPhysics.Collidables;
-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
-{
-    /// <summary>
-    /// A pyramid of boxes, because you can't have a physics engine without pyramids of boxes.
-    /// </summary>
-    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 DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new PositionFirstTimestepper());
-
-            var boxShape = new Box(1, 1, 1);
-            boxShape.ComputeInertia(1, out var boxInertia);
-            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,
-                            new CollidableDescription(boxIndex, 0.1f),
-                            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)));
-        }
-
-        //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 BodyVelocity(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 fa5e42e58..000000000
--- a/Demos/SpecializedTests/Media/ShrinkwrappedNewtsVideoDemo.cs
+++ /dev/null
@@ -1,68 +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<DeformableCollisionFilter>();
-            Simulation = Simulation.Create(BufferPool, new DeformableCallbacks { Filters = filters }, new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new PositionFirstTimestepper());
-
-            var meshContent = content.Load<MeshContent>("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), 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)));
-        }
-        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/MeshMeshTestDemo.cs b/Demos/SpecializedTests/MeshMeshTestDemo.cs
index 375adaf6c..fd530e994 100644
--- a/Demos/SpecializedTests/MeshMeshTestDemo.cs
+++ b/Demos/SpecializedTests/MeshMeshTestDemo.cs
@@ -1,56 +1,51 @@
-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
+namespace Demos.SpecializedTests;
+
+/// <summary>
+/// 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.
+/// </summary>
+public class MeshMeshTestDemo : Demo
 {
-    /// <summary>
-    /// 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.
-    /// </summary>
-    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 DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new PositionFirstTimestepper());
+        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);
-            new Box(2.5f, 1, 4).ComputeInertia(1, out var approximateInertia);
-            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,
-                    new CollidableDescription(meshShapeIndex, 0.1f), new BodyActivityDescription(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, new CollidableDescription(compoundShapeIndex, 0.1f), new BodyActivityDescription(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), new CollidableDescription(staticShapeIndex, 0.1f)));
-        }
+        Simulation.Statics.Add(new StaticDescription(new Vector3(0, -0.5f, 0), staticShapeIndex));
     }
 }
diff --git a/Demos/SpecializedTests/MeshReductionTestDemo.cs b/Demos/SpecializedTests/MeshReductionTestDemo.cs
new file mode 100644
index 000000000..dc3fee749
--- /dev/null
+++ b/Demos/SpecializedTests/MeshReductionTestDemo.cs
@@ -0,0 +1,170 @@
+using System;
+using System.Numerics;
+using BepuPhysics;
+using BepuPhysics.Collidables;
+using BepuPhysics.Constraints;
+using BepuUtilities;
+using BepuUtilities.Collections;
+using DemoContentLoader;
+using DemoRenderer;
+
+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;
+
+        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);
+
+
+
+        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);
+
+
+        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<Vector3>(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<Vector3>(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)
+                {
+                    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;
+                        //    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 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/MeshSerializationTestDemo.cs b/Demos/SpecializedTests/MeshSerializationTestDemo.cs
index 6fd44811f..882945681 100644
--- a/Demos/SpecializedTests/MeshSerializationTestDemo.cs
+++ b/Demos/SpecializedTests/MeshSerializationTestDemo.cs
@@ -6,49 +6,48 @@
 using BepuPhysics;
 using BepuPhysics.Collidables;
 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<byte>(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 DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new PositionFirstTimestepper());
-
-            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)));
-            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), new CollidableDescription(Simulation.Shapes.Add(loadedMesh), 0.1f)));
-
-
-            BufferPool.Return(ref serializedMeshBytes);
-
-            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));
-            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());
-                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 d8559f5a9..95a2fc1b7 100644
--- a/Demos/SpecializedTests/MeshTestDemo.cs
+++ b/Demos/SpecializedTests/MeshTestDemo.cs
@@ -1,112 +1,93 @@
 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;
 
-namespace Demos.SpecializedTests
+namespace Demos.SpecializedTests;
+
+public class MeshTestDemo : Demo
 {
-    public class MeshTestDemo : Demo
+    public override void Initialize(ContentArchive content, Camera camera)
     {
-        public unsafe 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 DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new PositionFirstTimestepper());
-
-            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 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 = new BodyDescription
-                        {
-                            Activity = new BodyActivityDescription { MinimumTimestepCountUnderThreshold = 32, SleepThreshold = 0.01f },
-                            Pose = new RigidPose
-                            {
-                                Orientation = Quaternion.Identity,
-                                Position = location
-                            },
-                            Collidable = new CollidableDescription
-                            {
-                                Continuity = new ContinuousDetectionSettings { Mode = ContinuousDetectionMode.Discrete },
-                                SpeculativeMargin = 0.1f
-                            }
-                        };
-                        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)));
 
 
-            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)));
+        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), new CollidableDescription(Simulation.Shapes.Add(new Box(15, 1, 15)), 0.1f)));
+        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), 0.1f)));
+        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);
-            Simulation.Statics.Add(new StaticDescription(new Vector3(-10, 0, -20), new CollidableDescription(Simulation.Shapes.Add(fanMesh), 0.1f)));
+        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,
-                (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(64, -10, 64), BepuUtilities.QuaternionEx.CreateFromAxisAngle(new Vector3(0, 1, 0), MathF.PI / 2),
-                new CollidableDescription(Simulation.Shapes.Add(planeMesh), 0.1f)));
-        }
+        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 396396d8c..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<Vector3> simplex, Vector3 position, Vector3 lineColor, Vector3 backgroundColor)
     {
-        public static void Draw(Renderer renderer, Buffer<Vector3> simplex, in Vector3 position, in Vector3 lineColor, in 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<TShapeA, TShapeWideA, TSupportFinderA, TShapeB, TShapeWideB, TSupportFinderB>
+        (in TShapeWideA a, in TShapeWideB b, in Vector3Wide localOffsetB, in Matrix3x3Wide localOrientationB, ref TSupportFinderA supportFinderA, ref TSupportFinderB supportFinderB, in Vector3Wide direction,
+        in Vector<int> terminatedLanes, out Vector3Wide support)
+        where TShapeA : IConvexShape
+        where TShapeWideA : IShapeWide<TShapeA>
+        where TSupportFinderA : ISupportFinder<TShapeA, TShapeWideA>
+        where TShapeB : IConvexShape
+        where TShapeWideB : IShapeWide<TShapeB>
+        where TSupportFinderB : ISupportFinder<TShapeB, TShapeWideB>
     {
-        [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        static void FindSupport<TShapeA, TShapeWideA, TSupportFinderA, TShapeB, TShapeWideB, TSupportFinderB>
-            (in TShapeWideA a, in TShapeWideB b, in Vector3Wide localOffsetB, in Matrix3x3Wide localOrientationB, ref TSupportFinderA supportFinderA, ref TSupportFinderB supportFinderB, in Vector3Wide direction,
-            in Vector<int> terminatedLanes, out Vector3Wide support)
-            where TShapeA : IConvexShape
-            where TShapeWideA : IShapeWide<TShapeA>
-            where TSupportFinderA : ISupportFinder<TShapeA, TShapeWideA>
-            where TShapeB : IConvexShape
-            where TShapeWideB : IShapeWide<TShapeB>
-            where TSupportFinderB : ISupportFinder<TShapeB, TShapeWideB>
-        {
-            //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<LineInstance> CreateLines<TShapeA, TShapeWideA, TSupportFinderA, TShapeB, TShapeWideB, TSupportFinderB>(
-            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)
-            where TShapeA : unmanaged, IConvexShape
-            where TShapeWideA : unmanaged, IShapeWide<TShapeA>
-            where TSupportFinderA : struct, ISupportFinder<TShapeA, TShapeWideA>
-            where TShapeB : unmanaged, IConvexShape
-            where TShapeWideB : unmanaged, IShapeWide<TShapeB>
-            where TSupportFinderB : struct, ISupportFinder<TShapeB, TShapeWideB>
+    public unsafe static Buffer<LineInstance> CreateLines<TShapeA, TShapeWideA, TSupportFinderA, TShapeB, TShapeWideB, TSupportFinderB>(
+        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<TShapeA>
+        where TSupportFinderA : struct, ISupportFinder<TShapeA, TShapeWideA>
+        where TShapeB : unmanaged, IConvexShape
+        where TShapeWideB : unmanaged, IShapeWide<TShapeB>
+        where TSupportFinderB : struct, ISupportFinder<TShapeB, TShapeWideB>
+    {
+        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 RawBuffer(memory, aWide.InternalAllocationSize));
-            }
-            if (bWide.InternalAllocationSize > 0)
-            {
-                var memory = stackalloc byte[bWide.InternalAllocationSize];
-                bWide.Initialize(new RawBuffer(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<LineInstance>(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<TShapeA, TShapeWideA, TSupportFinderA, TShapeB, TShapeWideB, TSupportFinderB>(aWide, bWide, localOffsetBWide, localOrientationBWide, ref supportFinderA, ref supportFinderB, sampleDirectionWide, Vector<int>.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<byte>(memory, aWide.InternalAllocationSize));
+        }
+        if (bWide.InternalAllocationSize > 0)
+        {
+            var memory = stackalloc byte[bWide.InternalAllocationSize];
+            bWide.Initialize(new Buffer<byte>(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<LineInstance>(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<TShapeA, TShapeWideA, TSupportFinderA, TShapeB, TShapeWideB, TSupportFinderB>(aWide, bWide, localOffsetBWide, localOrientationBWide, ref supportFinderA, ref supportFinderB, sampleDirectionWide, Vector<int>.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<LineInstance> lines, Renderer renderer)
+    public static void Draw(Buffer<LineInstance> 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
new file mode 100644
index 000000000..7184c5856
--- /dev/null
+++ b/Demos/SpecializedTests/NewtonsCradleDemo.cs
@@ -0,0 +1,60 @@
+using BepuPhysics;
+using BepuPhysics.Collidables;
+using BepuPhysics.Constraints;
+using DemoContentLoader;
+using DemoRenderer;
+using DemoUtilities;
+using System;
+using System.Numerics;
+
+namespace Demos.SpecializedTests;
+
+/// <summary>
+/// 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.
+/// </summary>
+public class NewtonsCradleDemo : Demo
+{
+    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)
+        {
+            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));
+    }
+
+}
diff --git a/Demos/SpecializedTests/PyramidAwakenerTestDemo.cs b/Demos/SpecializedTests/PyramidAwakenerTestDemo.cs
index d50f8609f..6005f6690 100644
--- a/Demos/SpecializedTests/PyramidAwakenerTestDemo.cs
+++ b/Demos/SpecializedTests/PyramidAwakenerTestDemo.cs
@@ -1,83 +1,75 @@
 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
+namespace Demos.Demos;
+
+/// <summary>
+/// Repeatedly checks for bugs related to multithreaded awakening and narrow phase flushing.
+/// </summary>
+public class PyramidAwakenerTestDemo : Demo
 {
-    /// <summary>
-    /// Repeatedly checks for bugs related to multithreaded awakening and narrow phase flushing.
-    /// </summary>
-    public class PyramidAwakenerTestDemo : Demo
+    public override void Initialize(ContentArchive content, Camera camera)
     {
-        public unsafe 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 DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new PositionFirstTimestepper());
+        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);
-            boxShape.ComputeInertia(1, out var boxInertia);
-            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,
-                            new CollidableDescription(boxIndex, 0.1f),
-                            new BodyActivityDescription(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), new CollidableDescription(staticShapeIndex, 0.1f)));
+        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)
+    {
+        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);
         }
-        
-        int frameIndex;
-        Random random = new Random(5);
-        public override void Update(Window window, Camera camera, Input input, float dt)
+        if (frameIndex % 192 == 0)
         {
-            frameIndex++;
-            if (frameIndex % 64 == 0)
-            {
-                var bulletShape = new Sphere(0.5f + 5 * (float)random.NextDouble());
-                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);
-                Simulation.Bodies.Add(bodyDescription);
-            }
-            if (frameIndex % 192 == 0)
-            {
-                Simulation.Dispose();
-                BufferPool.Clear();
-                for (int i = 0; i < ThreadDispatcher.ThreadCount; ++i)
-                    ThreadDispatcher.GetThreadMemoryPool(i).Clear();
-                Initialize(null, camera);
-            }
-            base.Update(window, camera, input, dt);
+            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/RagdollTubeDemo.cs b/Demos/SpecializedTests/RagdollTubeDemo.cs
deleted file mode 100644
index 2b51d1325..000000000
--- a/Demos/SpecializedTests/RagdollTubeDemo.cs
+++ /dev/null
@@ -1,83 +0,0 @@
-using BepuUtilities;
-using DemoRenderer;
-using BepuPhysics;
-using BepuPhysics.Collidables;
-using System.Numerics;
-using System;
-using DemoContentLoader;
-using Demos.Demos;
-
-namespace Demos.SpecializedTests
-{
-    /// <summary>
-    /// Subjects a bunch of unfortunate ragdolls to a tumble dry cycle.
-    /// </summary>
-    public class RagdollTubeDemo : 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<SubgroupCollisionFilter>();
-            Simulation = Simulation.Create(BufferPool, new SubgroupFilteredCallbacks { CollisionFilters = filters }, new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new PositionFirstTimestepper());
-
-            int ragdollIndex = 0;
-            var spacing = new Vector3(1.7f, 1.8f, 0.5f);
-            int width = 4;
-            int height = 4;
-            int length = 44;
-            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);
-                    }
-                }
-            }
-
-            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, new RigidPose(position, rotation), 1);
-            }
-            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()));
-            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
-                }
-            };
-            Simulation.Statics.Add(staticDescription);
-        }
-
-    }
-}
-
-
diff --git a/Demos/SpecializedTests/RayTesting.cs b/Demos/SpecializedTests/RayTesting.cs
index 6ac818fc8..6e56023f2 100644
--- a/Demos/SpecializedTests/RayTesting.cs
+++ b/Demos/SpecializedTests/RayTesting.cs
@@ -4,486 +4,504 @@
 using System;
 using System.Numerics;
 using System.Runtime.CompilerServices;
-namespace Demos.SpecializedTests
+namespace Demos.SpecializedTests;
+
+public interface IRayTester<T> 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<Sphere>
 {
-    public interface IRayTester<T> where T : IShape
+    public static void GetRandomShape(Random random, out Sphere shape)
     {
-        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);
+        const float sizeMin = 0.1f;
+        const float sizeSpan = 200;
+        shape = new Sphere(sizeMin + sizeSpan * random.NextSingle());
     }
-    public struct SphereRayTester : IRayTester<Sphere>
+    public static void GetPointInVolume(Random random, float innerMargin, ref Sphere shape, out Vector3 localPoint)
     {
-        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());
-        }
-        public 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((float)random.NextDouble(), (float)random.NextDouble(), (float)random.NextDouble());
+            localPoint = min + span * new Vector3(random.NextSingle(), random.NextSingle(), random.NextSingle());
 
-            } while (localPoint.LengthSquared() > radiusSquared);
-        }
+        } while (localPoint.LengthSquared() > radiusSquared);
+    }
 
-        public 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 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<Capsule>
+public struct CapsuleRayTester : IRayTester<Capsule>
+{
+    public static void GetRandomShape(Random random, out Capsule shape)
     {
-        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());
-        }
-        public 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((float)random.NextDouble(), (float)random.NextDouble(), (float)random.NextDouble());
-                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 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((float)random.NextDouble(), (float)random.NextDouble(), (float)random.NextDouble());
-                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 bool PointIsOnSurface(ref Capsule capsule, ref Vector3 localPoint)
+public struct CylinderRayTester : IRayTester<Cylinder>
+{
+    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<Cylinder>
+    public static void GetSurface(Random random, ref Cylinder cylinder, out Vector3 localPointOnCylinder, out Vector3 localNormal)
     {
-        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());
-        }
-        public 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((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);
+            } 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 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((float)random.NextDouble(), (float)random.NextDouble());
-                    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);
-            }
-            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((float)random.NextDouble(), (float)random.NextDouble());
-                    distanceSquared = randomHorizontal.LengthSquared();
-
-                } while (distanceSquared < cylinder.Radius * cylinder.Radius);
-                localPointOnCylinder = new Vector3(randomHorizontal.X, upperCap ? cylinder.HalfLength : -cylinder.HalfLength, randomHorizontal.Y);
-            }
-        }
+                randomHorizontal = min + span * new Vector2(random.NextSingle(), random.NextSingle());
+                distanceSquared = randomHorizontal.LengthSquared();
 
-        public 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<Box>
+    public static bool PointIsOnSurface(ref Cylinder cylinder, ref Vector3 localPoint)
     {
-        public 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 * (float)random.NextDouble(), sizeMin * sizeSpan * (float)random.NextDouble(), sizeMin * sizeSpan * (float)random.NextDouble());
+            //Too far up or down.
+            return false;
         }
-        public 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((float)random.NextDouble(), (float)random.NextDouble(), (float)random.NextDouble());
+            //Too far out.
+            return false;
         }
-
-        public void GetSurface(Random random, ref Box box, out Vector3 localPoint, out Vector3 localNormal)
+        if (MathF.Abs(localPoint.Y) > cylinder.HalfLength - epsilon)
         {
-            var a = (float)random.NextDouble();
-            var b = (float)random.NextDouble();
-            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 bool PointIsOnSurface(ref Box box, ref Vector3 localPoint)
+public struct BoxRayTester : IRayTester<Box>
+{
+    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 class RayTesting
+    public static bool PointIsOnSurface(ref Box box, ref Vector3 localPoint)
     {
-        internal static void GetUnitDirection(Random random, out Vector3 direction)
+        //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
+{
+    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
         {
             //Not much cleverness involved here. This does not produce a uniform distribution over the the unit sphere.
             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;
         }
-        static void GetUnitQuaternion(Random random, out Quaternion orientation)
+    }
+    static void GetUnitQuaternion(Random random, out Quaternion orientation)
+    {
+        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
         {
-            //Not much cleverness involved here. This does not produce a uniform distribution over the the unit sphere.
             float length;
             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<Quaternion, Vector4>(ref orientation) /= length;
         }
-        static void GetPointOnPlane(Random random, float centralExclusion, float span, ref Vector3 anchor, ref Vector3 normal, out Vector3 point)
+    }
+    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);
 
-            Vector2 localPoint;
-            var exclusionSquared = centralExclusion * centralExclusion;
-            do
-            {
-                localPoint = span * (new Vector2((float)random.NextDouble(), (float)random.NextDouble()) - new Vector2(0.5f));
-            } while (localPoint.LengthSquared() < exclusionSquared);
+        Vector3 basisX;
+        float basisXLengthSquared;
+        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;
+    }
 
-            Vector3 basisX;
-            float basisXLengthSquared;
-            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;
-        }
+    static void CheckWide<TShape, TShapeWide>(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<TShape>
+    {
+        RayWide rayWide;
+        Vector3Wide.Broadcast(origin, out rayWide.Origin);
+        Vector3Wide.Broadcast(direction, out rayWide.Direction);
 
-        static void CheckWide<TShape, TShapeWide>(ref RigidPoses poses, ref TShapeWide shapeWide, ref Vector3 origin, ref Vector3 direction, bool intersected, float t, ref Vector3 normal)
-                            where TShape : IConvexShape where TShapeWide : IShapeWide<TShape>
+        shapeWide.RayTest(ref poses, ref rayWide, out var intersectedWide, out var tWide, out var normalWide);
+        if (intersectedWide[0] < 0 != intersected)
         {
-            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)
+            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<TShape, TShapeWide, TTester>() where TShape : IConvexShape where TTester : struct, IRayTester<TShape> where TShapeWide : unmanaged, IShapeWide<TShape>
+    {
+        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 positionBoundsSpan = 100;
+        const float positionMin = positionBoundsSpan * -0.5f;
+
+        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 outwardPointingSpan = 1000f;
 
-        static void Test<TShape, TShapeWide, TTester>() where TShape : IConvexShape where TTester : struct, IRayTester<TShape> where TShapeWide : unmanaged, IShapeWide<TShape>
+        var random = new Random(5);
+        TShapeWide shapeWide = default;
+        for (int shapeIteration = 0; shapeIteration < shapeIterations; ++shapeIteration)
         {
-            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 positionBoundsSpan = 100;
-            const float positionMin = positionBoundsSpan * -0.5f;
-
-            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 outwardPointingSpan = 1000f;
-
-            var tester = default(TTester);
-            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)
             {
-                tester.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((float)random.NextDouble(), (float)random.NextDouble(), (float)random.NextDouble());
-                    GetUnitQuaternion(random, out pose.Orientation);
-                    Matrix3x3.CreateFromQuaternion(pose.Orientation, out var orientation);
-                    RigidPoses 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))
                     {
-                        tester.GetSurface(random, ref shape, out var pointOnSurface, out var normal);
-                        var localSourcePoint = pointOnSurface + normal * (outsideMinimumDistance + (float)random.NextDouble() * 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 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))
                         {
-                            //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 (!tester.PointIsOnSurface(ref shape, ref localHitLocation))
-                            {
-                                Console.WriteLine("Outside->inside ray detected non-surface impact.");
-                            }
+                            Console.WriteLine("Outside->inside ray detected non-surface impact.");
                         }
-                        else
-                        {
-                            Console.WriteLine($"Outside->inside ray detected no hit.");
-                        }
-                        CheckWide<TShape, TShapeWide>(ref poses, ref shapeWide, ref sourcePoint, ref direction, intersected, t, ref rayTestedNormal);
                     }
-                    for (int rayIndex = 0; rayIndex < insideRays; ++rayIndex)
+                    else
                     {
-                        tester.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<TShape, TShapeWide>(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 * (float)random.NextDouble());
-                        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<TShape, TShapeWide>(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.
-                        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();
-                        GetPointOnPlane(random, exclusion, span, ref localTargetPoint, ref localNormal, out var localSourcePoint);
-                        var directionScale = (0.01f + 2 * (float)random.NextDouble());
-                        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<TShape, TShapeWide>(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<TShape, TShapeWide>(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))
                     {
-                        tester.GetSurface(random, ref shape, out var pointOnSurface, out var localNormal);
-                        var localSourcePoint = pointOnSurface + localNormal * (tangentMinimumDistance + (float)random.NextDouble() * 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());
-                        } while (Vector3.Dot(localTargetPoint - localSourcePoint, localNormal) < 0);
-                        var directionScale = (0.01f + 2 * (float)random.NextDouble());
-                        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<TShape, TShapeWide>(ref poses, ref shapeWide, ref sourcePoint, ref direction, intersected, t, ref rayTestedNormal);
+                        Console.WriteLine($"Outside ray incorrectly detected an impact.");
+                    }
+                    CheckWide<TShape, TShapeWide>(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<TShape, TShapeWide>(ref poses, ref shapeWide, ref sourcePoint, ref direction, intersected, t, ref rayTestedNormal);
                 }
             }
         }
+    }
 
-        public static void Test()
-        {
-            Test<Sphere, SphereWide, SphereRayTester>();
-            Test<Capsule, CapsuleWide, CapsuleRayTester>();
-            Test<Cylinder, CylinderWide, CylinderRayTester>();
-            Test<Box, BoxWide, BoxRayTester>();
-        }
+    public static void Test()
+    {
+        Test<Sphere, SphereWide, SphereRayTester>();
+        Test<Capsule, CapsuleWide, CapsuleRayTester>();
+        Test<Cylinder, CylinderWide, CylinderRayTester>();
+        Test<Box, BoxWide, BoxRayTester>();
     }
 }
diff --git a/Demos/SpecializedTests/ScalarIntegrationTestDemo.cs b/Demos/SpecializedTests/ScalarIntegrationTestDemo.cs
new file mode 100644
index 000000000..1d724f148
--- /dev/null
+++ b/Demos/SpecializedTests/ScalarIntegrationTestDemo.cs
@@ -0,0 +1,187 @@
+using BepuPhysics.Collidables;
+using BepuPhysics.Constraints;
+using BepuPhysics;
+using BepuUtilities;
+using DemoContentLoader;
+using DemoRenderer;
+using System;
+using System.Numerics;
+using BepuUtilities.Collections;
+
+namespace Demos.SpecializedTests;
+
+public unsafe class ScalarIntegrationTestDemo : Demo
+{
+    struct ScalarIntegrationCallbacks : IPoseIntegratorCallbacks
+    {
+        public delegate*<int, Vector3, Quaternion, BodyInertia, int, float, BodyVelocity*, void> 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<int> bodyIndices, Vector3Wide position, QuaternionWide orientation, BodyInertiaWide localInertia, Vector<int> integrationMask, int workerIndex, Vector<float> 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<float>.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 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<Vector3>(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))));
+        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 d969aaab2..82d10c609 100644
--- a/Demos/SpecializedTests/ShapePileTestDemo.cs
+++ b/Demos/SpecializedTests/ShapePileTestDemo.cs
@@ -1,119 +1,135 @@
 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;
 
-namespace Demos.SpecializedTests
+namespace Demos.SpecializedTests;
+
+public class ShapePileTestDemo : Demo
 {
-    public class ShapePileTestDemo : Demo
+    public override void Initialize(ContentArchive content, Camera camera)
     {
-        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;
-            Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new PositionFirstTimestepper());
-            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);
-            const int pointCount = 32;
-            var points = new QuickList<Vector3>(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 = 8;
-            const int height = 16;
-            const int length = 8;
-            var shapeCount = 0;
-            for (int i = 0; i < width; ++i)
+        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<Vector3>(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 = 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 * 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 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:
+                        default:
+                            bodyDescription.Collidable.Shape = hullIndex;
+                            bodyDescription.LocalInertia = hullInertia;
+                            break;
                     }
+                    Simulation.Bodies.Add(bodyDescription);
+
                 }
             }
-
-            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(), 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 0daee0dfd..c67efa3f7 100644
--- a/Demos/SpecializedTests/SimulationScrambling.cs
+++ b/Demos/SpecializedTests/SimulationScrambling.cs
@@ -1,395 +1,375 @@
 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
+namespace Demos.SpecializedTests;
+
+public static class SimulationScrambling
 {
-    public static class SimulationScrambling
+    public static void ScrambleConstraints(Solver solver)
     {
-        public static void ScrambleBodies(Simulation simulation) 
+        Random random = new Random(5);
+        ref var activeSet = ref solver.ActiveSet;
+        for (int i = 0; i < activeSet.Batches.Count; ++i)
         {
-            //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)
+            for (int j = 0; j < activeSet.Batches[i].TypeBatches.Count; ++j)
             {
-                //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));
+                ref var typeBatch = ref activeSet.Batches[i].TypeBatches[j];
+                solver.TypeProcessors[typeBatch.TypeId].Scramble(ref typeBatch, random, ref solver.HandleToConstraint);
             }
-
         }
-
-        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)
-            {
-                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);
-                }
-            }
-        }
-        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<T> where T : unmanaged, IConstraintDescription<T>
-        {
-            public T Description;
-            public int BodyA;
-            public int BodyB;
-        }
+    struct CachedConstraint<T> where T : unmanaged, IConstraintDescription<T>
+    {
+        public T Description;
+        public int BodyA;
+        public int BodyB;
+    }
 
-        struct BodyEnumerator : IForEach<int>
-        {
-            public Bodies Bodies;
-            public int[] HandlesToIdentity;
-            public int IdentityA;
-            public int IdentityB;
-            public int IndexInConstraint;
+    struct BodyEnumerator : IForEach<int>
+    {
+        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 connectedBodyIndex)
-            {
-                var entryIndex = HandlesToIdentity[Bodies.ActiveSet.IndexToHandle[connectedBodyIndex].Value];
-                if (IndexInConstraint == 0)
-                    IdentityA = entryIndex;
-                else
-                    IdentityB = entryIndex;
-                ++IndexInConstraint;
-            }
         }
-
-
-        static void RemoveConstraint(Simulation simulation, ConstraintHandle constraintHandle, int[] constraintHandlesToIdentity, ConstraintHandle[] constraintHandles, List<int> 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<int>
-        {
-            public Simulation Simulation;
-            public ConstraintHandle ConstraintHandle;
-            public void LoopBody(int bodyIndex)
-            {
-                //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.
-                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<int> 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<int>
+    {
+        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")]
-        static void Validate(Simulation simulation, List<int> removedConstraints, List<int> removedBodies, int originalBodyCount, int originalConstraintCount)
+    [Conditional("DEBUG")]
+    [MethodImpl(MethodImplOptions.AggressiveInlining)]
+    static void WriteLine(string message)
+    {
+        //Debug.WriteLine(message);
+    }
+
+    [Conditional("DEBUG")]
+    static void Validate(Simulation simulation, List<int> removedConstraints, List<int> 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;
-                        typeProcessor.EnumerateConnectedBodyIndices(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<T>(List<T> 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<int> removedConstraints, List<int> 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<T>(List<T> 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<T>(Simulation simulation, BodyHandle[] bodyHandles, int[] bodyHandlesToIdentity, ConstraintHandle[] constraintHandles,
-            int[] constraintHandlesToIdentity, CachedConstraint<T>[] constraintDescriptions,
-            List<int> removedConstraints, List<int> removedBodies, Random random) where T : unmanaged, IConstraintDescription<T>
+    [MethodImpl(MethodImplOptions.NoInlining)]
+    private static void ChurnAddBody(Simulation simulation, BodyDescription[] bodyDescriptions, BodyHandle[] bodyHandles, int[] bodyHandlesToIdentity,
+        int originalConstraintCount, List<int> removedConstraints, List<int> 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<T>(Simulation simulation, BodyHandle[] bodyHandles, int[] bodyHandlesToIdentity, ConstraintHandle[] constraintHandles,
+        int[] constraintHandlesToIdentity, CachedConstraint<T>[] constraintDescriptions,
+        List<int> removedConstraints, List<int> removedBodies, Random random) where T : unmanaged, IConstraintDescription<T>
+    {
+        //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<T>(Simulation simulation, BodyHandle[] bodyHandles, ConstraintHandle[] constraintHandles, int[] constraintHandlesToIdentity,
-            CachedConstraint<T>[] constraintDescriptions, List<int> removedConstraints, List<int> removedBodies, Random random) where T : unmanaged, ITwoBodyConstraintDescription<T>
+    [MethodImpl(MethodImplOptions.NoInlining)]
+    private static void ChurnAddConstraint<T>(Simulation simulation, BodyHandle[] bodyHandles, ConstraintHandle[] constraintHandles, int[] constraintHandlesToIdentity,
+        CachedConstraint<T>[] constraintDescriptions, List<int> removedConstraints, List<int> removedBodies, Random random) where T : unmanaged, ITwoBodyConstraintDescription<T>
+    {
+        //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, ref 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<T>(Simulation simulation, int originalBodyCount,
-            int[] constraintHandlesToIdentity, ConstraintHandle[] constraintHandles, CachedConstraint<T>[] constraintDescriptions, List<int> removedConstraints, List<int> removedBodies, Random random)
-            where T : unmanaged, IConstraintDescription<T>
+    [MethodImpl(MethodImplOptions.NoInlining)]
+    private static void ChurnRemoveConstraint<T>(Simulation simulation, int originalBodyCount,
+        int[] constraintHandlesToIdentity, ConstraintHandle[] constraintHandles, CachedConstraint<T>[] constraintDescriptions, List<int> removedConstraints, List<int> removedBodies, Random random)
+        where T : unmanaged, IConstraintDescription<T>
+    {
+        //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<T>(Simulation simulation, int iterations, BodyHandle[] bodyHandles, ConstraintHandle[] constraintHandles) where T : unmanaged, ITwoBodyConstraintDescription<T>
+    public static double AddRemoveChurn<T>(Simulation simulation, int iterations, BodyHandle[] bodyHandles, ConstraintHandle[] constraintHandles) where T : unmanaged, ITwoBodyConstraintDescription<T>
+    {
+        //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<T>[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<T>[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);
-                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);
-                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<int>();
-            var removedBodies = new List<int>();
-            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<int>();
+        var removedBodies = new List<int>();
+        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 8dd332938..a6e5dc345 100644
--- a/Demos/SpecializedTests/SolverBatchTestDemo.cs
+++ b/Demos/SpecializedTests/SolverBatchTestDemo.cs
@@ -5,150 +5,103 @@
 using BepuPhysics.Collidables;
 using System;
 using System.Numerics;
-using System.Diagnostics;
-using BepuUtilities.Memory;
-using BepuUtilities.Collections;
 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 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.Solver.IterationCount = 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);
-            clothNodeShape.ComputeInertia(1, out var clothNodeInertia);
-            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 = 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
-                    };
-                    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], ref left);
-                    if (j >= 1)
-                        Simulation.Solver.Add(nodeHandles[i][j], nodeHandles[i][j - 1], ref up);
-                    if (i >= 1 && j >= 1)
-                        Simulation.Solver.Add(nodeHandles[i][j], nodeHandles[i - 1][j - 1], ref leftUp);
-                    if (i < width - 1 && j >= 1)
-                        Simulation.Solver.Add(nodeHandles[i][j], nodeHandles[i + 1][j - 1], ref 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 = 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
-                }
-            };
-            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 = 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
-                }
-            };
-            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 += 1 / 60f;
-            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<int>
     {
+        [MethodImpl(MethodImplOptions.AggressiveInlining)]
+        public int Compare(ref int a, ref int b)
+        {
+            return a < b ? -1 : a > b ? 1 : 0;
+        }
+    }
 
-        struct Comparer : IComparerRef<int>
+    static void VerifySort(ref Buffer<int> 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<int> keys)
+        var bufferPool = new BufferPool();
+        bufferPool.Take<int>(elementCount, out var keys);
+        bufferPool.Take<int>(elementCount, out var indexMap);      
+        bufferPool.Take<int>(elementCount, out var keys2);
+        bufferPool.Take<int>(elementCount, out var indexMap2);
+        bufferPool.Take<int>(elementCount, out var keys3);
+        bufferPool.Take<int>(elementCount, out var indexMap3);
+        bufferPool.Take<int>(elementCount, out var keys4);
+        bufferPool.Take<int>(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<int>(elementCount, out var keys);
-            bufferPool.Take<int>(elementCount, out var indexMap);      
-            bufferPool.Take<int>(elementCount, out var keys2);
-            bufferPool.Take<int>(elementCount, out var indexMap2);
-            bufferPool.Take<int>(elementCount, out var keys3);
-            bufferPool.Take<int>(elementCount, out var indexMap3);
-            bufferPool.Take<int>(elementCount, out var keys4);
-            bufferPool.Take<int>(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/TaskQueueTestDemo.cs b/Demos/SpecializedTests/TaskQueueTestDemo.cs
new file mode 100644
index 000000000..5deef8a76
--- /dev/null
+++ b/Demos/SpecializedTests/TaskQueueTestDemo.cs
@@ -0,0 +1,203 @@
+using BepuUtilities;
+using System;
+using System.Diagnostics;
+using System.Numerics;
+using DemoContentLoader;
+using DemoRenderer;
+using BepuPhysics;
+using BepuPhysics.Constraints;
+using System.Threading;
+using System.Runtime.InteropServices;
+using BepuUtilities.TaskScheduling;
+
+namespace Demos.SpecializedTests;
+
+
+public unsafe class TaskQueueTestDemo : Demo
+{
+    static int DoSomeWork(int iterations, int sum)
+    {
+        for (int i = 0; i < iterations; ++i)
+        {
+            sum = (sum ^ i) * i;
+        }
+        return sum;
+    }
+
+    //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(100, 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(100, 0);
+        Interlocked.Add(ref ((DynamicContext2*)context)->Context->Sum, sum);
+    }
+    static void Test<T>(long taskId, void* context, int workerIndex, IThreadDispatcher dispatcher) where T : unmanaged
+    {
+        var sum = DoSomeWork(100, 0);
+        var typedContext = (Context*)context;
+        //if ((taskId & 7) == 0)
+        {
+            const int subtaskCount = 8;
+            var context1 = new DynamicContext1 { Context = typedContext };
+            var context2 = new DynamicContext2 { Context = typedContext };
+
+            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);
+    }
+    static void STTest(long taskId, void* context, int workerIndex, IThreadDispatcher dispatcher)
+    {
+        var sum = DoSomeWork(100, 0);
+        var typedContext = (Context*)context;
+        //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)
+            {
+                DynamicallyEnqueuedTest2(i, &context2, workerIndex, dispatcher);
+            }
+        }
+        Interlocked.Add(ref typedContext->Sum, sum);
+    }
+
+    static void DispatcherBody<T>(int workerIndex, IThreadDispatcher dispatcher) where T : unmanaged
+    {
+        //if (workerIndex > 1)
+        //    return;
+        var taskStack = (TaskStack*)dispatcher.UnmanagedContext;
+        while (taskStack->TryPopAndRun(workerIndex, dispatcher) != PopTaskResult.Stop) ;
+
+    }
+
+    struct Context
+    {
+        public void* TaskPile;
+        public int Sum;
+    }
+
+    static void IssueStop<T>(long id, void* context, int workerIndex, IThreadDispatcher dispatcher) where T : unmanaged
+    {
+        var typedContext = (Context*)context;
+        ((TaskStack*)typedContext->TaskPile)->RequestStop();
+
+    }
+
+    static void EmptyDispatch(int workerIndex, IThreadDispatcher dispatcher)
+    {
+
+    }
+
+    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));
+
+        //Console.WriteLine($"Task size: {Unsafe.SizeOf<TaskQ>()}, {Unsafe.SizeOf<TaskPS>()}");
+
+        int iterationCount = 4;
+        int tasksPerIteration = 64;
+
+        //Test(() =>
+        //{
+        //    for (int i = 0; i < 1024; ++i)
+        //        ThreadDispatcher.DispatchWorkers(&EmptyDispatch);
+        //    return 0;
+        //}, "Dispatch");
+
+        //LINKED TASK STACK
+
+        for (int i = 0; i < 10; ++i)
+        {
+            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 Task(&IssueStop<TaskStack>, &context));
+                for (int i = 0; i < iterationCount; ++i)
+                {
+                    linkedTaskStackPointer->PushForUnsafely(&Test<TaskStack>, &context, i * tasksPerIteration, tasksPerIteration, 0, ThreadDispatcher, continuation: continuation);
+                }
+                //taskQueuePointer->TryEnqueueStopUnsafely();
+                //taskQueuePointer->EnqueueTasks()
+                ThreadDispatcher.DispatchWorkers(&DispatcherBody<TaskStack>, unmanagedContext: linkedTaskStackPointer);
+                return context.Sum;
+            }, "MT Linked Stack", () => linkedTaskStackPointer->Reset(ThreadDispatcher));
+            linkedTaskStack.Dispose(BufferPool, ThreadDispatcher);
+        }
+
+
+
+        //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");
+
+    }
+
+    delegate int TestFunction();
+
+    static void Test(TestFunction function, string name, Action reset = null)
+    {
+        long accumulatedTime = 0;
+        const int testCount = 1024;
+        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();
+        }
+        Console.WriteLine($"{name} time per execution (ms): {(accumulatedTime) * 1e3 / (testCount * Stopwatch.Frequency)}, acc: {accumulator}");
+    }
+
+
+}
diff --git a/Demos/SpecializedTests/TestHelpers.cs b/Demos/SpecializedTests/TestHelpers.cs
index b2ecb521a..996773db5 100644
--- a/Demos/SpecializedTests/TestHelpers.cs
+++ b/Demos/SpecializedTests/TestHelpers.cs
@@ -1,42 +1,40 @@
 using BepuPhysics;
 using BepuUtilities;
 using System;
-using System.Collections.Generic;
 using System.Numerics;
-using System.Text;
 
-namespace Demos.SpecializedTests
+namespace Demos.SpecializedTests;
+
+public static class TestHelpers
 {
-    public static class TestHelpers
+    /// <summary>
+    /// Gets a value roughly representing the amount of energy in the simulation. This is occasionally handy for debug purposes.
+    /// </summary>
+    public static float GetBodyEnergyHeuristic(Bodies bodies)
     {
-        /// <summary>
-        /// Gets a value roughly representing the amount of energy in the simulation. This is occasionally handy for debug purposes.
-        /// </summary>
-        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);
-            }
-            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((float)random.NextDouble(), (float)random.NextDouble(), (float)random.NextDouble());
-            var axis = new Vector3((float)random.NextDouble(), (float)random.NextDouble(), (float)random.NextDouble());
-            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());
-            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/TreeFiddlingTestDemo.cs b/Demos/SpecializedTests/TreeFiddlingTestDemo.cs
new file mode 100644
index 000000000..7f5649fb9
--- /dev/null
+++ b/Demos/SpecializedTests/TreeFiddlingTestDemo.cs
@@ -0,0 +1,487 @@
+using BepuUtilities;
+using BepuUtilities.Memory;
+using System;
+using System.Diagnostics;
+using System.Numerics;
+using System.Runtime.CompilerServices;
+using BepuPhysics.Trees;
+using DemoContentLoader;
+using DemoRenderer;
+using BepuPhysics;
+using BepuPhysics.Constraints;
+using BepuPhysics.Collidables;
+using BepuUtilities.TaskScheduling;
+using System.Threading;
+
+namespace Demos.SpecializedTests;
+
+public class TreeFiddlingTestDemo : Demo
+{
+    struct Pair : IEquatable<Pair>
+    {
+        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}";
+        }
+    }
+    public struct OverlapHandler : IOverlapHandler
+    {
+        public int OverlapCount;
+        public int OverlapSum;
+        public int OverlapHash;
+        public int TreeLeafCount;
+
+        //public HashSet<Pair> 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;
+        }
+    }
+
+
+    public struct ThreadedOverlapHandler : IThreadedOverlapHandler
+    {
+        public struct Worker
+        {
+            public int OverlapCount;
+            public int OverlapSum;
+        }
+        public Buffer<Worker> Workers;
+
+        public ThreadedOverlapHandler(BufferPool pool, int workerCount)
+        {
+            Workers = new Buffer<Worker>(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<Triangle> 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<Vector3>(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<Triangle>(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)
+        {
+            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<Triangle> CreateRandomSoupTriangles(BoundingBox bounds, int triangleCount, float minimumSize, float maximumSize)
+    {
+        Random random = new Random(5);
+        BufferPool.Take<Triangle>(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);
+
+            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;
+        }
+        return triangles;
+    }
+
+    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();
+            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;
+            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);
+
+
+            // @ @ @ @ @ @ 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;
+            long intervalSum = 0;
+            for (int testIndex = 0; testIndex < 16384; ++testIndex)
+            {
+                handler.OverlapCount = 0;
+                threadedHandler.Reset();
+                var start = Stopwatch.GetTimestamp();
+                //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;
+                intervalSum += end - start;
+                const int intervalSize = 128;
+                if ((testIndex + 1) % intervalSize == 0)
+                {
+                    mesh.Tree.Validate();
+                    var costMetric = mesh.Tree.MeasureCostMetric();
+                    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;
+                }
+            }
+            threadedHandler.Dispose(BufferPool);
+
+            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<NodeChild>(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, () =>
+            //{
+            //    mesh.Tree.BinnedBuild(subtrees, BufferPool, ThreadDispatcher);
+            //}, "Revamp Single Axis MT", ref mesh.Tree);
+
+            //BufferPool.Take<BoundingBox>(mesh.Triangles.Length, out var leafBounds);
+            //BufferPool.Take<int>(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<int> subtreeReferences = new(triangles.Length, BufferPool);
+            //QuickList<int> 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);
+
+    static void SelfTest(TestFunction function, int leafCount, string name)
+    {
+        var overlapHandler = new OverlapHandler();
+        overlapHandler.TreeLeafCount = leafCount;
+        //overlapHandler.Set = new HashSet<Pair>();
+        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}");
+    }
+
+
+    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<float, int>(ref sum.X) * 31 + Unsafe.As<float, int>(ref sum.Y) * 37 + Unsafe.As<float, int>(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)
+        {
+            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<float, int>(ref localSum.X) * 31 + Unsafe.As<float, int>(ref localSum.Y) * 37 + Unsafe.As<float, int>(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()}");
+    }
+}
diff --git a/Demos/SpecializedTests/TreeTest.cs b/Demos/SpecializedTests/TreeTest.cs
index edb20654d..a1884ed60 100644
--- a/Demos/SpecializedTests/TreeTest.cs
+++ b/Demos/SpecializedTests/TreeTest.cs
@@ -1,168 +1,162 @@
 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
+namespace Demos.SpecializedTests;
+
+public unsafe static class TreeTest
 {
-    public 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<BoundingBox>(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<BoundingBox>(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((float)random.NextDouble(), (float)random.NextDouble(), (float)random.NextDouble());
+                    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(ref leafBounds[i], pool);
-            }
-            tree.Validate();
+        for (int i = prebuiltCount; i < leafCount; ++i)
+        {
+            tree.Add(leafBounds[i], pool);
+        }
+        tree.Validate();
 
-            pool.TakeAtLeast<int>(leafCount, out var handleToLeafIndex);
-            pool.TakeAtLeast<int>(leafCount, out var leafIndexToHandle);
-            for (int i = 0; i < leafCount; ++i)
-            {
-                handleToLeafIndex[i] = i;
-                leafIndexToHandle[i] = i;
-            }
+        pool.TakeAtLeast<int>(leafCount, out var handleToLeafIndex);
+        pool.TakeAtLeast<int>(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 SimpleThreadDispatcher(Environment.ProcessorCount);
-            var refineContext = new Tree.RefitAndRefineMultithreadedContext();
-            var selfTestContext = new Tree.MultithreadedSelfTest<OverlapHandler>(pool);
-            var overlapHandlers = new OverlapHandler[threadDispatcher.ThreadCount];
-            Action<int> pairTestAction = selfTestContext.PairTest;
-            var removedLeafHandles = new QuickList<int>(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<OverlapHandler>(pool);
+        var overlapHandlers = new OverlapHandler[threadDispatcher.ThreadCount];
+        Action<int> pairTestAction = selfTestContext.PairTest;
+        var removedLeafHandles = new QuickList<int>(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(ref 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 cb5cd56d5..7aae92480 100644
--- a/Demos/SpecializedTests/TriangleRayTestDemo.cs
+++ b/Demos/SpecializedTests/TriangleRayTestDemo.cs
@@ -1,176 +1,170 @@
 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
+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 = (float)random.NextDouble();
-                b = (float)random.NextDouble();
-                c = (float)random.NextDouble();
-                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) * (float)random.NextDouble();
-                    break;
-                case 1:
-                    borderPoint = triangle.A + (triangle.C - triangle.A) * (float)random.NextDouble();
-                    break;
-                default:
-                    borderPoint = triangle.B + (triangle.C - triangle.B) * (float)random.NextDouble();
-                    break;
-            }
-            var center = (triangle.A + triangle.B + triangle.C) / 3f;
-            var offsetToBorder = borderPoint - center;
-            var localP = center + offsetToBorder * (1.01f + 4 * (float)random.NextDouble());
+            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, in Vector3 rayOrigin, in Vector3 rayDirection, bool expectedImpact, in 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);
-            RigidPoses.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 unsafe 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 DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new PositionFirstTimestepper());
 
-            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((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);
-                
-                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);
-
-                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;
-                    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 * (float)random.NextDouble()) / 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 * (float)random.NextDouble()) / 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 ceabc0aca..8bcf172bd 100644
--- a/Demos/SpecializedTests/TriangleTestDemo.cs
+++ b/Demos/SpecializedTests/TriangleTestDemo.cs
@@ -5,194 +5,207 @@
 using BepuPhysics.Collidables;
 using System;
 using System.Numerics;
-using System.Diagnostics;
-using BepuUtilities.Memory;
 using BepuUtilities.Collections;
 using BepuPhysics.CollisionDetection.CollisionTasks;
 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 unsafe override void Initialize(ContentArchive content, Camera camera)
         {
-            {
-                SphereTriangleTester tester;
-                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<float>(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<float>.Count, out var manifold);
-            }
-            {
-                CapsuleTriangleTester tester;
-                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<float>(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);
-                tester.Test(ref capsule, ref triangle, ref margin, ref offsetB, ref orientationA, ref orientationB, Vector<float>.Count, out var manifold);
-            }
-            {
-                BoxTriangleTester tester;
-                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<float>(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);
-                tester.Test(ref shape, ref triangle, ref margin, ref offsetB, ref orientationA, ref orientationB, Vector<float>.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)));
-
-                var margin = new Vector<float>(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);
-                tester.Test(ref a, ref b, ref margin, ref offsetB, ref orientationA, ref orientationB, Vector<float>.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 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);
-
-                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 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)));
-
-                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)));
-                var points = new QuickList<Vector3>(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(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)));
-
-                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 CollidableDescription(Simulation.Shapes.Add(compound), 0.1f), new BodyActivityDescription(-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<float>(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<float>.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<float>(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<float>.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<float>(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<float>.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<float>(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<float>.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<Vector3>(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<Triangle>(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 };
+                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(30, -2.5f, 0), new CollidableDescription(Simulation.Shapes.Add(testMesh), 0.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), 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 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));
             }
-        }
 
-        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);
+            {
+                var triangles = new QuickList<Triangle>(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);
     }
+
+
 }
diff --git a/Demos/SpecializedTests/VolumeQueryTests.cs b/Demos/SpecializedTests/VolumeQueryTests.cs
index 57be6f356..9cefc9bab 100644
--- a/Demos/SpecializedTests/VolumeQueryTests.cs
+++ b/Demos/SpecializedTests/VolumeQueryTests.cs
@@ -10,319 +10,289 @@
 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
+namespace Demos.SpecializedTests;
+
+public class VolumeQueryTests : Demo
 {
-    public class VolumeQueryTests : Demo
+    public struct NoCollisionCallbacks : INarrowPhaseCallbacks
     {
-        public unsafe 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)
-            {
-                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 unsafe bool ConfigureContactManifold<TManifold>(int workerIndex, CollidablePair pair, ref TManifold manifold, out PairMaterialProperties pairMaterial) where TManifold : unmanaged, IContactManifold<TManifold>
-            {
-                pairMaterial = new PairMaterialProperties();
-                return false;
-            }
+        [MethodImpl(MethodImplOptions.AggressiveInlining)]
+        public bool ConfigureContactManifold<TManifold>(int workerIndex, CollidablePair pair, ref TManifold manifold, out PairMaterialProperties pairMaterial) where TManifold : unmanaged, IContactManifold<TManifold>
+        {
+            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 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;
+        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 PositionFirstTimestepper());
-
-            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((float)random.NextDouble(), (float)random.NextDouble(), (float)random.NextDouble());
-                        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();
-                        QuaternionEx.Normalize(ref orientation);
-
-                        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
-                                }
-                            };
-                            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
-                                }
-                            };
-                            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<BoundingBox>(boxCount, BufferPool);
-            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 boxOrigin = randomMin + r * randomSpan;
-                var boxHalfSize = new Vector3(0.25f + 0.75f * (float)random.NextDouble());
-                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<BoundingBox>(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<BoundingBox> queryBoxes;
+    QuickList<BoundingBox> queryBoxes;
 
 
-        class BoxQueryAlgorithm
+    unsafe class BoxQueryAlgorithm
+    {
+        public string Name;
+        public int IntersectionCount;
+        public TimingsRingBuffer Timings;
+
+        Func<int, BoxQueryAlgorithm, int> worker;
+        Action<int> internalWorker;
+        public int JobIndex;
+
+        public BoxQueryAlgorithm(string name, BufferPool pool, Func<int, BoxQueryAlgorithm, int> 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<int, BoxQueryAlgorithm, int> worker;
-            Action<int> 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<int, BoxQueryAlgorithm, int> worker, int timingSampleCount = 16)
+        public void Execute(ref QuickList<BoundingBox> 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<BoundingBox> boxes, SimpleThreadDispatcher 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;
+        var pool = ThreadDispatcher.WorkerPools[workerIndex];
+        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, pool, ref hitHandler);
             }
-            return intersectionCount;
         }
+        return intersectionCount;
+    }
 
 
-        BoxQueryAlgorithm[] algorithms;
-
-        struct QueryJob
-        {
-            public int Start;
-            public int End;
-        }
-        Buffer<QueryJob> jobs;
-
+    BoxQueryAlgorithm[] algorithms;
 
-        unsafe struct HitHandler : IBreakableForEach<CollidableReference>
-        {
-            public int* IntersectionCount;
+    struct QueryJob
+    {
+        public int Start;
+        public int End;
+    }
+    Buffer<QueryJob> jobs;
 
-            [MethodImpl(MethodImplOptions.AggressiveInlining)]
-            public bool LoopBody(CollidableReference collidable)
-            {
-                ++*IntersectionCount;
-                return true;
-            }
-        }
 
-        bool shouldUseMultithreading = true;
+    unsafe struct HitHandler : IBreakableForEach<CollidableReference>
+    {
+        public int* IntersectionCount;
 
-        public unsafe 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<double> queue;
-        BufferPool pool;
+    QuickQueue<double> queue;
+    BufferPool pool;
 
-        /// <summary>
-        /// Gets or sets the maximum number of time measurements that can be held by the ring buffer.
-        /// </summary>
-        public int Capacity
+    /// <summary>
+    /// Gets or sets the maximum number of time measurements that can be held by the ring buffer.
+    /// </summary>
+    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<double>(maximumCapacity, pool);
-        }
+    }
+    public TimingsRingBuffer(int maximumCapacity, BufferPool pool)
+    {
+        if(maximumCapacity <= 0)
+            throw new ArgumentException("Capacity must be positive.");
+        this.pool = pool;
+        queue = new QuickQueue<double>(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 1af561503..ca2ba195f 100644
--- a/Demos/UI/DemoSwapper.cs
+++ b/Demos/UI/DemoSwapper.cs
@@ -1,85 +1,81 @@
 using DemoRenderer.UI;
 using DemoUtilities;
 using System;
-using System.Collections.Generic;
-using System.Diagnostics;
 using System.Numerics;
-using System.Text;
 
-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 e29c50bea..6d3ab468d 100644
--- a/Demos/UI/Graph.cs
+++ b/Demos/UI/Graph.cs
@@ -1,354 +1,348 @@
-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
+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
-    {
-        /// <summary>
-        /// Minimum location of the graph body in pixels, not including the interval labels.
-        /// </summary>
-        public Vector2 BodyMinimum;
-        /// <summary>
-        /// Span of the graph body in pixels, not including interval labels.
-        /// </summary>
-        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;
-        /// <summary>
-        /// The length of a tick mark line, measured from the axis.
-        /// </summary>
-        public float IntervalTickLength;
-        /// <summary>
-        /// Number of interval ticks along the horizontal axis, not including the start and end ticks.
-        /// </summary>
-        public int TargetHorizontalTickCount;
-        /// <summary>
-        /// Number of interval ticks along the vertical axis, not including the min and max ticks.
-        /// </summary>
-        public int TargetVerticalTickCount;
-        public float HorizontalTickTextPadding;
-        public float VerticalTickTextPadding;
+public struct GraphDescription
+{
+    /// <summary>
+    /// Minimum location of the graph body in pixels, not including the interval labels.
+    /// </summary>
+    public Vector2 BodyMinimum;
+    /// <summary>
+    /// Span of the graph body in pixels, not including interval labels.
+    /// </summary>
+    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;
+    /// <summary>
+    /// The length of a tick mark line, measured from the axis.
+    /// </summary>
+    public float IntervalTickLength;
+    /// <summary>
+    /// Number of interval ticks along the horizontal axis, not including the start and end ticks.
+    /// </summary>
+    public int TargetHorizontalTickCount;
+    /// <summary>
+    /// Number of interval ticks along the vertical axis, not including the min and max ticks.
+    /// </summary>
+    public int TargetVerticalTickCount;
+    public float HorizontalTickTextPadding;
+    public float VerticalTickTextPadding;
 
 
-        /// <summary>
-        /// Minimum location of the legend in pixels.
-        /// </summary>
-        public Vector2 LegendMinimum;
-        public float LegendNameHeight;
-        public float LegendLineLength;
+    /// <summary>
+    /// Minimum location of the legend in pixels.
+    /// </summary>
+    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<Series> 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<Series> 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<Series>(initialSeriesCapacity);
-        }
+    public Graph(GraphDescription description, int initialSeriesCapacity = 8)
+    {
+        Description = description;
+        if (initialSeriesCapacity <= 0)
+            throw new ArgumentException("Capacity must be positive.");
+        graphSeries = new List<Series>(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;
             }
         }
-
     }
+
 }
diff --git a/Documentation/Building.md b/Documentation/Building.md
index 76c237782..e8f23ff92 100644
--- a/Documentation/Building.md
+++ b/Documentation/Building.md
@@ -2,19 +2,25 @@
 
 ## 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.
 
-The libraries target .NET 5.
+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 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. 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 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
 
@@ -34,7 +40,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/ContinuousCollisionDetection.md b/Documentation/ContinuousCollisionDetection.md
new file mode 100644
index 000000000..b099736ac
--- /dev/null
+++ b/Documentation/ContinuousCollisionDetection.md
@@ -0,0 +1,93 @@
+# 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.
+
+<p align="center"><img src="images/ContinuousCollisionDetection/ccd.png"/></p>
+
+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.
+
+# 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. 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.
+
+<p align="center"><img src="images/ContinuousCollisionDetection/speculativeContact.png"/></p>
+
+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 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));
+```
+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.
+
+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.
+
+<p align="center"><img src="images/ContinuousCollisionDetection/ghostCollision.png"/></p>
+
+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` a full `CollidableDescription` like so:
+```cs
+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 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.
+
+<p align="center"><img src="images/ContinuousCollisionDetection/smallMarginNoGhostCollision.png"/></p>
+
+Using a smaller maximum speculative margin means that you can miss high velocity non-ghost collisions, though:
+
+<p align="center"><img src="images/ContinuousCollisionDetection/smallMarginNoCollision.png"/></p>
+
+# 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 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. 
+
+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!
+
+<p align="center"><img src="images/ContinuousCollisionDetection/smallMarginSweepCollision.png"/></p>
+
+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. 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:
+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 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.
+
+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).
+
diff --git a/Documentation/GettingStarted.md b/Documentation/GettingStarted.md
index c4575a8b2..938fdf526 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,17 +58,17 @@ 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
 
 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`.
 
@@ -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://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 in the [discussions](https://github.com/bepu/bepuphysics2/discussions).
\ No newline at end of file
diff --git a/Documentation/PackagingAndVersioning.md b/Documentation/PackagingAndVersioning.md
index 81d04e0b1..29de43743 100644
--- a/Documentation/PackagingAndVersioning.md
+++ b/Documentation/PackagingAndVersioning.md
@@ -4,8 +4,8 @@ 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.
 
-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/PerformanceTips.md b/Documentation/PerformanceTips.md
index 5a3490b19..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. 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, and the [Substepping documentation](Substepping.md) for more details.
\ No newline at end of file
diff --git a/Documentation/QuestionsAndAnswers.md b/Documentation/QuestionsAndAnswers.md
index 895840fd1..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,19 +47,19 @@ 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. :(
 
 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!
+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,9 +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.
 
-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?
+## 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 c8f4b7fa0..f83460d57 100644
--- a/Documentation/StabilityTips.md
+++ b/Documentation/StabilityTips.md
@@ -13,43 +13,43 @@ 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.
 
 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:
 
 ![](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 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/Substepping.md b/Documentation/Substepping.md
new file mode 100644
index 000000000..86dfd77b9
--- /dev/null
+++ b/Documentation/Substepping.md
@@ -0,0 +1,79 @@
+# 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.
+
+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 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. 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.
+
+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:
+```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`.
+
+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 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
+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 `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.
+
+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/qMX1ZLmfrEo).
+
+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. 
\ No newline at end of file
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/changelog.md b/Documentation/changelog.md
new file mode 100644
index 000000000..675b8f4e6
--- /dev/null
+++ b/Documentation/changelog.md
@@ -0,0 +1,52 @@
+# 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.
+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<byte>`.
+
+## 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.
+
+    
diff --git a/Documentation/docfx.json b/Documentation/docfx.json
new file mode 100644
index 000000000..933ff3ae9
--- /dev/null
+++ b/Documentation/docfx.json
@@ -0,0 +1,63 @@
+{
+  "$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,
+      "_gitContribute": {
+        "repo": "https://github.com/bepu/bepuphysics2",
+        "branch": "master"
+      },
+      "_gitUrlPattern": "github",
+      "_gitRepo": "https://github.com/bepu/bepuphysics2"
+    }
+  }
+}
\ No newline at end of file
diff --git a/Documentation/docs/toc.yml b/Documentation/docs/toc.yml
new file mode 100644
index 000000000..b769ca7f9
--- /dev/null
+++ b/Documentation/docs/toc.yml
@@ -0,0 +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
+- name: Upgrading from v1
+  href: ../UpgradingFromV1.md
+- name: Roadmap
+  href: ../roadmap.md
\ No newline at end of file
diff --git a/Documentation/favicon.ico b/Documentation/favicon.ico
new file mode 100644
index 000000000..f7aa9da22
Binary files /dev/null and b/Documentation/favicon.ico differ
diff --git a/Documentation/fix-links.ps1 b/Documentation/fix-links.ps1
new file mode 100644
index 000000000..9846f643c
--- /dev/null
+++ b/Documentation/fix-links.ps1
@@ -0,0 +1,81 @@
+param (
+    [string]$sitePath = "./_site",
+    [string]$repoUrl = "https://github.com/bepu/bepuphysics2/blob/master"
+)
+
+# 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) {
+    $content = Get-Content -Path $file.FullName -Raw
+    $originalContent = $content
+    
+    Write-Host "Considering file: $($file.FullName)"
+    
+    # Pattern to find all href links
+    $pattern = '(href=["''])([^"'']*)(["''])'
+    
+    # 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 an absolute URL or has special protocols
+        if ($path -match '^(https?:|mailto:|#|javascript:)') {
+            return $match.Value
+        }
+        
+        # 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: $path -> $repoUrl/$relativePath"
+            return "$prefix$repoUrl/$relativePath$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).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 $fileChanges links in $($file.Name)"
+    }
+}
+
+Write-Host "Link transformation complete. Fixed $linkCount links in total."
\ No newline at end of file
diff --git a/Documentation/images/ContinuousCollisionDetection/ccd.png b/Documentation/images/ContinuousCollisionDetection/ccd.png
new file mode 100644
index 000000000..774af626f
Binary files /dev/null and b/Documentation/images/ContinuousCollisionDetection/ccd.png differ
diff --git a/Documentation/images/ContinuousCollisionDetection/ghostCollision.png b/Documentation/images/ContinuousCollisionDetection/ghostCollision.png
new file mode 100644
index 000000000..101b8b590
Binary files /dev/null and b/Documentation/images/ContinuousCollisionDetection/ghostCollision.png differ
diff --git a/Documentation/images/ContinuousCollisionDetection/smallMarginNoCollision.png b/Documentation/images/ContinuousCollisionDetection/smallMarginNoCollision.png
new file mode 100644
index 000000000..95843261b
Binary files /dev/null and b/Documentation/images/ContinuousCollisionDetection/smallMarginNoCollision.png differ
diff --git a/Documentation/images/ContinuousCollisionDetection/smallMarginNoGhostCollision.png b/Documentation/images/ContinuousCollisionDetection/smallMarginNoGhostCollision.png
new file mode 100644
index 000000000..7e96bc452
Binary files /dev/null and b/Documentation/images/ContinuousCollisionDetection/smallMarginNoGhostCollision.png differ
diff --git a/Documentation/images/ContinuousCollisionDetection/smallMarginSweepCollision.png b/Documentation/images/ContinuousCollisionDetection/smallMarginSweepCollision.png
new file mode 100644
index 000000000..f322b3664
Binary files /dev/null and b/Documentation/images/ContinuousCollisionDetection/smallMarginSweepCollision.png differ
diff --git a/Documentation/images/ContinuousCollisionDetection/speculativeContact.png b/Documentation/images/ContinuousCollisionDetection/speculativeContact.png
new file mode 100644
index 000000000..afec8d57e
Binary files /dev/null and b/Documentation/images/ContinuousCollisionDetection/speculativeContact.png differ
diff --git a/Documentation/images/youtubeLink24.png b/Documentation/images/youtubeLink24.png
new file mode 100644
index 000000000..cb82c232c
Binary files /dev/null and b/Documentation/images/youtubeLink24.png differ
diff --git a/Documentation/index.md b/Documentation/index.md
new file mode 100644
index 000000000..11c5f00c0
--- /dev/null
+++ b/Documentation/index.md
@@ -0,0 +1,16 @@
+---
+_disableToc: false
+---
+# bepuphysics docs!
+
+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).
+
+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`.
+
+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/Documentation/roadmap.md b/Documentation/roadmap.md
index 627f536c4..f9ff5a10b 100644
--- a/Documentation/roadmap.md
+++ b/Documentation/roadmap.md
@@ -1,31 +1,39 @@
 # 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 (H2 2021)
+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.
 
-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.
+## Near term
 
-I'll be working in the background on other projects using bepuphysics2. Their requirements will drive various changes and future planning.
+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. 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. 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
 
-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.
-- 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
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
diff --git a/README.md b/README.md
index 31842ba90..244bcbd13 100644
--- a/README.md
+++ b/README.md
@@ -1,13 +1,15 @@
 # bepuphysics v2
-<p align="center"><a href="https://www.youtube.com/watch?v=sfgC_eNx9M8" target="_blank"><img src="Documentation/images/youtubeLink.png" width="700" height="394" border="0" /></a></p>
+<p align="center">
+<a href="https://www.youtube.com/watch?v=sfgC_eNx9M8" target="_blank"><img src="Documentation/images/youtubeLink.png" width="375" height="211" border="0" /></a>
+<a href="https://www.youtube.com/watch?v=tjtwSq3u6Dg" target="_blank"><img src="Documentation/images/youtubeLink24.png" width="375" height="211" border="0" /></a></p>
 
 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 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).
+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, 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
 
@@ -31,26 +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)
-
-[Contributing](CONTRIBUTING.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)