Vectorize contact solving and reuse world step storage

src/Box2D.NET/B2Arrays.cs

@@ -105,12 +105,13 @@ public static void b2Array_Set<T>(ref B2Array<T> a, int index, T value)
             if (index != a.count - 1)
             {
                 movedIndex = a.count - 1;
+                T removed = a.data[index];
                 a.data[index] = a.data[movedIndex];
 
                 // fixed, ikpil
                 if (!typeof(T).IsValueType)
                 {
-                    a.data[movedIndex] = new T();
+                    a.data[movedIndex] = removed;
                 }
             }
 

src/Box2D.NET/B2ContactConstraintWide.cs

@@ -10,8 +10,8 @@ namespace Box2D.NET
     // https://box2d.org/files/ErinCatto_SoftConstraints_GDC2011.pdf
     public struct B2ContactConstraintWide
     {
-        public B2FixedArray4<int> indexA; // = new int[B2_SIMD_WIDTH];
-        public B2FixedArray4<int> indexB; // = new int[B2_SIMD_WIDTH];
+        public B2FixedArray8<int> indexA; // = new int[B2_SIMD_WIDTH];
+        public B2FixedArray8<int> indexB; // = new int[B2_SIMD_WIDTH];
 
         public B2FloatW invMassA, invMassB;
         public B2FloatW invIA, invIB;

src/Box2D.NET/B2Contacts.cs

@@ -197,7 +197,7 @@ public static void b2CreateContact(B2World world, B2Shape shapeA, B2Shape shapeB
             int contactId = b2AllocId(world.contactIdPool);
             if (contactId == world.contacts.count)
             {
-                b2Array_Push(ref world.contacts, new B2Contact());
+                b2Array_Emplace(ref world.contacts);
             }
 
             int shapeIdA = shapeA.id;

src/Box2D.NET/B2Cores.cs

@@ -12,7 +12,7 @@ namespace Box2D.NET
     public static class B2Cores
     {
         // note: I tried width of 1 and got no performance change
-        public const int B2_SIMD_WIDTH = 4;
+        public const int B2_SIMD_WIDTH = 8;
 
         /// Get the current version of Box2D
         public static B2Version b2GetVersion()

src/Box2D.NET/B2FloatW.cs

@@ -12,27 +12,47 @@ namespace Box2D.NET
     [StructLayout(LayoutKind.Sequential)]
     public struct B2FloatW
     {
-        public float X;
-        public float Y;
-        public float Z;
-        public float W;
+#if NET8_0_OR_GREATER
+        private System.Runtime.Intrinsics.Vector256<float> _value;
+        public ref float X => ref AsSpan()[0];
+        public ref float Y => ref AsSpan()[1];
+        public ref float Z => ref AsSpan()[2];
+        public ref float W => ref AsSpan()[3];
+        public ref float E => ref AsSpan()[4];
+        public ref float F => ref AsSpan()[5];
+        public ref float G => ref AsSpan()[6];
+        public ref float H => ref AsSpan()[7];
+#else
+        public float X, Y, Z, W, E, F, G, H;
+#endif
 
+        public B2FloatW(float x, float y, float z, float w) : this(x, y, z, w, 0, 0, 0, 0) { }
 
-        public B2FloatW(float x, float y, float z, float w)
+        public B2FloatW(float x, float y, float z, float w, float e, float f, float g, float h)
         {
+#if NET8_0_OR_GREATER
+            _value = System.Runtime.Intrinsics.Vector256.Create(x, y, z, w, e, f, g, h);
+#else
             X = x;
             Y = y;
             Z = z;
             W = w;
+            E = e;
+            F = f;
+            G = g;
+            H = h;
+#endif
         }
 
-        // readonly so an "in" or "ref readonly" receiver does not force a defensive
-        // copy. This matches Span<T>.this[int], which is also a readonly ref T indexer.
-        public readonly ref float this[int index] => ref MemoryMarshal.CreateSpan(ref Unsafe.AsRef(in X), 4)[index];
+        public readonly ref float this[int index] => ref AsSpan()[index];
 
         public readonly Span<float> AsSpan()
         {
-            return MemoryMarshal.CreateSpan(ref Unsafe.AsRef(in X), 4);
+#if NET8_0_OR_GREATER
+            return MemoryMarshal.CreateSpan(ref Unsafe.As<System.Runtime.Intrinsics.Vector256<float>, float>(ref Unsafe.AsRef(in _value)), 8);
+#else
+            return MemoryMarshal.CreateSpan(ref Unsafe.AsRef(in X), 8);
+#endif
         }
     }
-}
+}

src/Box2D.NET/B2Islands.cs

@@ -34,7 +34,7 @@ public static B2Island b2CreateIsland(B2World world, int setIndex)
 
             if (islandId == world.islands.count)
             {
-                b2Array_Push(ref world.islands, new B2Island());
+                b2Array_Emplace(ref world.islands);
             }
             else
             {
@@ -47,9 +47,9 @@ public static B2Island b2CreateIsland(B2World world, int setIndex)
             island.setIndex = setIndex;
             island.localIndex = set.islandSims.count;
             island.islandId = islandId;
-            island.bodies = b2Array_Create<int>();
-            island.contacts = b2Array_Create<B2ContactLink>();
-            island.joints = b2Array_Create<B2JointLink>();
+            b2Array_Clear(ref island.bodies);
+            b2Array_Clear(ref island.contacts);
+            b2Array_Clear(ref island.joints);
             island.constraintRemoveCount = 0;
 
             ref B2IslandSim islandSim = ref b2Array_Emplace(ref set.islandSims);
@@ -70,23 +70,18 @@ public static void b2DestroyIsland(B2World world, int islandId)
             B2SolverSet set = b2Array_Get(ref world.solverSets, island.setIndex);
             {
                 int localIndex = island.localIndex;
-                int lastIndex = set.islandSims.count - 1;
-                B2_ASSERT(0 <= localIndex && localIndex <= lastIndex);
-                int moveIslandId = set.islandSims.data[lastIndex].islandId;
-                set.islandSims.data[localIndex].CopyFrom(set.islandSims.data[lastIndex]);
-                world.islands.data[moveIslandId].localIndex = localIndex;
-                if (localIndex != lastIndex)
+                int movedIndex = b2Array_RemoveSwap(ref set.islandSims, localIndex);
+                if (movedIndex != B2_NULL_INDEX)
                 {
-                    set.islandSims.data[lastIndex] = new B2IslandSim();
+                    int movedIslandId = set.islandSims.data[localIndex].islandId;
+                    world.islands.data[movedIslandId].localIndex = localIndex;
                 }
-
-                set.islandSims.count -= 1;
             }
 
             // Free island and id (preserve island revision)
-            b2Array_Destroy(ref island.bodies);
-            b2Array_Destroy(ref island.contacts);
-            b2Array_Destroy(ref island.joints);
+            b2Array_Clear(ref island.bodies);
+            b2Array_Clear(ref island.contacts);
+            b2Array_Clear(ref island.joints);
             island.constraintRemoveCount = 0;
             island.localIndex = B2_NULL_INDEX;
             island.islandId = B2_NULL_INDEX;

src/Box2D.NET/B2ParallelFor.cs

@@ -47,7 +47,16 @@ private static void b2ParallelForTrampoline(object taskContext)
                     end = itemCount;
                 }
 
-                callback(start, end, workerIndex, context);
+                try
+                {
+                    callback(start, end, workerIndex, context);
+                }
+                catch (Exception failure)
+                {
+                    System.Threading.Interlocked.CompareExchange(ref shared.failure, failure, null);
+                    b2AtomicStoreInt(ref shared.nextBlock, blockCount);
+                    break;
+                }
             }
         }
 
@@ -94,46 +103,81 @@ public static void b2ParallelFor(B2World world, b2ParallelForCallback callback,
             // No point enqueueing more tasks than blocks.
             int taskCount = b2MinInt(workerCount, blockCount);
 
-            B2ParallelForShared shared = new B2ParallelForShared
+            if (taskCount == 1)
             {
-                blockCount = blockCount,
-                blockSize = blockSize,
-                itemCount = itemCount,
-                callback = callback,
-                context = context,
-            };
-            b2AtomicStoreInt(ref shared.nextBlock, 0);
+                callback(0, itemCount, 0, context);
+                return;
+            }
 
-            Span<B2ParallelForTask> tasks = new B2ParallelForTask[B2_MAX_WORKERS];
-            object[] handles = new object[B2_MAX_WORKERS];
-            for (int i = 0; i < taskCount; ++i)
+            bool reuse = System.Threading.Interlocked.CompareExchange(ref world.parallelForInUse, 1, 0) == 0;
+            B2ParallelForShared shared = reuse
+                ? world.reusableParallelFor ?? (world.reusableParallelFor = new B2ParallelForShared())
+                : new B2ParallelForShared();
+            shared.blockCount = blockCount;
+            shared.blockSize = blockSize;
+            shared.itemCount = itemCount;
+            shared.callback = callback;
+            shared.context = context;
+            shared.failure = null;
+            b2AtomicStoreInt(ref shared.nextBlock, 0);
+            object[] handles = shared.handles;
+            int enqueuedCount = 0;
+            Exception workerFailure = null;
+            try
             {
-                tasks[i] = new B2ParallelForTask
+                for (int i = 0; i < taskCount; ++i)
                 {
-                    shared = shared,
-                    workerIndex = i,
-                };
-
-                if (world.taskCount < B2_MAX_TASKS)
-                {
-                    handles[i] = world.enqueueTaskFcn(b2ParallelForTrampoline, tasks[i], world.userTaskContext);
-                    world.taskCount += 1;
-                    world.activeTaskCount += handles[i] == null ? 0 : 1;
-                }
-                else
-                {
-                    handles[i] = null;
-                    b2ParallelForTrampoline(tasks[i]);
+                    B2ParallelForTask task = shared.tasks[i];
+                    if (world.taskCount < B2_MAX_TASKS)
+                    {
+                        handles[i] = world.enqueueTaskFcn(b2ParallelForTrampoline, task, world.userTaskContext);
+                        enqueuedCount = i + 1;
+                        world.taskCount += 1;
+                        world.activeTaskCount += handles[i] == null ? 0 : 1;
+                    }
+                    else
+                    {
+                        handles[i] = null;
+                        b2ParallelForTrampoline(task);
+                    }
                 }
             }
-
-            for (int i = 0; i < taskCount; ++i)
+            catch (Exception failure)
             {
-                if (handles[i] != null)
+                System.Threading.Interlocked.CompareExchange(ref shared.failure, failure, null);
+                b2AtomicStoreInt(ref shared.nextBlock, blockCount);
+            }
+            finally
+            {
+                for (int i = 0; i < enqueuedCount; ++i)
                 {
-                    world.finishTaskFcn(handles[i], world.userTaskContext);
-                    world.activeTaskCount -= 1;
+                    if (handles[i] != null)
+                    {
+                        try
+                        {
+                            world.finishTaskFcn(handles[i], world.userTaskContext);
+                        }
+                        catch (Exception failure)
+                        {
+                            System.Threading.Interlocked.CompareExchange(ref shared.failure, failure, null);
+                        }
+                        finally
+                        {
+                            world.activeTaskCount -= 1;
+                            handles[i] = null;
+                        }
+                    }
                 }
+                workerFailure = shared.failure;
+                shared.failure = null;
+                shared.callback = null;
+                shared.context = null;
+                if (reuse)
+                    System.Threading.Volatile.Write(ref world.parallelForInUse, 0);
+            }
+            if (workerFailure != null)
+            {
+                throw new InvalidOperationException("Parallel worker failed.", workerFailure);
             }
         }
     }

src/Box2D.NET/B2ParallelForShared.cs

@@ -8,6 +8,18 @@ namespace Box2D.NET
     // claim work, so a slow chunk can't strand the other threads.
     internal class B2ParallelForShared
     {
+        public readonly B2ParallelForTask[] tasks = new B2ParallelForTask[B2Constants.B2_MAX_WORKERS];
+        public readonly object[] handles = new object[B2Constants.B2_MAX_WORKERS];
+        public System.Exception failure;
+
+        public B2ParallelForShared()
+        {
+            for (int i = 0; i < tasks.Length; ++i)
+            {
+                tasks[i] = new B2ParallelForTask { shared = this, workerIndex = i };
+            }
+        }
+
         public B2AtomicInt nextBlock;
         public int blockCount;
         public int blockSize;

src/Box2D.NET/B2RevoluteJoints.cs

@@ -282,8 +282,8 @@ internal static void b2WarmStartRevoluteJoint(B2JointSim @base, B2StepContext co
             float iA = @base.invIA;
             float iB = @base.invIB;
 
-            // dummy state for static bodies
-            B2BodyState dummyState = B2BodyState.Create(b2_identityBodyState);
+            // Static-body branches only read this shared identity state.
+            B2BodyState dummyState = b2_identityBodyState;
 
             ref readonly B2RevoluteJoint joint = ref @base.uj.revoluteJoint;
             B2BodyState stateA = joint.indexA == B2_NULL_INDEX ? dummyState : context.states[joint.indexA];
@@ -316,8 +316,8 @@ internal static void b2SolveRevoluteJoint(B2JointSim @base, B2StepContext contex
             float iA = @base.invIA;
             float iB = @base.invIB;
 
-            // dummy state for static bodies
-            B2BodyState dummyState = B2BodyState.Create(b2_identityBodyState);
+            // Static-body branches only read this shared identity state.
+            B2BodyState dummyState = b2_identityBodyState;
 
             ref B2RevoluteJoint joint = ref @base.uj.revoluteJoint;
 

src/Box2D.NET/B2RuntimeValidator.cs

@@ -60,7 +60,7 @@ private void CheckSizeSeries()
         {
             ThrowIf(B2FixedArray8<int>.Size == B2_MAX_POLYGON_VERTICES, $"B2FixedArray8<int> and {nameof(B2_MAX_POLYGON_VERTICES)} have the same size, which is unexpected.");
             ThrowIf(B2FixedArray8<B2Vec2>.Size == B2_MAX_POLYGON_VERTICES, $"B2FixedArray8<B2Vec2> and {nameof(B2_MAX_POLYGON_VERTICES)} have the same size, which is unexpected.");
-            ThrowIf(B2FixedArray4<int>.Size == B2_SIMD_WIDTH, $"B2FixedArray4<int> and {nameof(B2_SIMD_WIDTH)}have the same size, which is unexpected.");
+            ThrowIf(B2FixedArray8<int>.Size == B2_SIMD_WIDTH, $"B2FixedArray8<int> and {nameof(B2_SIMD_WIDTH)}have the same size, which is unexpected.");
             ThrowIf(B2FixedArray8<float>.Size == B2_MAX_CONTINUOUS_SENSOR_HITS, $"B2FixedArray8<float> and {nameof(B2_MAX_CONTINUOUS_SENSOR_HITS)}have the same size, which is unexpected.");
         }
 

src/Box2D.NET/B2SolverSets.cs

@@ -27,16 +27,21 @@ public static B2SolverSet b2CreateSolverSet(B2World world)
             return set;
         }
 
-        public static void b2DestroySolverSet(B2World world, int setIndex)
+        public static void b2DestroySolverSet(B2World world, int setIndex) => b2DestroySolverSet(world, setIndex, false);
+
+        internal static void b2DestroySolverSet(B2World world, int setIndex, bool releaseStorage)
         {
             B2SolverSet set = b2Array_Get(ref world.solverSets, setIndex);
-            b2Array_Destroy(ref set.bodySims);
-            b2Array_Destroy(ref set.bodyStates);
-            b2Array_Destroy(ref set.contactSims);
-            b2Array_Destroy(ref set.jointSims);
-            b2Array_Destroy(ref set.islandSims);
-            b2FreeId(world.solverSetIdPool, setIndex);
-            //*set = ( b2SolverSet ){ 0 };
+            if (releaseStorage)
+            {
+                b2Array_Destroy(ref set.bodySims);
+                b2Array_Destroy(ref set.bodyStates);
+                b2Array_Destroy(ref set.contactSims);
+                b2Array_Destroy(ref set.jointSims);
+                b2Array_Destroy(ref set.islandSims);
+            }
+            if (set.setIndex != B2_NULL_INDEX)
+                b2FreeId(world.solverSetIdPool, setIndex);
             set.Clear();
             set.setIndex = B2_NULL_INDEX;
         }
@@ -205,9 +210,9 @@ internal static void b2TrySleepIsland(B2World world, int islandId)
             B2_ASSERT(0 <= island.localIndex && island.localIndex < awakeSet.islandSims.count);
 
             sleepSet.setIndex = sleepSetId;
-            sleepSet.bodySims = b2Array_Create<B2BodySim>(island.bodies.count);
-            sleepSet.contactSims = b2Array_Create<B2ContactSim>(island.contacts.count);
-            sleepSet.jointSims = b2Array_Create<B2JointSim>(island.joints.count);
+            b2Array_Reserve(ref sleepSet.bodySims, island.bodies.count);
+            b2Array_Reserve(ref sleepSet.contactSims, island.contacts.count);
+            b2Array_Reserve(ref sleepSet.jointSims, island.joints.count);
 
             // move awake bodies to sleeping set
             // this shuffles around bodies in the awake set