From e52b2560091a8ee377673854d9841be44c0ed4a8 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Thu, 7 Dec 2023 20:04:36 -0600 Subject: [PATCH] Revert. --- .../CollidableOverlapFinder.cs | 75 +++-- BepuPhysics/Trees/Tree_IntertreeQueriesMT2.cs | 272 ------------------ BepuPhysics/Trees/Tree_SelfQueriesMT.cs | 2 +- BepuPhysics/Trees/Tree_SelfQueriesMT2.cs | 229 --------------- 4 files changed, 35 insertions(+), 543 deletions(-) delete mode 100644 BepuPhysics/Trees/Tree_IntertreeQueriesMT2.cs delete mode 100644 BepuPhysics/Trees/Tree_SelfQueriesMT2.cs diff --git a/BepuPhysics/CollisionDetection/CollidableOverlapFinder.cs b/BepuPhysics/CollisionDetection/CollidableOverlapFinder.cs index 6aff5993..bed536c4 100644 --- a/BepuPhysics/CollisionDetection/CollidableOverlapFinder.cs +++ b/BepuPhysics/CollisionDetection/CollidableOverlapFinder.cs @@ -84,8 +84,8 @@ public CollidableOverlapFinder(NarrowPhase narrowPhase, BroadPhase b intertreeTestContext2 = new Tree.MultithreadedIntertreeTest(); //VERSION 3 - selfTestContext3 = new Tree.MultithreadedSelfTest2(); - intertreeTestContext3 = new Tree.MultithreadedIntertreeTest2(); + selfTestContext3 = new Tree.MultithreadedSelfTest(); + intertreeTestContext3 = new Tree.MultithreadedIntertreeTest(); } void Worker(int workerIndex) @@ -191,7 +191,7 @@ public override void DispatchOverlaps(float dt, IThreadDispatcher threadDispatch Debug.Assert(!narrowPhase.overlapWorkers[i].Batcher.batches.Allocated, "After execution, there should be no remaining allocated collision batchers."); } #endif - selfTestContext.CompleteSelfTest(); + selfTestContext.CompleteTest(); intertreeTestContext.CompleteTest(); } else @@ -535,7 +535,7 @@ public void DispatchOverlaps2(float dt, IThreadDispatcher threadDispatcher = nul Debug.Assert(!narrowPhase.overlapWorkers[i].Batcher.batches.Allocated, "After execution, there should be no remaining allocated collision batchers."); } #endif - selfTestContext2.CompleteSelfTest(); + selfTestContext2.CompleteTest(); intertreeTestContext2.CompleteTest(); } else @@ -562,59 +562,52 @@ public void DispatchOverlaps2(float dt, IThreadDispatcher threadDispatcher = nul unsafe static void PrepareSelfTestJob3(long id, void* context, int workerIndex, IThreadDispatcher threadDispatcher) { var overlapFinder = (CollidableOverlapFinder)threadDispatcher.ManagedContext; - //overlapFinder.selfTestContext3.PrepareJobs(ref overlapFinder.broadPhase.ActiveTree, overlapFinder.selfTestHandlers3, threadDispatcher.ThreadCount, workerIndex, threadDispatcher.WorkerPools[workerIndex]); - overlapFinder.selfTestContext3.PushRootTasks(ref overlapFinder.broadPhase.ActiveTree, overlapFinder.selfTestHandlers3, threadDispatcher.ThreadCount, workerIndex, threadDispatcher, threadDispatcher.WorkerPools[workerIndex], overlapFinder.broadStack, &SelfTestJob3); + overlapFinder.selfTestContext3.PrepareJobs(ref overlapFinder.broadPhase.ActiveTree, overlapFinder.selfTestHandlers3, threadDispatcher.ThreadCount, workerIndex, threadDispatcher.WorkerPools[workerIndex]); } unsafe static void PrepareIntertreeTestJob3(long id, void* context, int workerIndex, IThreadDispatcher threadDispatcher) { var overlapFinder = (CollidableOverlapFinder)threadDispatcher.ManagedContext; var broadPhase = overlapFinder.broadPhase; - //overlapFinder.intertreeTestContext3.PrepareJobs(ref broadPhase.ActiveTree, ref broadPhase.StaticTree, overlapFinder.intertreeTestHandlers3, threadDispatcher.ThreadCount, workerIndex, threadDispatcher.WorkerPools[workerIndex]); - overlapFinder.intertreeTestContext3.PushRootTasks(ref broadPhase.ActiveTree, ref broadPhase.StaticTree, overlapFinder.intertreeTestHandlers3, threadDispatcher.ThreadCount, workerIndex, threadDispatcher, threadDispatcher.WorkerPools[workerIndex], overlapFinder.broadStack, &IntertreeTestJob3); + overlapFinder.intertreeTestContext3.PrepareJobs(ref broadPhase.ActiveTree, ref broadPhase.StaticTree, overlapFinder.intertreeTestHandlers3, threadDispatcher.ThreadCount, workerIndex, threadDispatcher.WorkerPools[workerIndex]); } unsafe static void CompletedPreparation3(long id, void* context, int workerIndex, IThreadDispatcher threadDispatcher) { + var overlapFinder = (CollidableOverlapFinder)threadDispatcher.ManagedContext; - //By the time we reach this point, both the prepare phases are complete. The broad phase stack will grow no further, so we can go ahead and mark it for stopping. - overlapFinder.broadStack->RequestStop(); - - //var overlapFinder = (CollidableOverlapFinder)threadDispatcher.ManagedContext; - //var selfTest = overlapFinder.selfTestContext3; - //var intertreeTest = overlapFinder.intertreeTestContext3; - //var broadTaskCount = selfTest.JobCount + intertreeTest.JobCount; - ////We'll use a continuation to notify us when all broad jobs are complete by stopping the broad stack. - //ContinuationHandle broadPhaseCompleteContinuation = default; - //var broadStack = overlapFinder.broadStack; - //if (broadTaskCount > 0) - // broadPhaseCompleteContinuation = broadStack->AllocateContinuation(broadTaskCount, workerIndex, threadDispatcher, TaskStack.GetRequestStopTask(broadStack)); - //if (selfTest.JobCount > 0) - // broadStack->PushFor(&SelfTestJob3, null, 0, selfTest.JobCount, workerIndex, threadDispatcher, continuation: broadPhaseCompleteContinuation); - //if (intertreeTest.JobCount > 0) - // broadStack->PushFor(&IntertreeTestJob3, null, 0, intertreeTest.JobCount, workerIndex, threadDispatcher, continuation: broadPhaseCompleteContinuation); - - ////Go ahead and flush the narrow phase work that the preparation phase generated, if any. If there is any left, then it's not full sized (because that would have been flushed), but that's fine. - ////This is mostly free. At this point, we almost certainly have idling workers. - //overlapFinder.taskAccumulators[workerIndex].FlushToStack(workerIndex, threadDispatcher); - - //if (broadTaskCount == 0) - //{ - // //The broad phase didn't actually have work to do, so we can just stop it now. - // //Note that this stop was submitted *after* we flushed the stack! That's because the stop is a sync point, and we want to make sure that all the narrow phase work created by the preparation phase is submitted to the narrow phase stack. - // broadStack->RequestStop(); - //} + var selfTest = overlapFinder.selfTestContext3; + var intertreeTest = overlapFinder.intertreeTestContext3; + var broadTaskCount = selfTest.JobCount + intertreeTest.JobCount; + //We'll use a continuation to notify us when all broad jobs are complete by stopping the broad stack. + ContinuationHandle broadPhaseCompleteContinuation = default; + var broadStack = overlapFinder.broadStack; + if (broadTaskCount > 0) + broadPhaseCompleteContinuation = broadStack->AllocateContinuation(broadTaskCount, workerIndex, threadDispatcher, TaskStack.GetRequestStopTask(broadStack)); + if (selfTest.JobCount > 0) + broadStack->PushFor(&SelfTestJob3, null, 0, selfTest.JobCount, workerIndex, threadDispatcher, continuation: broadPhaseCompleteContinuation); + if (intertreeTest.JobCount > 0) + broadStack->PushFor(&IntertreeTestJob3, null, 0, intertreeTest.JobCount, workerIndex, threadDispatcher, continuation: broadPhaseCompleteContinuation); + + //Go ahead and flush the narrow phase work that the preparation phase generated, if any. If there is any left, then it's not full sized (because that would have been flushed), but that's fine. + //This is mostly free. At this point, we almost certainly have idling workers. + overlapFinder.taskAccumulators[workerIndex].FlushToStack(workerIndex, threadDispatcher); + + if (broadTaskCount == 0) + { + //The broad phase didn't actually have work to do, so we can just stop it now. + //Note that this stop was submitted *after* we flushed the stack! That's because the stop is a sync point, and we want to make sure that all the narrow phase work created by the preparation phase is submitted to the narrow phase stack. + broadStack->RequestStop(); + } } unsafe static void SelfTestJob3(long id, void* context, int workerIndex, IThreadDispatcher threadDispatcher) { var overlapFinder = (CollidableOverlapFinder)threadDispatcher.ManagedContext; - //overlapFinder.selfTestContext3.ExecuteJob((int)id, workerIndex); - overlapFinder.selfTestContext3.ExecuteJob(id, workerIndex); + overlapFinder.selfTestContext3.ExecuteJob((int)id, workerIndex); } unsafe static void IntertreeTestJob3(long id, void* context, int workerIndex, IThreadDispatcher threadDispatcher) { var overlapFinder = (CollidableOverlapFinder)threadDispatcher.ManagedContext; - //overlapFinder.intertreeTestContext3.ExecuteJob((int)id, workerIndex); - overlapFinder.intertreeTestContext3.ExecuteJob(id, workerIndex); + overlapFinder.intertreeTestContext3.ExecuteJob((int)id, workerIndex); } unsafe static void NarrowPhaseJob3(long id, void* untypedContext, int workerIndex, IThreadDispatcher threadDispatcher) @@ -806,8 +799,8 @@ public void Handle(int indexA, int indexB) Buffer taskAccumulators; int previousPairCount3; int taskSize; - Tree.MultithreadedSelfTest2 selfTestContext3; - Tree.MultithreadedIntertreeTest2 intertreeTestContext3; + Tree.MultithreadedSelfTest selfTestContext3; + Tree.MultithreadedIntertreeTest intertreeTestContext3; TaskStack* broadStack, narrowStack; diff --git a/BepuPhysics/Trees/Tree_IntertreeQueriesMT2.cs b/BepuPhysics/Trees/Tree_IntertreeQueriesMT2.cs deleted file mode 100644 index 75cfaf22..00000000 --- a/BepuPhysics/Trees/Tree_IntertreeQueriesMT2.cs +++ /dev/null @@ -1,272 +0,0 @@ -using BepuPhysics.CollisionDetection; -using BepuUtilities; -using BepuUtilities.Collections; -using BepuUtilities.Memory; -using BepuUtilities.TaskScheduling; -using System; -using System.Diagnostics; -using System.Runtime.CompilerServices; -using System.Threading; - -namespace BepuPhysics.Trees; - -partial struct Tree -{ - static long EncodeJobId(int a, int b) => (long)a << 32 | (uint)b; - static (int a, int b) DecodeJobId(long id) => ((int)(id >> 32), (int)id); - public unsafe class MultithreadedIntertreeTest2 where TOverlapHandler : struct, IOverlapHandler - { - public BufferPool Pool; - public TaskStack* Stack; - public delegate* TaskFunction; - int leafThreshold; - public Tree TreeA; - public Tree TreeB; - public TOverlapHandler[] OverlapHandlers; - - /// - /// Prepares the jobs associated with a self test. Must be called before a dispatch over PairTest. - /// - /// Callbacks used to handle individual overlaps detected by the self test. - /// Number of threads to prepare jobs for. - /// Index of the worker executing the preparation job. - /// Pool to allocate from. - /// Task stack to push jobs into. - /// Function to be invoked by tasks pushed onto the stack. - public void PushRootTasks(ref Tree treeA, ref Tree treeB, TOverlapHandler[] overlapHandlers, int threadCount, int workerIndex, IThreadDispatcher dispatcher, BufferPool pool, - TaskStack* taskStack, delegate* taskFunction) - { - Pool = pool; - Stack = taskStack; - TaskFunction = taskFunction; - if (treeA.LeafCount == 0 || treeB.LeafCount == 0) - { - //If either tree has zero leaves, no intertree test is required. - //Since this context has a count property for scheduling purposes that reads the jobs list, clear it to ensure no spurious jobs are executed. - return; - } - Debug.Assert(overlapHandlers.Length >= threadCount); - const float jobMultiplier = 8f; - var targetJobCount = Math.Max(1, jobMultiplier * threadCount); - //TODO: Not a lot of thought was put into this leaf threshold for intertree. Probably better options. - leafThreshold = (int)((treeA.LeafCount + treeB.LeafCount) / targetJobCount); - this.OverlapHandlers = overlapHandlers; - this.TreeA = treeA; - this.TreeB = treeB; - //Collect jobs. - ref var handler = ref OverlapHandlers[workerIndex]; - if (treeA.LeafCount >= 2 && treeB.LeafCount >= 2) - { - //Both trees have complete nodes; we can use a general case. - GetJobsBetweenDifferentNodes(ref treeA.Nodes[0], ref treeB.Nodes[0], workerIndex, dispatcher, ref handler); - } - else if (treeA.LeafCount == 1 && treeB.LeafCount >= 2) - { - //Tree A is degenerate; needs a special case. - ref var a = ref treeA.Nodes[0]; - ref var b = ref treeB.Nodes[0]; - var aaIntersects = BoundingBox.IntersectsUnsafe(a.A, b.A); - var abIntersects = BoundingBox.IntersectsUnsafe(a.A, b.B); - if (aaIntersects) - { - DispatchTestForNodes(ref a.A, ref b.A, workerIndex, dispatcher, ref handler); - } - if (abIntersects) - { - DispatchTestForNodes(ref a.A, ref b.B, workerIndex, dispatcher, ref handler); - } - } - else if (treeA.LeafCount >= 2 && treeB.LeafCount == 1) - { - //Tree B is degenerate; needs a special case. - ref var a = ref treeA.Nodes[0]; - ref var b = ref treeB.Nodes[0]; - var aaIntersects = BoundingBox.IntersectsUnsafe(a.A, b.A); - var baIntersects = BoundingBox.IntersectsUnsafe(a.B, b.A); - if (aaIntersects) - { - DispatchTestForNodes(ref a.A, ref b.A, workerIndex, dispatcher, ref handler); - } - if (baIntersects) - { - DispatchTestForNodes(ref a.B, ref b.A, workerIndex, dispatcher, ref handler); - } - } - else - { - Debug.Assert(treeA.LeafCount == 1 && treeB.LeafCount == 1); - if (BoundingBox.IntersectsUnsafe(treeA.Nodes[0].A, treeB.Nodes[0].A)) - { - DispatchTestForNodes(ref treeA.Nodes[0].A, ref treeB.Nodes[0].A, workerIndex, dispatcher, ref handler); - } - } - - } - - /// - /// Cleans up after a multithreaded self test. - /// - public void CompleteTest() - { - Pool = null; - Stack = null; - TaskFunction = null; - OverlapHandlers = null; - TreeA = default; - TreeB = default; - } - - /// - /// Executes a an intertree test job given by an encoded job id. - /// - /// Encoded job id. - /// Index of the worker executing the job. - public void ExecuteJob(long encodedJobId, int workerIndex) - { - var (a, b) = DecodeJobId(encodedJobId); - if (a >= 0) - { - if (b >= 0) - { - //Different internal nodes. - TreeA.GetOverlapsBetweenDifferentNodes(ref TreeA.Nodes[a], ref TreeB.Nodes[b], ref TreeB, ref OverlapHandlers[workerIndex]); - } - else - { - //A is an internal node, B is a leaf. - var leafIndex = Encode(b); - ref var leaf = ref TreeB.Leaves[leafIndex]; - ref var childOwningLeaf = ref Unsafe.Add(ref TreeB.Nodes[leaf.NodeIndex].A, leaf.ChildIndex); - TreeA.TestNodeAgainstLeaf(a, leafIndex, ref childOwningLeaf, ref OverlapHandlers[workerIndex]); - } - } - else - { - //A is a leaf, B is internal. - var leafIndex = Encode(a); - ref var leaf = ref TreeA.Leaves[leafIndex]; - ref var childOwningLeaf = ref Unsafe.Add(ref TreeA.Nodes[leaf.NodeIndex].A, leaf.ChildIndex); - TreeA.TestLeafAgainstNode(leafIndex, ref childOwningLeaf, b, ref TreeB, ref OverlapHandlers[workerIndex]); - - //NOTE THAT WE DO NOT HANDLE THE CASE THAT BOTH A AND B ARE LEAVES HERE. - //The collection routine should take care of that, since it has more convenient access to bounding boxes and because a single test isn't worth an atomic increment. - } - } - - void DispatchTestForLeaf(ref Tree nodeOwner, int leafIndex, ref NodeChild leafChild, int nodeIndex, int nodeLeafCount, int workerIndex, IThreadDispatcher dispatcher, ref TOverlapHandler results) - { - if (nodeIndex < 0) - { - //Maintain the order of trees. Leaves from tree A should always be the first parameter. - if (Tree.Equals(nodeOwner, TreeA)) - results.Handle(Encode(nodeIndex), leafIndex); - else - results.Handle(leafIndex, Encode(nodeIndex)); - } - else - { - if (nodeLeafCount <= leafThreshold) - { - //Maintain the order of trees. Leaves from tree A should always be the first parameter. - var jobId = Tree.Equals(nodeOwner, TreeA) ? EncodeJobId(nodeIndex, Encode(leafIndex)) : EncodeJobId(Encode(leafIndex), nodeIndex); - if (jobId == -1) - Console.WriteLine("hm"); - Stack->Push(new Task(TaskFunction, null, jobId), workerIndex, dispatcher); - } - else - TestLeafAgainstNode(ref nodeOwner, leafIndex, ref leafChild, nodeIndex, workerIndex, dispatcher, ref results); - } - } - - void TestLeafAgainstNode(ref Tree nodeOwner, int leafIndex, ref NodeChild leafChild, int nodeIndex, int workerIndex, IThreadDispatcher dispatcher, ref TOverlapHandler results) - { - ref var node = ref nodeOwner.Nodes[nodeIndex]; - ref var a = ref node.A; - ref var b = ref node.B; - //Despite recursion, leafBounds should remain in L1- it'll be used all the way down the recursion from here. - //However, while we likely loaded child B when we loaded child A, there's no guarantee that it will stick around. - //Reloading that in the event of eviction would require more work than keeping the derived data on the stack. - //TODO: this is some pretty questionable microtuning. It's not often that the post-leaf-found recursion will be long enough to evict L1. Definitely test it. - var bIndex = b.Index; - var bLeafCount = b.LeafCount; - var aIntersects = BoundingBox.IntersectsUnsafe(leafChild, a); - var bIntersects = BoundingBox.IntersectsUnsafe(leafChild, b); - if (aIntersects) - { - DispatchTestForLeaf(ref nodeOwner, leafIndex, ref leafChild, a.Index, a.LeafCount, workerIndex, dispatcher, ref results); - } - if (bIntersects) - { - DispatchTestForLeaf(ref nodeOwner, leafIndex, ref leafChild, bIndex, bLeafCount, workerIndex, dispatcher, ref results); - } - } - - [MethodImpl(MethodImplOptions.AggressiveInlining)] - void DispatchTestForNodes(ref NodeChild a, ref NodeChild b, int workerIndex, IThreadDispatcher dispatcher, ref TOverlapHandler results) - { - if (a.Index >= 0) - { - if (b.Index >= 0) - { - if (a.LeafCount + b.LeafCount <= leafThreshold) - { - if (EncodeJobId(a.Index, b.Index) == -1) - Console.WriteLine("hm"); - Stack->Push(new Task(TaskFunction, null, EncodeJobId(a.Index, b.Index)), workerIndex, dispatcher); - } - else - GetJobsBetweenDifferentNodes(ref TreeA.Nodes[a.Index], ref TreeB.Nodes[b.Index], workerIndex, dispatcher, ref results); - - } - else - { - //leaf B versus node A. - TestLeafAgainstNode(ref TreeA, Encode(b.Index), ref b, a.Index, workerIndex, dispatcher, ref results); - } - } - else if (b.Index >= 0) - { - //leaf A versus node B. - TestLeafAgainstNode(ref TreeB, Encode(a.Index), ref a, b.Index, workerIndex, dispatcher, ref results); - } - else - { - //Two leaves. - results.Handle(Encode(a.Index), Encode(b.Index)); - } - } - - void GetJobsBetweenDifferentNodes(ref Node a, ref Node b, int workerIndex, IThreadDispatcher dispatcher, ref TOverlapHandler results) - { - //There are no shared children, so test them all. - - ref var aa = ref a.A; - ref var ab = ref a.B; - ref var ba = ref b.A; - ref var bb = ref b.B; - var aaIntersects = BoundingBox.IntersectsUnsafe(aa, ba); - var abIntersects = BoundingBox.IntersectsUnsafe(aa, bb); - var baIntersects = BoundingBox.IntersectsUnsafe(ab, ba); - var bbIntersects = BoundingBox.IntersectsUnsafe(ab, bb); - - if (aaIntersects) - { - DispatchTestForNodes(ref aa, ref ba, workerIndex, dispatcher, ref results); - } - if (abIntersects) - { - DispatchTestForNodes(ref aa, ref bb, workerIndex, dispatcher, ref results); - } - if (baIntersects) - { - DispatchTestForNodes(ref ab, ref ba, workerIndex, dispatcher, ref results); - } - if (bbIntersects) - { - DispatchTestForNodes(ref ab, ref bb, workerIndex, dispatcher, ref results); - } - - } - } - -} diff --git a/BepuPhysics/Trees/Tree_SelfQueriesMT.cs b/BepuPhysics/Trees/Tree_SelfQueriesMT.cs index a03aec38..4e0f7c48 100644 --- a/BepuPhysics/Trees/Tree_SelfQueriesMT.cs +++ b/BepuPhysics/Trees/Tree_SelfQueriesMT.cs @@ -90,7 +90,7 @@ public void PrepareJobs(ref Tree tree, TOverlapHandler[] overlapHandlers, int th /// /// Cleans up after a multithreaded self test. Returns resources to the pool used by . /// - public void CompleteSelfTest() + public void CompleteTest() { //Note that a tree with 0 or 1 entries won't have any jobs. if (jobs.Span.Allocated) diff --git a/BepuPhysics/Trees/Tree_SelfQueriesMT2.cs b/BepuPhysics/Trees/Tree_SelfQueriesMT2.cs deleted file mode 100644 index 7868ed56..00000000 --- a/BepuPhysics/Trees/Tree_SelfQueriesMT2.cs +++ /dev/null @@ -1,229 +0,0 @@ -using BepuUtilities; -using BepuUtilities.Collections; -using BepuUtilities.Memory; -using BepuUtilities.TaskScheduling; -using System; -using System.Diagnostics; -using System.Runtime.CompilerServices; -using System.Threading; - -namespace BepuPhysics.Trees; - -partial struct Tree -{ - public unsafe class MultithreadedSelfTest2 where TOverlapHandler : struct, IOverlapHandler - { - public BufferPool Pool; - public TaskStack* Stack; - public delegate* TaskFunction; - int leafThreshold; - public Tree Tree; - public TOverlapHandler[] OverlapHandlers; - - /// - /// Prepares the jobs associated with a self test. Must be called before a dispatch over PairTest. - /// - /// Tree to test against itself. - /// Callbacks used to handle individual overlaps detected by the self test. - /// Number of threads to prepare jobs for. - /// Index of the worker executing the preparation job. - /// Pool to allocate from. - public void PushRootTasks(ref Tree tree, TOverlapHandler[] overlapHandlers, int threadCount, int workerIndex, IThreadDispatcher dispatcher, BufferPool pool, - TaskStack* taskStack, delegate* taskFunction) - { - Pool = pool; - Stack = taskStack; - TaskFunction = taskFunction; - //If there are not multiple children, there's no need to recurse. - //This provides a guarantee that there are at least 2 children in each internal node considered by GetOverlapsInNode. - if (tree.LeafCount < 2) - { - return; - } - Debug.Assert(overlapHandlers.Length >= threadCount); - const float jobMultiplier = 8f; - var targetJobCount = Math.Max(1, jobMultiplier * threadCount); - leafThreshold = (int)(tree.LeafCount / targetJobCount); - this.OverlapHandlers = overlapHandlers; - this.Tree = tree; - //Collect jobs. - CollectJobsInNode(0, tree.LeafCount, workerIndex, dispatcher, ref OverlapHandlers[workerIndex]); - } - - /// - /// Cleans up after a multithreaded self test. - /// - public void CompleteTest() - { - Pool = null; - Stack = null; - TaskFunction = null; - OverlapHandlers = null; - Tree = default; - } - - public void ExecuteJob(long encodedJobId, int workerIndex) - { - var (a, b) = DecodeJobId(encodedJobId); - if (a >= 0) - { - if (a == b) - { - //Same node. - Tree.GetOverlapsInNode(ref Tree.Nodes[a], ref OverlapHandlers[workerIndex]); - } - else if (b >= 0) - { - //Different nodes. - Tree.GetOverlapsBetweenDifferentNodes(ref Tree.Nodes[a], ref Tree.Nodes[b], ref OverlapHandlers[workerIndex]); - } - else - { - //A is an internal node, B is a leaf. - var leafIndex = Encode(b); - ref var leaf = ref Tree.Leaves[leafIndex]; - ref var childOwningLeaf = ref Unsafe.Add(ref Tree.Nodes[leaf.NodeIndex].A, leaf.ChildIndex); - Tree.TestLeafAgainstNode(leafIndex, ref childOwningLeaf, a, ref OverlapHandlers[workerIndex]); - } - } - else - { - //A is a leaf, B is internal. - var leafIndex = Encode(a); - ref var leaf = ref Tree.Leaves[leafIndex]; - ref var childOwningLeaf = ref Unsafe.Add(ref Tree.Nodes[leaf.NodeIndex].A, leaf.ChildIndex); - Tree.TestLeafAgainstNode(leafIndex, ref childOwningLeaf, b, ref OverlapHandlers[workerIndex]); - - //NOTE THAT WE DO NOT HANDLE THE CASE THAT BOTH A AND B ARE LEAVES HERE. - //The collection routine should take care of that, since it has more convenient access to bounding boxes and because a single test isn't worth an atomic increment. - } - } - - void DispatchTestForLeaf(int leafIndex, ref NodeChild leafChild, int nodeIndex, int nodeLeafCount, int workerIndex, IThreadDispatcher dispatcher, ref TOverlapHandler results) - { - if (nodeIndex < 0) - { - results.Handle(leafIndex, Encode(nodeIndex)); - } - else - { - if (nodeLeafCount <= leafThreshold) - Stack->Push(new Task(TaskFunction, null, EncodeJobId(Encode(leafIndex), nodeIndex)), workerIndex, dispatcher); - else - TestLeafAgainstNode(leafIndex, ref leafChild, nodeIndex, workerIndex, dispatcher, ref results); - } - } - - void TestLeafAgainstNode(int leafIndex, ref NodeChild leafChild, int nodeIndex, int workerIndex, IThreadDispatcher dispatcher, ref TOverlapHandler results) - { - ref var node = ref Tree.Nodes[nodeIndex]; - ref var a = ref node.A; - ref var b = ref node.B; - //Despite recursion, leafBounds should remain in L1- it'll be used all the way down the recursion from here. - //However, while we likely loaded child B when we loaded child A, there's no guarantee that it will stick around. - //Reloading that in the event of eviction would require more work than keeping the derived data on the stack. - //TODO: this is some pretty questionable microtuning. It's not often that the post-leaf-found recursion will be long enough to evict L1. Definitely test it. - var bIndex = b.Index; - var bLeafCount = b.LeafCount; - var aIntersects = BoundingBox.IntersectsUnsafe(leafChild, a); - var bIntersects = BoundingBox.IntersectsUnsafe(leafChild, b); - if (aIntersects) - { - DispatchTestForLeaf(leafIndex, ref leafChild, a.Index, a.LeafCount, workerIndex, dispatcher, ref results); - } - if (bIntersects) - { - DispatchTestForLeaf(leafIndex, ref leafChild, bIndex, bLeafCount, workerIndex, dispatcher, ref results); - } - } - - [MethodImpl(MethodImplOptions.AggressiveInlining)] - void DispatchTestForNodes(ref NodeChild a, ref NodeChild b, int workerIndex, IThreadDispatcher dispatcher, ref TOverlapHandler results) - { - if (a.Index >= 0) - { - if (b.Index >= 0) - { - if (a.LeafCount + b.LeafCount <= leafThreshold) - Stack->Push(new Task(TaskFunction, null, EncodeJobId(a.Index, b.Index)), workerIndex, dispatcher); - else - GetJobsBetweenDifferentNodes(ref Tree.Nodes[a.Index], ref Tree.Nodes[b.Index], workerIndex, dispatcher, ref results); - - } - else - { - //leaf B versus node A. - TestLeafAgainstNode(Encode(b.Index), ref b, a.Index, workerIndex, dispatcher, ref results); - } - } - else if (b.Index >= 0) - { - //leaf A versus node B. - TestLeafAgainstNode(Encode(a.Index), ref a, b.Index, workerIndex, dispatcher, ref results); - } - else - { - //Two leaves. - results.Handle(Encode(a.Index), Encode(b.Index)); - } - } - - void GetJobsBetweenDifferentNodes(ref Node a, ref Node b, int workerIndex, IThreadDispatcher dispatcher, ref TOverlapHandler results) - { - //There are no shared children, so test them all. - - ref var aa = ref a.A; - ref var ab = ref a.B; - ref var ba = ref b.A; - ref var bb = ref b.B; - var aaIntersects = BoundingBox.IntersectsUnsafe(aa, ba); - var abIntersects = BoundingBox.IntersectsUnsafe(aa, bb); - var baIntersects = BoundingBox.IntersectsUnsafe(ab, ba); - var bbIntersects = BoundingBox.IntersectsUnsafe(ab, bb); - - if (aaIntersects) - { - DispatchTestForNodes(ref aa, ref ba, workerIndex, dispatcher, ref results); - } - if (abIntersects) - { - DispatchTestForNodes(ref aa, ref bb, workerIndex, dispatcher, ref results); - } - if (baIntersects) - { - DispatchTestForNodes(ref ab, ref ba, workerIndex, dispatcher, ref results); - } - if (bbIntersects) - { - DispatchTestForNodes(ref ab, ref bb, workerIndex, dispatcher, ref results); - } - - } - - void CollectJobsInNode(int nodeIndex, int leafCount, int workerIndex, IThreadDispatcher dispatcher, ref TOverlapHandler results) - { - if (leafCount <= leafThreshold) - { - Stack->Push(new Task(TaskFunction, null, EncodeJobId(nodeIndex, nodeIndex)), workerIndex, dispatcher); - return; - } - - ref var node = ref Tree.Nodes[nodeIndex]; - ref var a = ref node.A; - ref var b = ref node.B; - - var ab = BoundingBox.IntersectsUnsafe(a, b); - - if (a.Index >= 0) - CollectJobsInNode(a.Index, a.LeafCount, workerIndex, dispatcher, ref results); - if (b.Index >= 0) - CollectJobsInNode(b.Index, b.LeafCount, workerIndex, dispatcher, ref results); - - if (ab) - { - DispatchTestForNodes(ref a, ref b, workerIndex, dispatcher, ref results); - } - - } - } -}