Concurrency optimizations

src/Jitter2/Collision/DynamicTree.cs

@@ -74,7 +74,7 @@ public interface IRayCastable
 /// </summary>
 public partial class DynamicTree
 {
-    private volatile SlimBag<IDynamicTreeProxy>[] lists = Array.Empty<SlimBag<IDynamicTreeProxy>>();
+    private readonly SlimBag<IDynamicTreeProxy> movedProxies = new();
 
     private readonly ActiveList<IDynamicTreeProxy> proxies = new();
 
@@ -134,7 +134,6 @@ public struct Node
     /// </summary>
     public int Root => root;
 
-
     public Func<IDynamicTreeProxy, IDynamicTreeProxy, bool> Filter { get; set; }
 
     /// <summary>
@@ -144,7 +143,7 @@ public struct Node
     public DynamicTree(Func<IDynamicTreeProxy, IDynamicTreeProxy, bool> filter)
     {
         scanMoved = ScanForMovedProxies;
-        scanOverlaps = batch => { ScanForOverlaps(batch.BatchIndex); };
+        scanOverlaps = ScanForOverlaps;
         updateBoundingBoxes = UpdateBoundingBoxesCallback;
 
         Filter = filter;
@@ -161,12 +160,10 @@ public enum Timings
 
     public readonly double[] DebugTimings = new double[(int)Timings.Last];
 
-    private int updatedProxies;
-
     /// <summary>
     /// Gets the number of updated proxies.
     /// </summary>
-    public int UpdatedProxies => updatedProxies;
+    public int UpdatedProxies => movedProxies.Count;
 
     /// <summary>
     /// Updates all entities that are marked as active in the active list.
@@ -187,38 +184,25 @@ void SetTime(Timings type)
 
         this.step_dt = dt;
 
-        CheckBagCount(multiThread);
-
         SetTime(Timings.UpdateBoundingBoxes);
 
         if (multiThread)
         {
             proxies.ParallelForBatch(256, updateBoundingBoxes);
             SetTime(Timings.UpdateBoundingBoxes);
 
-            const int taskThreshold = 24;
-            int numTasks = Math.Clamp(proxies.ActiveCount / taskThreshold, 1, ThreadPool.Instance.ThreadCount);
-            Parallel.ForBatch(0, proxies.ActiveCount, numTasks, scanMoved);
-
+            movedProxies.Clear();
+            proxies.ParallelForBatch(24, scanMoved);
             SetTime(Timings.ScanMoved);
 
-            updatedProxies = 0;
-
-            for (int ntask = 0; ntask < numTasks; ntask++)
+            for (int i = 0; i < movedProxies.Count; i++)
             {
-                var sl = lists[ntask];
-                updatedProxies += sl.Count;
-
-                for (int i = 0; i < sl.Count; i++)
-                {
-                    var proxy = sl[i];
-                    InternalAddRemoveProxy(proxy);
-                }
+                InternalAddRemoveProxy(movedProxies[i]);
             }
 
             SetTime(Timings.UpdateProxies);
 
-            Parallel.ForBatch(0, proxies.ActiveCount, numTasks, scanOverlaps);
+            movedProxies.ParallelForBatch(24, scanOverlaps);
 
             SetTime(Timings.ScanOverlaps);
         }
@@ -228,22 +212,23 @@ void SetTime(Timings type)
             UpdateBoundingBoxesCallback(batch);
             SetTime(Timings.UpdateBoundingBoxes);
 
+            movedProxies.Clear();
             scanMoved(batch);
             SetTime(Timings.ScanMoved);
 
-            var sl = lists[0];
-            for (int i = 0; i < sl.Count; i++)
+            for (int i = 0; i < movedProxies.Count; i++)
             {
-                IDynamicTreeProxy proxy = sl[i];
-                InternalAddRemoveProxy(proxy);
+                InternalAddRemoveProxy(movedProxies[i]);
             }
 
             SetTime(Timings.UpdateProxies);
 
-            scanOverlaps(new Parallel.Batch(0, proxies.ActiveCount));
+            scanOverlaps(new Parallel.Batch(0, movedProxies.Count));
 
             SetTime(Timings.ScanOverlaps);
         }
+
+        movedProxies.TrackAndNullOutOne();
     }
 
     private Real step_dt;
@@ -522,19 +507,6 @@ private void FreeNode(int node)
         freeNodes.Push(node);
     }
 
-    private void CheckBagCount(bool multiThread)
-    {
-        int numThreads = multiThread ? ThreadPool.Instance.ThreadCount : 1;
-        if (lists.Length != numThreads)
-        {
-            lists = new SlimBag<IDynamicTreeProxy>[numThreads];
-            for (int i = 0; i < numThreads; i++)
-            {
-                lists[i] = new SlimBag<IDynamicTreeProxy>();
-            }
-        }
-    }
-
     private double Cost(ref Node node)
     {
         if (node.IsLeaf)
@@ -590,9 +562,6 @@ private void OverlapCheckRemove(int index, int node)
 
     private void ScanForMovedProxies(Parallel.Batch batch)
     {
-        var list = lists[batch.BatchIndex];
-        list.Clear();
-
         for (int i = batch.Start; i < batch.End; i++)
         {
             var proxy = proxies[i];
@@ -602,7 +571,7 @@ private void ScanForMovedProxies(Parallel.Batch batch)
             if (node.ForceUpdate || !node.ExpandedBox.Encompasses(proxy.WorldBoundingBox))
             {
                 node.ForceUpdate = false;
-                list.Add(proxy);
+                movedProxies.ConcurrentAdd(proxy);
             }
 
             // else proxy is well contained within the nodes expanded Box:
@@ -611,15 +580,13 @@ private void ScanForMovedProxies(Parallel.Batch batch)
         // Make sure we do not hold too many dangling references
         // in the internal array of the SlimBag<T> data structure which might
         // prevent GC. But do only free them one-by-one to prevent overhead.
-        list.NullOutOne();
     }
 
-    private void ScanForOverlaps(int fraction)
+    private void ScanForOverlaps(Parallel.Batch batch)
     {
-        var sl = lists[fraction];
-        for (int i = 0; i < sl.Count; i++)
+        for (int i = batch.Start; i < batch.End; i++)
         {
-            OverlapCheckAdd(root, sl[i].NodePtr);
+            OverlapCheckAdd(root, movedProxies[i].NodePtr);
         }
     }
 

src/Jitter2/Collision/PairHashSet.cs

@@ -31,14 +31,9 @@
 namespace Jitter2.Collision;
 
 /// <summary>
-/// A hash set implementation optimized for thread-safe additions of (int,int)-pairs.
+/// A hash set implementation which stores pairs of (int, int) values.
+/// The implementation is based on open addressing.
 /// </summary>
-/// <remarks>
-/// - The <see cref="Add(Pair)"/> method is thread-safe and can be called concurrently
-///   from multiple threads without additional synchronization.
-/// - Other operations, such as <see cref="Remove(Pair)"/> or enumeration, are NOT thread-safe
-///   and require external synchronization if used concurrently.
-/// </remarks>
 public unsafe class PairHashSet : IEnumerable<PairHashSet.Pair>
 {
     [StructLayout(LayoutKind.Explicit, Size = 8)]
@@ -106,8 +101,8 @@ public void Reset()
         }
     }
 
-    public volatile Pair[] Slots = Array.Empty<Pair>();
-    private volatile int count;
+    public Pair[] Slots = Array.Empty<Pair>();
+    private int count;
 
     // 16384*8/1024 KB = 128 KB
     public const int MinimumSize = 16384;
@@ -150,12 +145,11 @@ private void Resize(int size)
             if (pair.ID != 0)
             {
                 int hash = pair.GetHash();
-                int hash_i = FindSlot(newSlots, hash, pair.ID);
-                newSlots[hash_i] = pair;
+                int hashIndex = FindSlot(newSlots, hash, pair.ID);
+                newSlots[hashIndex] = pair;
             }
         }
 
-        Interlocked.MemoryBarrier();
         Slots = newSlots;
     }
 
@@ -175,11 +169,11 @@ private int FindSlot(Pair[] slots, int hash, long id)
     public bool Add(Pair pair)
     {
         int hash = pair.GetHash();
-        int hash_i = FindSlot(Slots, hash, pair.ID);
+        int hashIndex = FindSlot(Slots, hash, pair.ID);
 
-        if (Slots[hash_i].ID == 0)
+        if (Slots[hashIndex].ID == 0)
         {
-            Slots[hash_i] = pair;
+            Slots[hashIndex] = pair;
             Interlocked.Increment(ref count);
 
             if (Slots.Length < 2 * count)
@@ -195,49 +189,52 @@ public bool Add(Pair pair)
 
     private Jitter2.Parallelization.ReaderWriterLock rwLock;
 
-    internal void ConcurrentAdd(Pair pair)
+    internal bool ConcurrentAdd(Pair pair)
     {
-        // TODO: implement a better lock-free version
-
         int hash = pair.GetHash();
 
+        // Fast path: This is a *huge* optimization for the case of frequent additions
+        // of already existing entries. Entirely bypassing any locks or synchronization.
+        int fpHashIndex = FindSlot(Slots, hash, pair.ID);
+        if (Slots[fpHashIndex].ID != 0) return false;
+
         rwLock.EnterReadLock();
 
         fixed (Pair* slotsPtr = Slots)
         {
             while (true)
             {
-                int hash_i = FindSlot(Slots, hash, pair.ID);
-
-                Pair* slotPtr = &slotsPtr[hash_i];
+                var hashIndex = FindSlot(Slots, hash, pair.ID);
+                var slotPtr = &slotsPtr[hashIndex];
 
                 if (slotPtr->ID == pair.ID)
                 {
                     rwLock.ExitReadLock();
-                    return;
+                    return false;
                 }
 
-                if (Interlocked.CompareExchange(ref *(long*)slotPtr,
-                        *(long*)&pair, 0) == 0)
+                if (Interlocked.CompareExchange(ref *(long*)slotPtr, pair.ID, 0) != 0)
                 {
-                    Interlocked.Increment(ref count);
+                    continue;
+                }
 
-                    rwLock.ExitReadLock();
+                Interlocked.Increment(ref count);
+                rwLock.ExitReadLock();
+
+                if (Slots.Length < 2 * count)
+                {
+                    rwLock.EnterWriteLock();
 
+                    // check if another thread already performed a resize.
                     if (Slots.Length < 2 * count)
                     {
-                        rwLock.EnterWriteLock();
-                        // check if another thread already did the work for us
-                        if (Slots.Length < 2 * count)
-                        {
-                            Resize(PickSize(Slots.Length * 2));
-                        }
-                        rwLock.ExitWriteLock();
+                        Resize(PickSize(Slots.Length * 2));
                     }
 
-                    return;
+                    rwLock.ExitWriteLock();
                 }
 
+                return true;
             } // while
         } // fixed
     }
@@ -287,8 +284,8 @@ public bool Remove(int slot)
     public bool Remove(Pair pair)
     {
         int hash = pair.GetHash();
-        int hash_i = FindSlot(Slots, hash, pair.ID);
-        return Remove(hash_i);
+        int hashIndex = FindSlot(Slots, hash, pair.ID);
+        return Remove(hashIndex);
     }
 
     public IEnumerator<Pair> GetEnumerator()

src/Jitter2/DataStructures/SlimBag.cs

@@ -24,6 +24,7 @@
 using System;
 using System.Collections.Generic;
 using System.Diagnostics;
+using System.Threading;
 
 namespace Jitter2.DataStructures;
 
@@ -86,7 +87,40 @@ public void Add(T item)
         }
 
         array[counter++] = item;
-        nullOut = counter;
+    }
+
+    private Jitter2.Parallelization.ReaderWriterLock rwLock;
+
+    /// <summary>
+    /// Adds an element to the <see cref="SlimBag{T}"/>.
+    /// </summary>
+    /// <param name="item">The element to add.</param>
+    public void ConcurrentAdd(T item)
+    {
+        int lc = Interlocked.Increment(ref counter) - 1;
+
+        again:
+
+        rwLock.EnterReadLock();
+
+        if (lc < array.Length)
+        {
+            array[lc] = item;
+            rwLock.ExitReadLock();
+        }
+        else
+        {
+            rwLock.ExitReadLock();
+
+            rwLock.EnterWriteLock();
+            if (lc >= array.Length)
+            {
+                Array.Resize(ref array, array.Length * 2);
+            }
+            rwLock.ExitWriteLock();
+
+            goto again;
+        }
     }
 
     /// <summary>
@@ -147,19 +181,14 @@ public void Clear()
     }
 
     /// <summary>
-    /// Sets unused positions in the internal array to their default values.
-    /// </summary>
-    public void NullOut()
-    {
-        Array.Clear(array, counter, nullOut - counter);
-        nullOut = counter;
-    }
-
-    /// <summary>
-    /// Null out a single position in the internal array.
+    /// This should be called after adding entries to the SlimBag in order
+    /// to keep track of the largest index used within the internal array of
+    /// this datastructure. It will set this item in the array to its default value
+    /// to allow for garbage collection.
     /// </summary>
-    public void NullOutOne()
+    public void TrackAndNullOutOne()
     {
+        nullOut = Math.Max(nullOut, counter);
         if (nullOut <= counter) return;
         array[--nullOut] = default!;
     }