Fix DynamicTree Clear (space-wizards#6651)

Author: metalgearsloth

RELEASE-NOTES.md

@@ -43,6 +43,7 @@ END TEMPLATE-->
 
 ### Bugfixes
 
+* Fix DynamicTree.Clear not removing node references.
 * Reverted validation for `UiBox2i` `ctor`s as it was causing regressions in debug UIs.
 
 ### Other

Robust.Shared.Tests/Physics/B2DynamicTree_Test.cs

@@ -66,5 +66,39 @@ public void AddAndQuery()
                 }
             });
         }
+
+        [Test]
+        public void RebuildFullPreservesQueries()
+        {
+            var dt = new B2DynamicTree<int>();
+
+            for (var i = 0; i < aabbs1.Length; ++i)
+            {
+                dt.CreateProxy(aabbs1[i], uint.MaxValue, i);
+            }
+
+            dt.Rebuild(true);
+
+            var point = new Vector2(0, 0);
+            var box = Box2.CenteredAround(point, new Vector2(0.1f, 0.1f));
+            var results = new HashSet<int>();
+
+            dt.Query(proxy =>
+            {
+                results.Add(dt.GetUserData(proxy));
+                return true;
+            }, box);
+
+            Assert.Multiple(() =>
+            {
+                for (var i = 0; i < aabbs1.Length; i++)
+                {
+                    if (aabbs1[i].Intersects(box))
+                    {
+                        Assert.That(results, Does.Contain(i));
+                    }
+                }
+            });
+        }
     }
 }

Robust.Shared.Tests/Physics/DynamicTree_Test.cs

@@ -60,11 +60,12 @@ internal sealed class DynamicTree_Test
         [Test]
         public void AddAndGrow()
         {
-            var dt = new DynamicTree<int>((in int x) => aabbs1[x], capacity: 16, growthFunc: x => x += 2);
+            const int proxyCapacity = 16;
+            var dt = new DynamicTree<int>((in int x) => aabbs1[x], capacity: proxyCapacity, growthFunc: x => x += 2);
 
             var initCap = dt.Capacity;
 
-            Assert.That(initCap, Is.EqualTo(16));
+            Assert.That(initCap, Is.EqualTo(2 * proxyCapacity - 1));
 
             Assert.Multiple(() =>
             {
@@ -194,6 +195,38 @@ public void AddThenRemove()
             });
         }
 
+        [Test]
+        public void ClearRemovesAllEntries()
+        {
+            var aabbs = new[]
+            {
+                ((Box2) default).Enlarged(1),
+                new Box2(float.NaN, float.NaN, float.NaN, float.NaN),
+            };
+
+            var dt = new DynamicTree<int>((in int x) => aabbs[x], capacity: 16, growthFunc: x => x += 2);
+
+            Assert.That(dt.Add(0), Is.True);
+            Assert.That(dt.Add(1), Is.True);
+            Assert.That(dt.Count, Is.EqualTo(2));
+
+            dt.Clear();
+
+            Assert.Multiple(() =>
+            {
+                Assert.That(dt.Count, Is.Zero);
+                Assert.That(dt.Contains(0), Is.False);
+                Assert.That(dt.Contains(1), Is.False);
+                Assert.That(dt, Is.Empty);
+            });
+
+            Assert.Multiple(() =>
+            {
+                Assert.That(dt.Add(0), Is.True);
+                Assert.That(dt.Add(1), Is.True);
+            });
+        }
+
         [Test]
         public void AddAndQuery() {
             var dt = new DynamicTree<int>((in int x) => aabbs1[x], capacity: 16, growthFunc: x => x += 2);

Robust.Shared/Physics/B2DynamicTree.cs

@@ -251,7 +251,8 @@ public B2DynamicTree(float aabbExtendSize = 1f / 32, int capacity = 256, Func<in
             capacity = Math.Max(MinimumCapacity, capacity);
 
             _root = Proxy.Free;
-            _nodes = new Node[capacity];
+            // Maximum node count for a full binary tree is 2 * leafCount - 1.
+            _nodes = new Node[2 * capacity - 1];
 
             // Build a linked list for the free list.
             ref var node = ref _nodes[0];
@@ -294,6 +295,10 @@ private ref Node AllocateNode(out Proxy proxy)
             _freeList = allocNode.Next;
             Assert(_freeList == -1 || _nodes[_freeList].IsFree);
             allocNode = default;
+            allocNode.Parent = Proxy.Free;
+            allocNode.Next = Proxy.Free;
+            allocNode.Child1 = Proxy.Free;
+            allocNode.Child2 = Proxy.Free;
             ++NodeCount;
             proxy = alloc;
             return ref allocNode;
@@ -1897,7 +1902,7 @@ public int Rebuild(bool fullBuild)
 
 	        var nodeIndex = _root;
             ref var baseRef = ref _nodes[0];
-	        var node = baseRef;
+	        ref var node = ref Unsafe.Add(ref baseRef, nodeIndex);
 
 	        // These are the nodes that get sorted to rebuild the tree.
 	        // I'm using indices because the node pool may grow during the build.
@@ -1942,7 +1947,7 @@ public int Rebuild(bool fullBuild)
                         stack.Push(node.Child2);
 			        }
 
-                    node = Unsafe.Add(ref baseRef, nodeIndex);
+                    node = ref Unsafe.Add(ref baseRef, nodeIndex);
 
 			        // Remove doomed node
 			        FreeNode(doomedNodeIndex);
@@ -1956,7 +1961,7 @@ public int Rebuild(bool fullBuild)
 		        }
 
                 nodeIndex = stack.Pop();
-		        node = Unsafe.Add(ref baseRef, nodeIndex);
+		        node = ref Unsafe.Add(ref baseRef, nodeIndex);
 	        }
 
         #if B2_VALIDATE
@@ -2068,6 +2073,9 @@ internal void RayCastNew(RayCastInput input, long mask, ref WorldRayCastContext
             var p1 = input.Origin;
             var d = input.Translation;
 
+            if (d.LengthSquared() < float.Epsilon)
+                return;
+
             var r = d.Normalized();
 
 	        // v is perpendicular to the segment.
@@ -2084,7 +2092,7 @@ internal void RayCastNew(RayCastInput input, long mask, ref WorldRayCastContext
 	        // Build a bounding box for the segment.
 	        var segmentAABB = new Box2(Vector2.Min(p1, p2), Vector2.Max(p1, p2));
 
-	        var stack = new GrowableStack<Proxy>(stackalloc Proxy[256]);
+	        var stack = new GrowableStack<Proxy>(stackalloc Proxy[TreeStackSize]);
             ref var baseRef = ref _nodes[0];
 	        stack.Push(_root);
 
@@ -2141,15 +2149,10 @@ internal void RayCastNew(RayCastInput input, long mask, ref WorldRayCastContext
 		        }
 		        else
                 {
-                    var stackCount = stack.GetCount();
-			        Assert( stackCount < 256 - 1 );
-			        if (stackCount < 256 - 1 )
-			        {
-				        // TODO_ERIN just put one node on the stack, continue on a child node
-				        // TODO_ERIN test ordering children by nearest to ray origin
-				        stack.Push(node.Child1);
-				        stack.Push(node.Child2);
-			        }
+			        // TODO_ERIN just put one node on the stack, continue on a child node
+			        // TODO_ERIN test ordering children by nearest to ray origin
+			        stack.Push(node.Child1);
+			        stack.Push(node.Child2);
 		        }
 	        }
         }
@@ -2205,7 +2208,7 @@ internal void ShapeCast(ShapeCastInput input, long maskBits, TreeShapeCastCallba
 	        var subInput = input;
 
             ref var baseRef = ref _nodes[0];
-            var stack = new GrowableStack<Proxy>(stackalloc Proxy[256]);
+            var stack = new GrowableStack<Proxy>(stackalloc Proxy[TreeStackSize]);
 	        stack.Push(_root);
 
 	        while (stack.GetCount() > 0)
@@ -2258,16 +2261,10 @@ internal void ShapeCast(ShapeCastInput input, long maskBits, TreeShapeCastCallba
 		        }
 		        else
 		        {
-                    var stackCount = stack.GetCount();
-			        Assert(stackCount < 256 - 1);
-
-			        if (stackCount < 255)
-			        {
-				        // TODO_ERIN just put one node on the stack, continue on a child node
-				        // TODO_ERIN test ordering children by nearest to ray origin
-				        stack.Push(node.Child1);
-				        stack.Push(node.Child2);
-			        }
+			        // TODO_ERIN just put one node on the stack, continue on a child node
+			        // TODO_ERIN test ordering children by nearest to ray origin
+			        stack.Push(node.Child1);
+			        stack.Push(node.Child2);
 		        }
 	        }
         }

Robust.Shared/Physics/DynamicTree.cs

@@ -103,8 +103,11 @@ public void Clear()
         {
             foreach (var proxy in _nodeLookup.Values)
             {
-                _b2Tree.DestroyProxy(proxy);
+                if (proxy != DynamicTree.Proxy.Free)
+                    _b2Tree.DestroyProxy(proxy);
             }
+
+            _nodeLookup.Clear();
         }
 
         public bool Contains(T item)