Optimize DynamicTree

src/Jitter2/Collision/DynamicTree.cs

@@ -22,6 +22,7 @@
  */
 
 using System;
+using System.Collections.Concurrent;
 using System.Collections.Generic;
 using System.Diagnostics;
 using Jitter2.DataStructures;
@@ -146,7 +147,7 @@ public DynamicTree(Func<IDynamicTreeProxy, IDynamicTreeProxy, bool> filter)
         scanOverlapsPre = batch =>
         {
             ScanForMovedProxies(batch);
-            ScanForOverlaps(batch.BatchIndex, false);
+            //ScanForOverlaps(batch.BatchIndex, false);
         };
 
         scanOverlapsPost = batch => { ScanForOverlaps(batch.BatchIndex, true); };
@@ -220,8 +221,7 @@ void SetTime(Timings type)
                 for (int i = 0; i < sl.Count; i++)
                 {
                     var proxy = sl[i];
-                    InternalRemoveProxy(proxy);
-                    InternalAddProxy(proxy);
+                    InternalAddRemoveProxy(proxy);
                 }
             }
 
@@ -245,8 +245,7 @@ void SetTime(Timings type)
             for (int i = 0; i < sl.Count; i++)
             {
                 IDynamicTreeProxy proxy = sl[i];
-                InternalRemoveProxy(proxy);
-                InternalAddProxy(proxy);
+                InternalAddRemoveProxy(proxy);
             }
 
             SetTime(Timings.UpdateProxies);
@@ -354,11 +353,12 @@ private void EnumerateAABB(ref Node node, Action<JBBox, int> action, int depth =
     private uint stepper;
 
     /// <summary>
-    /// Removes entries from <see cref="PotentialPairs"/> which are both marked as inactive.
+    /// Removes entries from <see cref="PotentialPairs"/> which are both marked as inactive or
+    /// whose expanded bounding box do not overlap any longer.
     /// Only searches a small subset of the pair hashset (1/128) per call to reduce
     /// overhead.
     /// </summary>
-    public void TrimInactivePairs()
+    public void TrimInvalidPairs()
     {
         // We actually only search 1/128 of the whole potentialPairs Hashset for
         // potentially prunable contacts. No need to sweep through the whole hashset
@@ -376,7 +376,12 @@ public void TrimInactivePairs()
             var proxyA = Nodes[n.ID1].Proxy;
             var proxyB = Nodes[n.ID2].Proxy;
 
-            if (IsActive(proxyA) || IsActive(proxyB)) continue;
+            if (proxyA != null && proxyB != null &&
+                Nodes[proxyA.NodePtr].ExpandedBox.NotDisjoint(Nodes[proxyB.NodePtr].ExpandedBox) &&
+                    (IsActive(proxyA) || IsActive(proxyB)))
+            {
+                continue;
+            }
 
             PotentialPairs.Remove(t);
             i -= 1;
@@ -593,8 +598,7 @@ private void ScanForMovedProxies(Parallel.Batch batch)
 
             ref var node = ref Nodes[proxy.NodePtr];
 
-            if (node.ForceUpdate || node.ExpandedBox.Contains(proxy.WorldBoundingBox) !=
-                JBBox.ContainmentType.Contains)
+            if (node.ForceUpdate || !node.ExpandedBox.Encompasses(proxy.WorldBoundingBox))
             {
                 node.ForceUpdate = false;
                 list.Add(proxy);
@@ -651,6 +655,27 @@ private static Real GenerateRandom(ulong seed)
         return MathR.Abs((Real)randomBits / uint.MaxValue);
     }
 
+    private void InternalAddRemoveProxy(IDynamicTreeProxy proxy)
+    {
+        JBBox box = proxy.WorldBoundingBox;
+
+        int parent = RemoveLeaf(proxy.NodePtr);
+
+        int index = proxy.NodePtr;
+
+        Real pseudoRandomExt = GenerateRandom((ulong)index);
+
+        ExpandBoundingBox(ref box, proxy.Velocity * ExpandFactor * ((Real)1.0 + pseudoRandomExt));
+
+        Nodes[index].Proxy = proxy;
+        Nodes[index].Height = 1;
+        proxy.NodePtr = index;
+
+        Nodes[index].ExpandedBox = box;
+
+        InsertLeaf(index, parent);
+    }
+
     private void InternalAddProxy(IDynamicTreeProxy proxy)
     {
         JBBox box = proxy.WorldBoundingBox;
@@ -666,23 +691,24 @@ private void InternalAddProxy(IDynamicTreeProxy proxy)
 
         Nodes[index].ExpandedBox = box;
 
-        AddLeaf(index);
+        InsertLeaf(index, root);
     }
 
-    private void InternalRemoveProxy(IDynamicTreeProxy proxy)
+    private int InternalRemoveProxy(IDynamicTreeProxy proxy)
     {
         Debug.Assert(Nodes[proxy.NodePtr].IsLeaf);
 
-        RemoveLeaf(proxy.NodePtr);
+        int result = RemoveLeaf(proxy.NodePtr);
         FreeNode(proxy.NodePtr);
+        return result;
     }
 
-    private void RemoveLeaf(int node)
+    private int RemoveLeaf(int node)
     {
         if (node == root)
         {
             root = NullNode;
-            return;
+            return NullNode;
         }
 
         int parent = Nodes[node].Parent;
@@ -710,12 +736,16 @@ private void RemoveLeaf(int node)
                 Nodes[index].Height = 1 + Math.Max(Nodes[left].Height, Nodes[rght].Height);
                 index = Nodes[index].Parent;
             }
+
+            return grandParent;
         }
         else
         {
             root = sibling;
             Nodes[sibling].Parent = NullNode;
             FreeNode(parent);
+
+            return root;
         }
     }
 
@@ -742,7 +772,7 @@ private static double MergedSurface(in JBBox box1, in JBBox box2)
         return 2.0d * (x * y + x * z + z * y);
     }
 
-    private void AddLeaf(int node)
+    private void InsertLeaf(int node, int where)
     {
         if (root == NullNode)
         {
@@ -751,11 +781,22 @@ private void AddLeaf(int node)
             return;
         }
 
-        // search for the best sibling
-        // int sibling = root;
         JBBox nodeBox = Nodes[node].ExpandedBox;
+
+        while (where != root)
+        {
+            if (Nodes[where].ExpandedBox.Encompasses(nodeBox))
+            {
+                break;
+            }
 
-        int sibling = root;
+            where = Nodes[where].Parent;
+        }
+
+        int insertionParent = Nodes[where].Parent;
+
+        // search for the best sibling
+        int sibling = where;
 
         while (!Nodes[sibling].IsLeaf)
         {
@@ -828,13 +869,14 @@ private void AddLeaf(int node)
         }
 
         int index = Nodes[node].Parent;
-        while (index != NullNode)
+        while (index != insertionParent)
         {
             int lft = Nodes[index].Left;
             int rgt = Nodes[index].Right;
 
             JBBox.CreateMerged(Nodes[lft].ExpandedBox, Nodes[rgt].ExpandedBox, out Nodes[index].ExpandedBox);
             Nodes[index].Height = 1 + Math.Max(Nodes[lft].Height, Nodes[rgt].Height);
+
             index = Nodes[index].Parent;
         }
     }

src/Jitter2/DataStructures/ActiveList.cs

@@ -25,6 +25,7 @@
 using System.Collections;
 using System.Collections.Generic;
 using System.Diagnostics;
+using System.Runtime.CompilerServices;
 
 namespace Jitter2.DataStructures;
 
@@ -163,6 +164,7 @@ private void Swap(int index0, int index1)
         elements[index1].ListIndex = index1;
     }
 
+    [MethodImpl(MethodImplOptions.AggressiveInlining)]
     public bool IsActive(T element)
     {
         Debug.Assert(element.ListIndex != -1);

src/Jitter2/LinearMath/JBBox.cs

@@ -233,6 +233,13 @@ public readonly bool Disjoint(in JBBox box)
                  Max.Z >= box.Min.Z && Min.Z <= box.Max.Z);
     }
 
+    public readonly bool Encompasses(in JBBox box)
+    {
+        return Min.X <= box.Min.X && Max.X >= box.Max.X &&
+               Min.Y <= box.Min.Y && Max.Y >= box.Max.Y &&
+               Min.Z <= box.Min.Z && Max.Z >= box.Max.Z;
+    }
+
     public static JBBox CreateMerged(in JBBox original, in JBBox additional)
     {
         CreateMerged(original, additional, out JBBox result);

src/Jitter2/World.Step.cs

@@ -27,7 +27,6 @@
 using System.Runtime.CompilerServices;
 using System.Threading;
 using Jitter2.Collision;
-using Jitter2.Collision.Shapes;
 using Jitter2.DataStructures;
 using Jitter2.Dynamics;
 using Jitter2.Dynamics.Constraints;
@@ -155,9 +154,8 @@ void SetTime(Timings type)
         // Add the new arbiters to their respective rigid body.
 
         // Go through potential pairs in the collision system and remove
-        // pairs which are inactive. This speeds up the enumeration of all
-        // collisions of interest.
-        DynamicTree.TrimInactivePairs();
+        // pairs which are invalid (i.e. not overlapping or inactive).
+        DynamicTree.TrimInvalidPairs();
 
         SetTime(Timings.TrimPotentialPairs);
 
@@ -412,6 +410,9 @@ private void DetectCollisionsCallback(Parallel.Batch batch)
             var proxyA = DynamicTree.Nodes[node.ID1].Proxy;
             var proxyB = DynamicTree.Nodes[node.ID2].Proxy;
 
+            if(proxyA == null || proxyB == null) continue;
+            if(!DynamicTree.Filter(proxyA, proxyB)) continue;
+
             if (!proxyA.WorldBoundingBox.Disjoint(proxyB.WorldBoundingBox))
             {
                 Detect(proxyA, proxyB);