Version: 187.1.2

MSBuild/Robust.Engine.Version.props

@@ -1,4 +1,4 @@
 <Project>
 
-    <!-- This file automatically reset by Tools/version.py -->
+    <!-- This file automatically reset by Tools/version.py -->
 

RELEASE-NOTES.md

@@ -54,6 +54,29 @@ END TEMPLATE-->
 *None yet*
 
 
+## 187.1.2
+
+### Bugfixes
+
+* Hotfix contact nullrefs if they're modified during manifold generation.
+
+
+## 187.1.1
+
+### Bugfixes
+
+* Revert physics solver job to fix crashes until box2d v3 rolls around.
+* Don't RegenerateContacts if the body isn't collidable to avoid putting non-collidable proxies on the movebuffer.
+
+
+## 187.1.0
+
+### Bugfixes
+
+* Apply default audio params to all audio sources not just non-buffered ones.
+* Avoid re-allocating broadphase job every tick and maybe fix a rare nullref for it.
+
+
 ## 187.0.0
 
 ### New features

Robust.Client/Audio/AudioManager.Public.cs

@@ -278,6 +278,7 @@ internal partial class AudioManager
 
         var audioSource = new AudioSource(this, source, stream);
         _audioSources.Add(source, new WeakReference<BaseAudioSource>(audioSource));
+        ApplyDefaultParams(audioSource);
         return audioSource;
     }
 
@@ -294,9 +295,18 @@ internal partial class AudioManager
 
         var audioSource = new BufferedAudioSource(this, source, AL.GenBuffers(buffers), floatAudio);
         _bufferedAudioSources.Add(source, new WeakReference<BufferedAudioSource>(audioSource));
+        ApplyDefaultParams(audioSource);
         return audioSource;
     }
 
+    private void ApplyDefaultParams(IAudioSource source)
+    {
+        source.MaxDistance = AudioParams.Default.MaxDistance;
+        source.Pitch = AudioParams.Default.Pitch;
+        source.ReferenceDistance = AudioParams.Default.ReferenceDistance;
+        source.RolloffFactor = AudioParams.Default.RolloffFactor;
+    }
+
     /// <inheritdoc />
     public void StopAllAudio()
     {

Robust.Shared/Physics/Dynamics/Contacts/Contact.cs

@@ -355,21 +355,27 @@ namespace Robust.Shared.Physics.Dynamics.Contacts
     internal enum ContactFlags : byte
     {
         None = 0,
+
+        /// <summary>
+        /// Is the contact pending its first manifold generation.
+        /// </summary>
+        PreInit = 1 << 0,
+
         /// <summary>
         ///     Has this contact already been added to an island?
         /// </summary>
-        Island = 1 << 0,
+        Island = 1 << 1,
 
         /// <summary>
         ///     Does this contact need re-filtering?
         /// </summary>
-        Filter = 1 << 1,
+        Filter = 1 << 2,
 
         /// <summary>
         /// Is this a special contact for grid-grid collisions
         /// </summary>
-        Grid = 1 << 2,
+        Grid = 1 << 3,
 
-        Deleting = 1 << 3,
+        Deleting = 1 << 4,
     }
 }

Robust.Shared/Physics/Systems/SharedBroadphaseSystem.cs

@@ -51,10 +51,19 @@ namespace Robust.Shared.Physics.Systems
         private ObjectPool<List<FixtureProxy>> _bufferPool =
             new DefaultObjectPool<List<FixtureProxy>>(new ListPolicy<FixtureProxy>(), 2048);
 
+        private BroadphaseJob _broadphaseJob;
+
         public override void Initialize()
         {
             base.Initialize();
 
+            _broadphaseJob = new BroadphaseJob()
+            {
+                Broadphase = this,
+                BroadphaseExpand = _broadphaseExpand,
+                MapManager = _mapManager,
+            };
+
             _broadphaseQuery = GetEntityQuery<BroadphaseComponent>();
             _gridQuery = GetEntityQuery<MapGridComponent>();
             _physicsQuery = GetEntityQuery<PhysicsComponent>();
@@ -191,17 +200,11 @@ namespace Robust.Shared.Physics.Systems
                 pMoveBuffer[idx++] = (proxy, aabb);
             }
 
-            var job = new BroadphaseJob()
-            {
-                Broadphase = this,
-                BroadphaseExpand = _broadphaseExpand,
-                ContactBuffer = contactBuffer,
-                PMoveBuffer = pMoveBuffer,
-                MapId = mapId,
-                MapManager = _mapManager
-            };
+            _broadphaseJob.ContactBuffer = contactBuffer;
+            _broadphaseJob.PMoveBuffer = pMoveBuffer;
+            _broadphaseJob.MapId = mapId;
 
-            _parallel.ProcessNow(job, count);
+            _parallel.ProcessNow(_broadphaseJob, count);
 
             for (var i = 0; i < count; i++)
             {
@@ -402,6 +405,13 @@ namespace Robust.Shared.Physics.Systems
 
         public void RegenerateContacts(EntityUid uid, PhysicsComponent body, FixturesComponent? fixtures = null, TransformComponent? xform = null)
         {
+            // If it can't collide then we can't touch proxies and add them to the movebuffer anyway.
+            if (!body.CanCollide)
+            {
+                // Sleep body may still have contacts around.
+                return;
+            }
+
             _physicsSystem.DestroyContacts(body);
             if (!Resolve(uid, ref xform, ref fixtures))
                 return;

Robust.Shared/Physics/Systems/SharedPhysicsSystem.Contacts.cs

@@ -120,6 +120,7 @@ public abstract partial class SharedPhysicsSystem
         public bool Return(Contact obj)
         {
             SetContact(obj,
+                false,
                 EntityUid.Invalid, EntityUid.Invalid,
                 string.Empty, string.Empty,
                 null, 0,
@@ -130,6 +131,7 @@ public abstract partial class SharedPhysicsSystem
     }
 
     private static void SetContact(Contact contact,
+        bool enabled,
         EntityUid uidA, EntityUid uidB,
         string fixtureAId, string fixtureBId,
         Fixture? fixtureA, int indexA,
@@ -137,9 +139,9 @@ public abstract partial class SharedPhysicsSystem
         PhysicsComponent? bodyA,
         PhysicsComponent? bodyB)
     {
-        contact.Enabled = true;
+        contact.Enabled = enabled;
         contact.IsTouching = false;
-        contact.Flags = ContactFlags.None;
+        contact.Flags = ContactFlags.None | ContactFlags.PreInit;
         // TOIFlag = false;
 
         contact.EntityA = uidA;
@@ -227,11 +229,11 @@ public abstract partial class SharedPhysicsSystem
         // Edge+Polygon is non-symmetrical due to the way Erin handles collision type registration.
         if ((type1 >= type2 || (type1 == ShapeType.Edge && type2 == ShapeType.Polygon)) && !(type2 == ShapeType.Edge && type1 == ShapeType.Polygon))
         {
-            SetContact(contact, uidA, uidB, fixtureAId, fixtureBId, fixtureA, indexA, fixtureB, indexB, bodyA, bodyB);
+            SetContact(contact, true, uidA, uidB, fixtureAId, fixtureBId, fixtureA, indexA, fixtureB, indexB, bodyA, bodyB);
         }
         else
         {
-            SetContact(contact, uidB, uidA, fixtureBId, fixtureAId, fixtureB, indexB, fixtureA, indexA, bodyB, bodyA);
+            SetContact(contact, true, uidB, uidA, fixtureBId, fixtureAId, fixtureB, indexB, fixtureA, indexA, bodyB, bodyA);
         }
 
         contact.Type = _registers[(int)type1, (int)type2];
@@ -374,6 +376,12 @@ public abstract partial class SharedPhysicsSystem
             var contact = node.Value;
             node = node.Next;
 
+            // It's possible the contact was destroyed by content in which case we just skip it.
+            if (!contact.Enabled)
+                continue;
+
+            // No longer pre-init and can be used in the solver.
+            contact.Flags &= ~ContactFlags.PreInit;
             Fixture fixtureA = contact.FixtureA!;
             Fixture fixtureB = contact.FixtureB!;
             int indexA = contact.ChildIndexA;
@@ -502,6 +510,7 @@ public abstract partial class SharedPhysicsSystem
             {
                 Log.Error($"Insufficient contact length at 429! Index {index} and length is {contacts.Length}. Tell Sloth");
             }
+
             contacts[index++] = contact;
         }
 
@@ -522,6 +531,12 @@ public abstract partial class SharedPhysicsSystem
 
             var contact = contacts[i];
 
+            // It's possible the contact was disabled above if DestroyContact lead to even more being destroyed.
+            if (!contact.Enabled)
+            {
+                continue;
+            }
+
             switch (status[i])
             {
                 case ContactStatus.StartTouching:
@@ -636,12 +651,12 @@ public abstract partial class SharedPhysicsSystem
 
         // TODO: Temporary measure. When Box2D 3.0 comes out expect a major refactor
         // of everything
-        if (contact.FixtureA == null || contact.FixtureB == null)
+        // It's okay past sloth it can't hurt you anymore.
+        // This can happen if DestroyContact is called and content deletes contacts that were already processed.
+        if (!contact.Enabled)
         {
-            Log.Error($"Found a null contact for contact at {index}");
             status[index] = ContactStatus.NoContact;
             wake[index] = false;
-            DebugTools.Assert(false);
             return;
         }
 

Robust.Shared/Physics/Systems/SharedPhysicsSystem.Island.cs

@@ -10,7 +10,6 @@ using Robust.Shared.Physics.Components;
 using Robust.Shared.Physics.Dynamics;
 using Robust.Shared.Physics.Dynamics.Contacts;
 using Robust.Shared.Physics.Dynamics.Joints;
-using Robust.Shared.Threading;
 using Robust.Shared.Utility;
 
 namespace Robust.Shared.Physics.Systems;
@@ -193,6 +192,9 @@ public abstract partial class SharedPhysicsSystem
     private float _velocityThreshold;
     private float _baumgarte;
 
+    private const int VelocityConstraintsPerThread = 16;
+    private const int PositionConstraintsPerThread = 16;
+
     #region Setup
 
     private void InitializeIsland()
@@ -383,8 +385,8 @@ public abstract partial class SharedPhysicsSystem
                     var contact = node.Value;
                     node = node.Next;
 
-                    // Has this contact already been added to an island?
-                    if ((contact.Flags & ContactFlags.Island) != 0x0) continue;
+                    // Has this contact already been added to an island / is it pre-init?
+                    if ((contact.Flags & (ContactFlags.Island | ContactFlags.PreInit)) != 0x0) continue;
 
                     // Is this contact solid and touching?
                     if (!contact.Enabled || !contact.IsTouching) continue;
@@ -597,9 +599,6 @@ public abstract partial class SharedPhysicsSystem
 
     private void SolveIslands(EntityUid uid, PhysicsMapComponent component, List<IslandData> islands, float frameTime, float dtRatio, float invDt, bool prediction)
     {
-        if (islands.Count == 0)
-            return;
-
         var iBegin = 0;
         var gravity = _gravity.GetGravity(uid);
 
@@ -656,21 +655,27 @@ public abstract partial class SharedPhysicsSystem
             sleepStatus[i] = false;
         }
 
-        var job = new SolveIslandJob()
+        var options = new ParallelOptions()
         {
-            Physics = this,
-            Islands = actualIslands,
-            Data = data,
-            Gravity = gravity,
-            Prediction = prediction,
-            SolvedPositions = solvedPositions,
-            SolvedAngles = solvedAngles,
-            LinearVelocities = linearVelocities,
-            AngularVelocities = angularVelocities,
-            SleepStatus = sleepStatus,
+            MaxDegreeOfParallelism = _parallel.ParallelProcessCount,
         };
 
-        _parallel.ProcessNow(job, actualIslands.Length);
+        while (iBegin < actualIslands.Length)
+        {
+            ref var island = ref actualIslands[iBegin];
+
+            if (!InternalParallel(island))
+                break;
+
+            SolveIsland(ref island, in data, options, gravity, prediction, solvedPositions, solvedAngles, linearVelocities, angularVelocities, sleepStatus);
+            iBegin++;
+        }
+
+        Parallel.For(iBegin, actualIslands.Length, options, i =>
+        {
+            ref var island = ref actualIslands[i];
+            SolveIsland(ref island, in data, null, gravity, prediction, solvedPositions, solvedAngles, linearVelocities, angularVelocities, sleepStatus);
+        });
 
         // Update data sequentially
         var metaQuery = GetEntityQuery<MetaDataComponent>();
@@ -714,10 +719,10 @@ public abstract partial class SharedPhysicsSystem
     /// <summary>
     ///     Go through all the bodies in this island and solve.
     /// </summary>
-    /// <param name="parallel">Should we run internal tasks in parallel (true), or is the entire island being solved in parallel with others (false).</param>
     private void SolveIsland(
         ref IslandData island,
         in SolverData data,
+        ParallelOptions? options,
         Vector2 gravity,
         bool prediction,
         Vector2[] solvedPositions,
@@ -817,7 +822,7 @@ public abstract partial class SharedPhysicsSystem
                     island.BrokenJoints.Add((island.Joints[j].Original, error));
             }
 
-            SolveVelocityConstraints(island, velocityConstraints, linearVelocities, angularVelocities);
+            SolveVelocityConstraints(island, options, velocityConstraints, linearVelocities, angularVelocities);
         }
 
         // Store for warm starting.
@@ -856,7 +861,7 @@ public abstract partial class SharedPhysicsSystem
 
         for (var i = 0; i < data.PositionIterations; i++)
         {
-            var contactsOkay = SolvePositionConstraints(data, in island, positionConstraints, positions, angles);
+            var contactsOkay = SolvePositionConstraints(data, in island, options, positionConstraints, positions, angles);
             var jointsOkay = true;
 
             for (var j = 0; j < island.Joints.Count; ++j)
@@ -886,19 +891,23 @@ public abstract partial class SharedPhysicsSystem
         // Solve positions now and store for later; we can't write this safely in parallel.
         var bodies = island.Bodies;
 
-        var finaliseJob = new FinalisePositionJob()
+        if (options != null)
         {
-            Physics = this,
-            Offset = offset,
-            Bodies = bodies,
-            XformQuery = xformQuery,
-            Positions = positions,
-            Angles = angles,
-            SolvedPositions = solvedPositions,
-            SolvedAngles = solvedAngles,
-        };
+            const int FinaliseBodies = 32;
+            var batches = (int)MathF.Ceiling((float) bodyCount / FinaliseBodies);
 
-        _parallel.ProcessSerialNow(finaliseJob, bodyCount);
+            Parallel.For(0, batches, options, i =>
+            {
+                var start = i * FinaliseBodies;
+                var end = Math.Min(bodyCount, start + FinaliseBodies);
+
+                FinalisePositions(start, end, offset, bodies, xformQuery, positions, angles, solvedPositions, solvedAngles);
+            });
+        }
+        else
+        {
+            FinalisePositions(0, bodyCount, offset, bodies,xformQuery, positions, angles, solvedPositions, solvedAngles);
+        }
 
         // Check sleep status for all of the bodies
         // Writing sleep timer is safe but updating awake or not is not safe.
@@ -975,26 +984,29 @@ public abstract partial class SharedPhysicsSystem
         ArrayPool<ContactPositionConstraint>.Shared.Return(positionConstraints);
     }
 
-    private void FinalisePositions(int index, int offset, List<PhysicsComponent> bodies, EntityQuery<TransformComponent> xformQuery, Vector2[] positions, float[] angles, Vector2[] solvedPositions, float[] solvedAngles)
+    private void FinalisePositions(int start, int end, int offset, List<PhysicsComponent> bodies, EntityQuery<TransformComponent> xformQuery, Vector2[] positions, float[] angles, Vector2[] solvedPositions, float[] solvedAngles)
     {
-        var body = bodies[index];
+        for (var i = start; i < end; i++)
+        {
+            var body = bodies[i];
 
-        if (body.BodyType == BodyType.Static)
-            return;
+            if (body.BodyType == BodyType.Static)
+                continue;
 
-        var xform = xformQuery.GetComponent(body.Owner);
-        var parentXform = xformQuery.GetComponent(xform.ParentUid);
-        var (_, parentRot, parentInvMatrix) = parentXform.GetWorldPositionRotationInvMatrix(xformQuery);
-        var worldRot = (float) (parentRot + xform._localRotation);
+            var xform = xformQuery.GetComponent(body.Owner);
+            var parentXform = xformQuery.GetComponent(xform.ParentUid);
+            var (_, parentRot, parentInvMatrix) = parentXform.GetWorldPositionRotationInvMatrix(xformQuery);
+            var worldRot = (float) (parentRot + xform._localRotation);
 
-        var angle = angles[index];
+            var angle = angles[i];
 
-        var q = new Quaternion2D(angle);
-        var adjustedPosition = positions[index] - Physics.Transform.Mul(q, body.LocalCenter);
+            var q = new Quaternion2D(angle);
+            var adjustedPosition = positions[i] - Physics.Transform.Mul(q, body.LocalCenter);
 
-        var solvedPosition = parentInvMatrix.Transform(adjustedPosition);
-        solvedPositions[offset + index] = solvedPosition - xform.LocalPosition;
-        solvedAngles[offset + index] = angles[index] - worldRot;
+            var solvedPosition = parentInvMatrix.Transform(adjustedPosition);
+            solvedPositions[offset + i] = solvedPosition - xform.LocalPosition;
+            solvedAngles[offset + i] = angles[i] - worldRot;
+        }
     }
 
     /// <summary>
@@ -1055,7 +1067,7 @@ public abstract partial class SharedPhysicsSystem
             }
 
             // TODO: Should check if the values update.
-            Dirty(uid, body, metaQuery.GetComponent(uid));
+            Dirty(body, metaQuery.GetComponent(uid));
         }
     }
 
@@ -1075,49 +1087,4 @@ public abstract partial class SharedPhysicsSystem
             SetAwake(body.Owner, body, false);
         }
     }
-
-    #region Jobs
-
-    private record struct SolveIslandJob : IParallelRobustJob
-    {
-        public int BatchSize => 1;
-
-        public SharedPhysicsSystem Physics;
-        public IslandData[] Islands;
-        public SolverData Data;
-        public Vector2 Gravity;
-        public bool Prediction;
-        public Vector2[] SolvedPositions;
-        public float[] SolvedAngles;
-        public Vector2[] LinearVelocities;
-        public float[] AngularVelocities;
-        public bool[] SleepStatus;
-
-        public void Execute(int index)
-        {
-            ref var island = ref Islands[index];
-            Physics.SolveIsland(ref island, Data,Gravity, Prediction, SolvedPositions, SolvedAngles, LinearVelocities, AngularVelocities, SleepStatus);
-        }
-    }
-
-    private record struct FinalisePositionJob : IParallelRobustJob
-    {
-        public int BatchSize => 32;
-
-        public SharedPhysicsSystem Physics;
-        public EntityQuery<TransformComponent> XformQuery;
-        public int Offset;
-        public List<PhysicsComponent> Bodies;
-        public Vector2[] Positions;
-        public float[] Angles;
-        public Vector2[] SolvedPositions;
-        public float[] SolvedAngles;
-
-        public void Execute(int index)
-        {
-            Physics.FinalisePositions(index, Offset, Bodies, XformQuery, Positions, Angles, SolvedPositions, SolvedAngles);
-        }
-    }
-
-    #endregion
 }

Robust.Shared/Physics/Systems/SharedPhysicsSystem.Solver.cs

@@ -21,7 +21,6 @@
 */
 
 using System;
-using System.Buffers;
 using System.Numerics;
 using System.Threading;
 using System.Threading.Tasks;
@@ -30,7 +29,6 @@ using Robust.Shared.Physics.Collision;
 using Robust.Shared.Physics.Components;
 using Robust.Shared.Physics.Dynamics;
 using Robust.Shared.Physics.Dynamics.Contacts;
-using Robust.Shared.Threading;
 using Robust.Shared.Utility;
 
 namespace Robust.Shared.Physics.Systems;
@@ -296,16 +294,11 @@ public abstract partial class SharedPhysicsSystem
         Vector2[] linearVelocities,
         float[] angularVelocities)
     {
-        var contactCount = island.Contacts.Count;
-
-        if (contactCount == 0)
-            return;
-
         var offset = island.Offset;
 
-        for (var i = 0; i < contactCount; ++i)
+        for (var i = 0; i < island.Contacts.Count; ++i)
         {
-            ref var velocityConstraint = ref velocityConstraints[i];
+            var velocityConstraint = velocityConstraints[i];
 
             var indexA = velocityConstraint.IndexA;
             var indexB = velocityConstraint.IndexB;
@@ -336,302 +329,312 @@ public abstract partial class SharedPhysicsSystem
     }
 
     private void SolveVelocityConstraints(IslandData island,
+        ParallelOptions? options,
         ContactVelocityConstraint[] velocityConstraints,
         Vector2[] linearVelocities,
         float[] angularVelocities)
     {
         var contactCount = island.Contacts.Count;
 
-        if (contactCount == 0)
-            return;
-
-        var job = new SolveVelocityJob()
+        if (options != null && contactCount > VelocityConstraintsPerThread * 2)
         {
-            Physics = this,
-            Island = island,
-            VelocityConstraints = velocityConstraints,
-            LinearVelocities = linearVelocities,
-            AngularVelocities = angularVelocities,
-        };
+            var batches = (int) Math.Ceiling((float) contactCount / VelocityConstraintsPerThread);
 
-        _parallel.ProcessSerialNow(job, contactCount);
+            Parallel.For(0, batches, options, i =>
+            {
+                var start = i * VelocityConstraintsPerThread;
+                var end = Math.Min(start + VelocityConstraintsPerThread, contactCount);
+                SolveVelocityConstraints(island, start, end, velocityConstraints, linearVelocities, angularVelocities);
+            });
+        }
+        else
+        {
+            SolveVelocityConstraints(island, 0, contactCount, velocityConstraints, linearVelocities, angularVelocities);
+        }
     }
 
-    private void SolveVelocityConstraint(
+    private void SolveVelocityConstraints(
         IslandData island,
-        ref ContactVelocityConstraint velocityConstraint,
+        int start,
+        int end,
+        ContactVelocityConstraint[] velocityConstraints,
         Vector2[] linearVelocities,
         float[] angularVelocities)
     {
         var offset = island.Offset;
 
         // Here be dragons
-        var indexA = velocityConstraint.IndexA;
-        var indexB = velocityConstraint.IndexB;
-        var mA = velocityConstraint.InvMassA;
-        var iA = velocityConstraint.InvIA;
-        var mB = velocityConstraint.InvMassB;
-        var iB = velocityConstraint.InvIB;
-        var pointCount = velocityConstraint.PointCount;
-
-        ref var vA = ref linearVelocities[offset + indexA];
-        ref var wA = ref angularVelocities[offset + indexA];
-        ref var vB = ref linearVelocities[offset + indexB];
-        ref var wB = ref angularVelocities[offset + indexB];
-
-        var normal = velocityConstraint.Normal;
-        var tangent = Vector2Helpers.Cross(normal, 1.0f);
-        var friction = velocityConstraint.Friction;
-
-        DebugTools.Assert(pointCount is 1 or 2);
-
-        // Solve tangent constraints first because non-penetration is more important
-        // than friction.
-        for (var j = 0; j < pointCount; ++j)
+        for (var i = start; i < end; ++i)
         {
-            ref var velConstraintPoint = ref velocityConstraint.Points[j];
+            ref var velocityConstraint = ref velocityConstraints[i];
 
-            // Relative velocity at contact
-            var dv = vB + Vector2Helpers.Cross(wB, velConstraintPoint.RelativeVelocityB) - vA - Vector2Helpers.Cross(wA, velConstraintPoint.RelativeVelocityA);
+            var indexA = velocityConstraint.IndexA;
+            var indexB = velocityConstraint.IndexB;
+            var mA = velocityConstraint.InvMassA;
+            var iA = velocityConstraint.InvIA;
+            var mB = velocityConstraint.InvMassB;
+            var iB = velocityConstraint.InvIB;
+            var pointCount = velocityConstraint.PointCount;
 
-            // Compute tangent force
-            float vt = Vector2.Dot(dv, tangent) - velocityConstraint.TangentSpeed;
-            float lambda = velConstraintPoint.TangentMass * (-vt);
+            ref var vA = ref linearVelocities[offset + indexA];
+            ref var wA = ref angularVelocities[offset + indexA];
+            ref var vB = ref linearVelocities[offset + indexB];
+            ref var wB = ref angularVelocities[offset + indexB];
 
-            // b2Clamp the accumulated force
-            var maxFriction = friction * velConstraintPoint.NormalImpulse;
-            var newImpulse = Math.Clamp(velConstraintPoint.TangentImpulse + lambda, -maxFriction, maxFriction);
-            lambda = newImpulse - velConstraintPoint.TangentImpulse;
-            velConstraintPoint.TangentImpulse = newImpulse;
+            var normal = velocityConstraint.Normal;
+            var tangent = Vector2Helpers.Cross(normal, 1.0f);
+            var friction = velocityConstraint.Friction;
 
-            // Apply contact impulse
-            Vector2 P = tangent * lambda;
+            DebugTools.Assert(pointCount is 1 or 2);
 
-            vA -= P * mA;
-            wA -= iA * Vector2Helpers.Cross(velConstraintPoint.RelativeVelocityA, P);
-
-            vB += P * mB;
-            wB += iB * Vector2Helpers.Cross(velConstraintPoint.RelativeVelocityB, P);
-        }
-
-        // Solve normal constraints
-        if (velocityConstraint.PointCount == 1)
-        {
-            ref var vcp = ref velocityConstraint.Points[0];
-
-            // Relative velocity at contact
-            Vector2 dv = vB + Vector2Helpers.Cross(wB, vcp.RelativeVelocityB) - vA - Vector2Helpers.Cross(wA, vcp.RelativeVelocityA);
-
-            // Compute normal impulse
-            float vn = Vector2.Dot(dv, normal);
-            float lambda = -vcp.NormalMass * (vn - vcp.VelocityBias);
-
-            // b2Clamp the accumulated impulse
-            float newImpulse = Math.Max(vcp.NormalImpulse + lambda, 0.0f);
-            lambda = newImpulse - vcp.NormalImpulse;
-            vcp.NormalImpulse = newImpulse;
-
-            // Apply contact impulse
-            Vector2 P = normal * lambda;
-            vA -= P * mA;
-            wA -= iA * Vector2Helpers.Cross(vcp.RelativeVelocityA, P);
-
-            vB += P * mB;
-            wB += iB * Vector2Helpers.Cross(vcp.RelativeVelocityB, P);
-        }
-        else
-        {
-            // Block solver developed in collaboration with Dirk Gregorius (back in 01/07 on Box2D_Lite).
-            // Build the mini LCP for this contact patch
-            //
-            // vn = A * x + b, vn >= 0, , vn >= 0, x >= 0 and vn_i * x_i = 0 with i = 1..2
-            //
-            // A = J * W * JT and J = ( -n, -r1 x n, n, r2 x n )
-            // b = vn0 - velocityBias
-            //
-            // The system is solved using the "Total enumeration method" (s. Murty). The complementary constraint vn_i * x_i
-            // implies that we must have in any solution either vn_i = 0 or x_i = 0. So for the 2D contact problem the cases
-            // vn1 = 0 and vn2 = 0, x1 = 0 and x2 = 0, x1 = 0 and vn2 = 0, x2 = 0 and vn1 = 0 need to be tested. The first valid
-            // solution that satisfies the problem is chosen.
-            //
-            // In order to account of the accumulated impulse 'a' (because of the iterative nature of the solver which only requires
-            // that the accumulated impulse is clamped and not the incremental impulse) we change the impulse variable (x_i).
-            //
-            // Substitute:
-            //
-            // x = a + d
-            //
-            // a := old total impulse
-            // x := new total impulse
-            // d := incremental impulse
-            //
-            // For the current iteration we extend the formula for the incremental impulse
-            // to compute the new total impulse:
-            //
-            // vn = A * d + b
-            //    = A * (x - a) + b
-            //    = A * x + b - A * a
-            //    = A * x + b'
-            // b' = b - A * a;
-
-            ref var cp1 = ref velocityConstraint.Points[0];
-            ref var cp2 = ref velocityConstraint.Points[1];
-
-            Vector2 a = new Vector2(cp1.NormalImpulse, cp2.NormalImpulse);
-            DebugTools.Assert(a.X >= 0.0f && a.Y >= 0.0f);
-
-            // Relative velocity at contact
-            Vector2 dv1 = vB + Vector2Helpers.Cross(wB, cp1.RelativeVelocityB) - vA - Vector2Helpers.Cross(wA, cp1.RelativeVelocityA);
-            Vector2 dv2 = vB + Vector2Helpers.Cross(wB, cp2.RelativeVelocityB) - vA - Vector2Helpers.Cross(wA, cp2.RelativeVelocityA);
-
-            // Compute normal velocity
-            float vn1 = Vector2.Dot(dv1, normal);
-            float vn2 = Vector2.Dot(dv2, normal);
-
-            Vector2 b = new Vector2
+            // Solve tangent constraints first because non-penetration is more important
+            // than friction.
+            for (var j = 0; j < pointCount; ++j)
             {
-                X = vn1 - cp1.VelocityBias,
-                Y = vn2 - cp2.VelocityBias
-            };
+                ref var velConstraintPoint = ref velocityConstraint.Points[j];
 
-            // Compute b'
-            b -= Physics.Transform.Mul(velocityConstraint.K, a);
+                // Relative velocity at contact
+                var dv = vB + Vector2Helpers.Cross(wB, velConstraintPoint.RelativeVelocityB) - vA - Vector2Helpers.Cross(wA, velConstraintPoint.RelativeVelocityA);
 
-            //const float k_errorTol = 1e-3f;
-            //B2_NOT_USED(k_errorTol);
+                // Compute tangent force
+                float vt = Vector2.Dot(dv, tangent) - velocityConstraint.TangentSpeed;
+                float lambda = velConstraintPoint.TangentMass * (-vt);
 
-            for (; ; )
+                // b2Clamp the accumulated force
+                var maxFriction = friction * velConstraintPoint.NormalImpulse;
+                var newImpulse = Math.Clamp(velConstraintPoint.TangentImpulse + lambda, -maxFriction, maxFriction);
+                lambda = newImpulse - velConstraintPoint.TangentImpulse;
+                velConstraintPoint.TangentImpulse = newImpulse;
+
+                // Apply contact impulse
+                Vector2 P = tangent * lambda;
+
+                vA -= P * mA;
+                wA -= iA * Vector2Helpers.Cross(velConstraintPoint.RelativeVelocityA, P);
+
+                vB += P * mB;
+                wB += iB * Vector2Helpers.Cross(velConstraintPoint.RelativeVelocityB, P);
+            }
+
+            // Solve normal constraints
+            if (velocityConstraint.PointCount == 1)
             {
-                //
-                // Case 1: vn = 0
-                //
-                // 0 = A * x + b'
-                //
-                // Solve for x:
-                //
-                // x = - inv(A) * b'
-                //
-                Vector2 x = -Physics.Transform.Mul(velocityConstraint.NormalMass, b);
+                ref var vcp = ref velocityConstraint.Points[0];
 
-                if (x.X >= 0.0f && x.Y >= 0.0f)
+                // Relative velocity at contact
+                Vector2 dv = vB + Vector2Helpers.Cross(wB, vcp.RelativeVelocityB) - vA - Vector2Helpers.Cross(wA, vcp.RelativeVelocityA);
+
+                // Compute normal impulse
+                float vn = Vector2.Dot(dv, normal);
+                float lambda = -vcp.NormalMass * (vn - vcp.VelocityBias);
+
+                // b2Clamp the accumulated impulse
+                float newImpulse = Math.Max(vcp.NormalImpulse + lambda, 0.0f);
+                lambda = newImpulse - vcp.NormalImpulse;
+                vcp.NormalImpulse = newImpulse;
+
+                // Apply contact impulse
+                Vector2 P = normal * lambda;
+                vA -= P * mA;
+                wA -= iA * Vector2Helpers.Cross(vcp.RelativeVelocityA, P);
+
+                vB += P * mB;
+                wB += iB * Vector2Helpers.Cross(vcp.RelativeVelocityB, P);
+            }
+            else
+            {
+                // Block solver developed in collaboration with Dirk Gregorius (back in 01/07 on Box2D_Lite).
+                // Build the mini LCP for this contact patch
+                //
+                // vn = A * x + b, vn >= 0, , vn >= 0, x >= 0 and vn_i * x_i = 0 with i = 1..2
+                //
+                // A = J * W * JT and J = ( -n, -r1 x n, n, r2 x n )
+                // b = vn0 - velocityBias
+                //
+                // The system is solved using the "Total enumeration method" (s. Murty). The complementary constraint vn_i * x_i
+                // implies that we must have in any solution either vn_i = 0 or x_i = 0. So for the 2D contact problem the cases
+                // vn1 = 0 and vn2 = 0, x1 = 0 and x2 = 0, x1 = 0 and vn2 = 0, x2 = 0 and vn1 = 0 need to be tested. The first valid
+                // solution that satisfies the problem is chosen.
+                //
+                // In order to account of the accumulated impulse 'a' (because of the iterative nature of the solver which only requires
+                // that the accumulated impulse is clamped and not the incremental impulse) we change the impulse variable (x_i).
+                //
+                // Substitute:
+                //
+                // x = a + d
+                //
+                // a := old total impulse
+                // x := new total impulse
+                // d := incremental impulse
+                //
+                // For the current iteration we extend the formula for the incremental impulse
+                // to compute the new total impulse:
+                //
+                // vn = A * d + b
+                //    = A * (x - a) + b
+                //    = A * x + b - A * a
+                //    = A * x + b'
+                // b' = b - A * a;
+
+                ref var cp1 = ref velocityConstraint.Points[0];
+                ref var cp2 = ref velocityConstraint.Points[1];
+
+                Vector2 a = new Vector2(cp1.NormalImpulse, cp2.NormalImpulse);
+                DebugTools.Assert(a.X >= 0.0f && a.Y >= 0.0f);
+
+                // Relative velocity at contact
+                Vector2 dv1 = vB + Vector2Helpers.Cross(wB, cp1.RelativeVelocityB) - vA - Vector2Helpers.Cross(wA, cp1.RelativeVelocityA);
+                Vector2 dv2 = vB + Vector2Helpers.Cross(wB, cp2.RelativeVelocityB) - vA - Vector2Helpers.Cross(wA, cp2.RelativeVelocityA);
+
+                // Compute normal velocity
+                float vn1 = Vector2.Dot(dv1, normal);
+                float vn2 = Vector2.Dot(dv2, normal);
+
+                Vector2 b = new Vector2
                 {
-                    // Get the incremental impulse
-                    Vector2 d = x - a;
+                    X = vn1 - cp1.VelocityBias,
+                    Y = vn2 - cp2.VelocityBias
+                };
 
-                    // Apply incremental impulse
-                    Vector2 P1 = normal * d.X;
-                    Vector2 P2 = normal * d.Y;
-                    vA -= (P1 + P2) * mA;
-                    wA -= iA * (Vector2Helpers.Cross(cp1.RelativeVelocityA, P1) + Vector2Helpers.Cross(cp2.RelativeVelocityA, P2));
+                // Compute b'
+                b -= Physics.Transform.Mul(velocityConstraint.K, a);
 
-                    vB += (P1 + P2) * mB;
-                    wB += iB * (Vector2Helpers.Cross(cp1.RelativeVelocityB, P1) + Vector2Helpers.Cross(cp2.RelativeVelocityB, P2));
+                //const float k_errorTol = 1e-3f;
+                //B2_NOT_USED(k_errorTol);
 
-                    // Accumulate
-                    cp1.NormalImpulse = x.X;
-                    cp2.NormalImpulse = x.Y;
+                for (; ; )
+                {
+                    //
+                    // Case 1: vn = 0
+                    //
+                    // 0 = A * x + b'
+                    //
+                    // Solve for x:
+                    //
+                    // x = - inv(A) * b'
+                    //
+                    Vector2 x = -Physics.Transform.Mul(velocityConstraint.NormalMass, b);
 
+                    if (x.X >= 0.0f && x.Y >= 0.0f)
+                    {
+                        // Get the incremental impulse
+                        Vector2 d = x - a;
+
+                        // Apply incremental impulse
+                        Vector2 P1 = normal * d.X;
+                        Vector2 P2 = normal * d.Y;
+                        vA -= (P1 + P2) * mA;
+                        wA -= iA * (Vector2Helpers.Cross(cp1.RelativeVelocityA, P1) + Vector2Helpers.Cross(cp2.RelativeVelocityA, P2));
+
+                        vB += (P1 + P2) * mB;
+                        wB += iB * (Vector2Helpers.Cross(cp1.RelativeVelocityB, P1) + Vector2Helpers.Cross(cp2.RelativeVelocityB, P2));
+
+                        // Accumulate
+                        cp1.NormalImpulse = x.X;
+                        cp2.NormalImpulse = x.Y;
+
+                        break;
+                    }
+
+                    //
+                    // Case 2: vn1 = 0 and x2 = 0
+                    //
+                    //   0 = a11 * x1 + a12 * 0 + b1'
+                    // vn2 = a21 * x1 + a22 * 0 + b2'
+                    //
+                    x.X = -cp1.NormalMass * b.X;
+                    x.Y = 0.0f;
+                    vn1 = 0.0f;
+                    vn2 = velocityConstraint.K.Y * x.X + b.Y;
+
+                    if (x.X >= 0.0f && vn2 >= 0.0f)
+                    {
+                        // Get the incremental impulse
+                        Vector2 d = x - a;
+
+                        // Apply incremental impulse
+                        Vector2 P1 = normal * d.X;
+                        Vector2 P2 = normal * d.Y;
+                        vA -= (P1 + P2) * mA;
+                        wA -= iA * (Vector2Helpers.Cross(cp1.RelativeVelocityA, P1) + Vector2Helpers.Cross(cp2.RelativeVelocityA, P2));
+
+                        vB += (P1 + P2) * mB;
+                        wB += iB * (Vector2Helpers.Cross(cp1.RelativeVelocityB, P1) + Vector2Helpers.Cross(cp2.RelativeVelocityB, P2));
+
+                        // Accumulate
+                        cp1.NormalImpulse = x.X;
+                        cp2.NormalImpulse = x.Y;
+
+                        break;
+                    }
+
+
+                    //
+                    // Case 3: vn2 = 0 and x1 = 0
+                    //
+                    // vn1 = a11 * 0 + a12 * x2 + b1'
+                    //   0 = a21 * 0 + a22 * x2 + b2'
+                    //
+                    x.X = 0.0f;
+                    x.Y = -cp2.NormalMass * b.Y;
+                    vn1 = velocityConstraint.K.Z * x.Y + b.X;
+                    vn2 = 0.0f;
+
+                    if (x.Y >= 0.0f && vn1 >= 0.0f)
+                    {
+                        // Resubstitute for the incremental impulse
+                        Vector2 d = x - a;
+
+                        // Apply incremental impulse
+                        Vector2 P1 = normal * d.X;
+                        Vector2 P2 = normal * d.Y;
+                        vA -= (P1 + P2) * mA;
+                        wA -= iA * (Vector2Helpers.Cross(cp1.RelativeVelocityA, P1) + Vector2Helpers.Cross(cp2.RelativeVelocityA, P2));
+
+                        vB += (P1 + P2) * mB;
+                        wB += iB * (Vector2Helpers.Cross(cp1.RelativeVelocityB, P1) + Vector2Helpers.Cross(cp2.RelativeVelocityB, P2));
+
+                        // Accumulate
+                        cp1.NormalImpulse = x.X;
+                        cp2.NormalImpulse = x.Y;
+
+                        break;
+                    }
+
+                    //
+                    // Case 4: x1 = 0 and x2 = 0
+                    //
+                    // vn1 = b1
+                    // vn2 = b2;
+                    x.X = 0.0f;
+                    x.Y = 0.0f;
+                    vn1 = b.X;
+                    vn2 = b.Y;
+
+                    if (vn1 >= 0.0f && vn2 >= 0.0f)
+                    {
+                        // Resubstitute for the incremental impulse
+                        Vector2 d = x - a;
+
+                        // Apply incremental impulse
+                        Vector2 P1 = normal * d.X;
+                        Vector2 P2 = normal * d.Y;
+                        vA -= (P1 + P2) * mA;
+                        wA -= iA * (Vector2Helpers.Cross(cp1.RelativeVelocityA, P1) + Vector2Helpers.Cross(cp2.RelativeVelocityA, P2));
+
+                        vB += (P1 + P2) * mB;
+                        wB += iB * (Vector2Helpers.Cross(cp1.RelativeVelocityB, P1) + Vector2Helpers.Cross(cp2.RelativeVelocityB, P2));
+
+                        // Accumulate
+                        cp1.NormalImpulse = x.X;
+                        cp2.NormalImpulse = x.Y;
+
+                        break;
+                    }
+
+                    // No solution, give up. This is hit sometimes, but it doesn't seem to matter.
                     break;
                 }
-
-                //
-                // Case 2: vn1 = 0 and x2 = 0
-                //
-                //   0 = a11 * x1 + a12 * 0 + b1'
-                // vn2 = a21 * x1 + a22 * 0 + b2'
-                //
-                x.X = -cp1.NormalMass * b.X;
-                x.Y = 0.0f;
-                vn1 = 0.0f;
-                vn2 = velocityConstraint.K.Y * x.X + b.Y;
-
-                if (x.X >= 0.0f && vn2 >= 0.0f)
-                {
-                    // Get the incremental impulse
-                    Vector2 d = x - a;
-
-                    // Apply incremental impulse
-                    Vector2 P1 = normal * d.X;
-                    Vector2 P2 = normal * d.Y;
-                    vA -= (P1 + P2) * mA;
-                    wA -= iA * (Vector2Helpers.Cross(cp1.RelativeVelocityA, P1) + Vector2Helpers.Cross(cp2.RelativeVelocityA, P2));
-
-                    vB += (P1 + P2) * mB;
-                    wB += iB * (Vector2Helpers.Cross(cp1.RelativeVelocityB, P1) + Vector2Helpers.Cross(cp2.RelativeVelocityB, P2));
-
-                    // Accumulate
-                    cp1.NormalImpulse = x.X;
-                    cp2.NormalImpulse = x.Y;
-
-                    break;
-                }
-
-
-                //
-                // Case 3: vn2 = 0 and x1 = 0
-                //
-                // vn1 = a11 * 0 + a12 * x2 + b1'
-                //   0 = a21 * 0 + a22 * x2 + b2'
-                //
-                x.X = 0.0f;
-                x.Y = -cp2.NormalMass * b.Y;
-                vn1 = velocityConstraint.K.Z * x.Y + b.X;
-                vn2 = 0.0f;
-
-                if (x.Y >= 0.0f && vn1 >= 0.0f)
-                {
-                    // Resubstitute for the incremental impulse
-                    Vector2 d = x - a;
-
-                    // Apply incremental impulse
-                    Vector2 P1 = normal * d.X;
-                    Vector2 P2 = normal * d.Y;
-                    vA -= (P1 + P2) * mA;
-                    wA -= iA * (Vector2Helpers.Cross(cp1.RelativeVelocityA, P1) + Vector2Helpers.Cross(cp2.RelativeVelocityA, P2));
-
-                    vB += (P1 + P2) * mB;
-                    wB += iB * (Vector2Helpers.Cross(cp1.RelativeVelocityB, P1) + Vector2Helpers.Cross(cp2.RelativeVelocityB, P2));
-
-                    // Accumulate
-                    cp1.NormalImpulse = x.X;
-                    cp2.NormalImpulse = x.Y;
-
-                    break;
-                }
-
-                //
-                // Case 4: x1 = 0 and x2 = 0
-                //
-                // vn1 = b1
-                // vn2 = b2;
-                x.X = 0.0f;
-                x.Y = 0.0f;
-                vn1 = b.X;
-                vn2 = b.Y;
-
-                if (vn1 >= 0.0f && vn2 >= 0.0f)
-                {
-                    // Resubstitute for the incremental impulse
-                    Vector2 d = x - a;
-
-                    // Apply incremental impulse
-                    Vector2 P1 = normal * d.X;
-                    Vector2 P2 = normal * d.Y;
-                    vA -= (P1 + P2) * mA;
-                    wA -= iA * (Vector2Helpers.Cross(cp1.RelativeVelocityA, P1) + Vector2Helpers.Cross(cp2.RelativeVelocityA, P2));
-
-                    vB += (P1 + P2) * mB;
-                    wB += iB * (Vector2Helpers.Cross(cp1.RelativeVelocityB, P1) + Vector2Helpers.Cross(cp2.RelativeVelocityB, P2));
-
-                    // Accumulate
-                    cp1.NormalImpulse = x.X;
-                    cp2.NormalImpulse = x.Y;
-
-                    break;
-                }
-
-                // No solution, give up. This is hit sometimes, but it doesn't seem to matter.
-                break;
             }
         }
     }
@@ -655,110 +658,107 @@ public abstract partial class SharedPhysicsSystem
     private bool SolvePositionConstraints(
         SolverData data,
         in IslandData island,
+        ParallelOptions? options,
         ContactPositionConstraint[] positionConstraints,
         Vector2[] positions,
         float[] angles)
     {
         var contactCount = island.Contacts.Count;
 
-        if (contactCount == 0)
-            return true;
-
-        var solved = ArrayPool<bool>.Shared.Rent(contactCount);
-
-        var job = new SolvePositionJob()
-        {
-            Physics = this,
-            Data = data,
-            PositionConstraints = positionConstraints,
-            Positions = positions,
-            Angles = angles,
-            Solved = solved
-        };
-
         // Parallel
-        _parallel.ProcessSerialNow(job, contactCount);
-        var isSolved = true;
-
-        for (var i = 0; i < contactCount; i++)
+        if (options != null && contactCount > PositionConstraintsPerThread * 2)
         {
-            if (solved[i])
-                continue;
+            var unsolved = 0;
+            var batches = (int) Math.Ceiling((float) contactCount / PositionConstraintsPerThread);
 
-            isSolved = false;
-            break;
+            Parallel.For(0, batches, options, i =>
+            {
+                var start = i * PositionConstraintsPerThread;
+                var end = Math.Min(start + PositionConstraintsPerThread, contactCount);
+
+                if (!SolvePositionConstraints(data, start, end, positionConstraints, positions, angles))
+                    Interlocked.Increment(ref unsolved);
+            });
+
+            return unsolved == 0;
         }
 
-        ArrayPool<bool>.Shared.Return(solved);
-
-        return isSolved;
+        // No parallel
+        return SolvePositionConstraints(data, 0, contactCount, positionConstraints, positions, angles);
     }
 
     /// <summary>
     ///     Tries to solve positions for all contacts specified.
     /// </summary>
     /// <returns>true if all positions solved</returns>
-    private bool SolvePositionConstraint(
+    private bool SolvePositionConstraints(
         SolverData data,
-        ref ContactPositionConstraint positionConstraint,
+        int start,
+        int end,
+        ContactPositionConstraint[] positionConstraints,
         Vector2[] positions,
         float[] angles)
     {
         float minSeparation = 0.0f;
 
-        int indexA = positionConstraint.IndexA;
-        int indexB = positionConstraint.IndexB;
-        Vector2 localCenterA = positionConstraint.LocalCenterA;
-        float mA = positionConstraint.InvMassA;
-        float iA = positionConstraint.InvIA;
-        Vector2 localCenterB = positionConstraint.LocalCenterB;
-        float mB = positionConstraint.InvMassB;
-        float iB = positionConstraint.InvIB;
-        int pointCount = positionConstraint.PointCount;
-
-        ref var centerA = ref positions[indexA];
-        ref var angleA = ref angles[indexA];
-        ref var centerB = ref positions[indexB];
-        ref var angleB = ref angles[indexB];
-
-        // Solve normal constraints
-        for (int j = 0; j < pointCount; ++j)
+        for (int i = start; i < end; ++i)
         {
-            Transform xfA = new Transform(angleA);
-            Transform xfB = new Transform(angleB);
-            xfA.Position = centerA - Physics.Transform.Mul(xfA.Quaternion2D, localCenterA);
-            xfB.Position = centerB - Physics.Transform.Mul(xfB.Quaternion2D, localCenterB);
+            var pc = positionConstraints[i];
 
-            Vector2 normal;
-            Vector2 point;
-            float separation;
+            int indexA = pc.IndexA;
+            int indexB = pc.IndexB;
+            Vector2 localCenterA = pc.LocalCenterA;
+            float mA = pc.InvMassA;
+            float iA = pc.InvIA;
+            Vector2 localCenterB = pc.LocalCenterB;
+            float mB = pc.InvMassB;
+            float iB = pc.InvIB;
+            int pointCount = pc.PointCount;
 
-            PositionSolverManifoldInitialize(positionConstraint, j, xfA, xfB, out normal, out point, out separation);
+            ref var centerA = ref positions[indexA];
+            ref var angleA = ref angles[indexA];
+            ref var centerB = ref positions[indexB];
+            ref var angleB = ref angles[indexB];
 
-            Vector2 rA = point - centerA;
-            Vector2 rB = point - centerB;
+            // Solve normal constraints
+            for (int j = 0; j < pointCount; ++j)
+            {
+                Transform xfA = new Transform(angleA);
+                Transform xfB = new Transform(angleB);
+                xfA.Position = centerA - Physics.Transform.Mul(xfA.Quaternion2D, localCenterA);
+                xfB.Position = centerB - Physics.Transform.Mul(xfB.Quaternion2D, localCenterB);
 
-            // Track max constraint error.
-            minSeparation = Math.Min(minSeparation, separation);
+                Vector2 normal;
+                Vector2 point;
+                float separation;
 
-            // Prevent large corrections and allow slop.
-            float C = Math.Clamp(data.Baumgarte * (separation + PhysicsConstants.LinearSlop), -_maxLinearCorrection, 0.0f);
+                PositionSolverManifoldInitialize(pc, j, xfA, xfB, out normal, out point, out separation);
 
-            // Compute the effective mass.
-            float rnA = Vector2Helpers.Cross(rA, normal);
-            float rnB = Vector2Helpers.Cross(rB, normal);
-            float K = mA + mB + iA * rnA * rnA + iB * rnB * rnB;
+                Vector2 rA = point - centerA;
+                Vector2 rB = point - centerB;
 
-            // Compute normal impulse
-            float impulse = K > 0.0f ? -C / K : 0.0f;
+                // Track max constraint error.
+                minSeparation = Math.Min(minSeparation, separation);
 
-            Vector2 P = normal * impulse;
+                // Prevent large corrections and allow slop.
+                float C = Math.Clamp(data.Baumgarte * (separation + PhysicsConstants.LinearSlop), -_maxLinearCorrection, 0.0f);
 
-            centerA -= P * mA;
-            angleA -= iA * Vector2Helpers.Cross(rA, P);
+                // Compute the effective mass.
+                float rnA = Vector2Helpers.Cross(rA, normal);
+                float rnB = Vector2Helpers.Cross(rB, normal);
+                float K = mA + mB + iA * rnA * rnA + iB * rnB * rnB;
 
-            centerB += P * mB;
-            angleB += iB * Vector2Helpers.Cross(rB, P);
+                // Compute normal impulse
+                float impulse = K > 0.0f ? -C / K : 0.0f;
+
+                Vector2 P = normal * impulse;
+
+                centerA -= P * mA;
+                angleA -= iA * Vector2Helpers.Cross(rA, P);
+
+                centerB += P * mB;
+                angleB += iB * Vector2Helpers.Cross(rB, P);
+            }
         }
 
         // We can't expect minSpeparation >= -b2_linearSlop because we don't
@@ -907,51 +907,4 @@ public abstract partial class SharedPhysicsSystem
 
             }
     }
-
-    #region Jobs
-
-    private record struct SolvePositionJob : IParallelRobustJob
-    {
-        public int BatchSize => 16;
-
-        public SharedPhysicsSystem Physics;
-        public SolverData Data;
-        public ContactPositionConstraint[] PositionConstraints;
-        public Vector2[] Positions;
-        public float[] Angles;
-        public bool[] Solved;
-
-        public void Execute(int index)
-        {
-            ref var constraint = ref PositionConstraints[index];
-
-            if (Physics.SolvePositionConstraint(Data, ref constraint, Positions, Angles))
-            {
-                Solved[index] = true;
-            }
-            else
-            {
-                Solved[index] = false;
-            }
-        }
-    }
-
-    private record struct SolveVelocityJob : IParallelRobustJob
-    {
-        public int BatchSize => 16;
-
-        public SharedPhysicsSystem Physics;
-        public IslandData Island;
-        public ContactVelocityConstraint[] VelocityConstraints;
-        public Vector2[] LinearVelocities;
-        public float[] AngularVelocities;
-
-        public void Execute(int index)
-        {
-            ref var constraint = ref VelocityConstraints[index];
-            Physics.SolveVelocityConstraint(Island, ref constraint, LinearVelocities, AngularVelocities);
-        }
-    }
-
-    #endregion
 }