unsafe void SplitLeavesIntoChildren(ref SweepResources leaves, int start, int count, int nodeIndex) { Debug.Assert(count >= 2); FindPartition(ref leaves, start, count, out int splitIndex, out BoundingBox aBounds, out BoundingBox bBounds, out int leafCountA, out int leafCountB); var node = nodes + nodeIndex; ref var a = ref node->A;
unsafe void FindPartition(ref SweepResources leaves, int start, int count, out int splitIndex, out BoundingBox a, out BoundingBox b, out int leafCountA, out int leafCountB) { //A variety of potential microoptimizations exist here. //Initialize the per-axis candidate maps. for (int i = 0; i < count; ++i) { var originalValue = leaves.IndexMap[i + start]; leaves.IndexMapX[i] = originalValue; leaves.IndexMapY[i] = originalValue; leaves.IndexMapZ[i] = originalValue; } int xSplitIndex, xLeafCountA, xLeafCountB, ySplitIndex, yLeafCountA, yLeafCountB, zSplitIndex, zLeafCountA, zLeafCountB; BoundingBox xA, xB, yA, yB, zA, zB; float xCost, yCost, zCost; FindPartitionForAxis(leaves.Bounds, leaves.Merged, leaves.CentroidsX, leaves.IndexMapX, count, out xSplitIndex, out xCost, out xA, out xB, out xLeafCountA, out xLeafCountB); FindPartitionForAxis(leaves.Bounds, leaves.Merged, leaves.CentroidsY, leaves.IndexMapY, count, out ySplitIndex, out yCost, out yA, out yB, out yLeafCountA, out yLeafCountB); FindPartitionForAxis(leaves.Bounds, leaves.Merged, leaves.CentroidsZ, leaves.IndexMapZ, count, out zSplitIndex, out zCost, out zA, out zB, out zLeafCountA, out zLeafCountB); int *bestIndexMap; if (xCost <= yCost && xCost <= zCost) { splitIndex = xSplitIndex; a = xA; b = xB; leafCountA = xLeafCountA; leafCountB = xLeafCountB; bestIndexMap = leaves.IndexMapX; } else if (yCost <= zCost) { splitIndex = ySplitIndex; a = yA; b = yB; leafCountA = yLeafCountA; leafCountB = yLeafCountB; bestIndexMap = leaves.IndexMapY; } else { splitIndex = zSplitIndex; a = zA; b = zB; leafCountA = zLeafCountA; leafCountB = zLeafCountB; bestIndexMap = leaves.IndexMapZ; } for (int i = 0; i < count; ++i) { leaves.IndexMap[i + start] = bestIndexMap[i]; } splitIndex += start; }
unsafe void SplitLeavesIntoChildren(ref SweepResources leaves, int start, int count, int nodeIndex) { if (count > 1) { FindPartition(ref leaves, start, count, out int splitIndex, out BoundingBox aBounds, out BoundingBox bBounds, out int leafCountA, out int leafCountB); var node = nodes + nodeIndex; var childIndexA = node->ChildCount++; var childIndexB = node->ChildCount++; Debug.Assert(node->ChildCount <= 2); ref var a = ref node->A; ref var b = ref node->B;
unsafe int CreateSweepBuilderNode(int parentIndex, int indexInParent, ref SweepResources leaves, int start, int count) { bool nodesInvalidated; var nodeIndex = AllocateNode(out nodesInvalidated); Debug.Assert(!nodesInvalidated, "The sweep builder should have allocated enough nodes ahead of time."); var node = nodes + nodeIndex; node->Parent = parentIndex; node->IndexInParent = indexInParent; if (count <= ChildrenCapacity) { //No need to do any sorting. This node can fit every remaining subtree. node->ChildCount = count; var bounds = &node->A; var children = &node->ChildA; var leafCounts = &node->LeafCountA; for (int i = 0; i < count; ++i) { var index = leaves.IndexMap[i + start]; bool leavesInvalidated; var leafIndex = AddLeaf(leaves.Ids[index], nodeIndex, i, out leavesInvalidated); Debug.Assert(!leavesInvalidated); bounds[i] = leaves.Bounds[index]; children[i] = Encode(leafIndex); leafCounts[i] = 1; } return(nodeIndex); } const int recursionDepth = ChildrenCapacity == 32 ? 4 : ChildrenCapacity == 16 ? 3 : ChildrenCapacity == 8 ? 2 : ChildrenCapacity == 4 ? 1 : 0; SplitLeavesIntoChildren(recursionDepth, ref leaves, start, count, nodeIndex); return(nodeIndex); }
unsafe void SplitLeavesIntoChildren(int depthRemaining, ref SweepResources leaves, int start, int count, int nodeIndex) { if (count > 1) { BoundingBox a, b; int leafCountA, leafCountB; int splitIndex; FindPartition(ref leaves, start, count, out splitIndex, out a, out b, out leafCountA, out leafCountB); if (depthRemaining > 0) { --depthRemaining; SplitLeavesIntoChildren(depthRemaining, ref leaves, start, splitIndex - start, nodeIndex); SplitLeavesIntoChildren(depthRemaining, ref leaves, splitIndex, start + count - splitIndex, nodeIndex); } else { //Recursion bottomed out. var node = nodes + nodeIndex; var childIndexA = node->ChildCount++; var childIndexB = node->ChildCount++; Debug.Assert(node->ChildCount <= ChildrenCapacity); var bounds = &node->A; var children = &node->ChildA; var leafCounts = &node->LeafCountA; bounds[childIndexA] = a; bounds[childIndexB] = b; leafCounts[childIndexA] = leafCountA; leafCounts[childIndexB] = leafCountB; if (leafCountA > 1) { children[childIndexA] = CreateSweepBuilderNode(nodeIndex, childIndexA, ref leaves, start, leafCountA); } else { Debug.Assert(leafCountA == 1); //Only one leaf. Don't create another node. bool leavesInvalidated; var leafIndex = AddLeaf(leaves.Ids[leaves.IndexMap[start]], nodeIndex, childIndexA, out leavesInvalidated); Debug.Assert(!leavesInvalidated); children[childIndexA] = Encode(leafIndex); } if (leafCountB > 1) { children[childIndexB] = CreateSweepBuilderNode(nodeIndex, childIndexB, ref leaves, splitIndex, leafCountB); } else { Debug.Assert(leafCountB == 1); //Only one leaf. Don't create another node. bool leavesInvalidated; var leafIndex = AddLeaf(leaves.Ids[leaves.IndexMap[splitIndex]], nodeIndex, childIndexB, out leavesInvalidated); Debug.Assert(!leavesInvalidated); children[childIndexB] = Encode(leafIndex); } } } else { Debug.Assert(count == 1); //Only one leaf. Just stick it directly into the node. var node = nodes + nodeIndex; var childIndex = node->ChildCount++; Debug.Assert(node->ChildCount <= ChildrenCapacity); bool leavesInvalidated; var index = leaves.IndexMap[start]; var leafIndex = AddLeaf(leaves.Ids[index], nodeIndex, childIndex, out leavesInvalidated); Debug.Assert(!leavesInvalidated); (&node->A)[childIndex] = leaves.Bounds[index]; (&node->ChildA)[childIndex] = Encode(leafIndex); (&node->LeafCountA)[childIndex] = 1; } }
unsafe void FindPartition(ref SweepResources leaves, int start, int count, out int splitIndex, out BoundingBox a, out BoundingBox b, out int leafCountA, out int leafCountB) { //A variety of potential microoptimizations exist here. //Initialize the per-axis candidate maps. for (int i = 0; i < count; ++i) { var originalValue = leaves.IndexMap[i + start]; leaves.IndexMapX[i] = originalValue; leaves.IndexMapY[i] = originalValue; leaves.IndexMapZ[i] = originalValue; } int xSplitIndex, xLeafCountA, xLeafCountB, ySplitIndex, yLeafCountA, yLeafCountB, zSplitIndex, zLeafCountA, zLeafCountB; BoundingBox xA, xB, yA, yB, zA, zB; float xCost, yCost, zCost; FindPartitionForAxis(leaves.Bounds, leaves.Merged, leaves.CentroidsX, leaves.IndexMapX, count, out xSplitIndex, out xCost, out xA, out xB, out xLeafCountA, out xLeafCountB); FindPartitionForAxis(leaves.Bounds, leaves.Merged, leaves.CentroidsY, leaves.IndexMapY, count, out ySplitIndex, out yCost, out yA, out yB, out yLeafCountA, out yLeafCountB); FindPartitionForAxis(leaves.Bounds, leaves.Merged, leaves.CentroidsZ, leaves.IndexMapZ, count, out zSplitIndex, out zCost, out zA, out zB, out zLeafCountA, out zLeafCountB); int* bestIndexMap; if (xCost <= yCost && xCost <= zCost) { splitIndex = xSplitIndex; a = xA; b = xB; leafCountA = xLeafCountA; leafCountB = xLeafCountB; bestIndexMap = leaves.IndexMapX; } else if (yCost <= zCost) { splitIndex = ySplitIndex; a = yA; b = yB; leafCountA = yLeafCountA; leafCountB = yLeafCountB; bestIndexMap = leaves.IndexMapY; } else { splitIndex = zSplitIndex; a = zA; b = zB; leafCountA = zLeafCountA; leafCountB = zLeafCountB; bestIndexMap = leaves.IndexMapZ; } for (int i = 0; i < count; ++i) { leaves.IndexMap[i + start] = bestIndexMap[i]; } splitIndex += start; }
unsafe int CreateSweepBuilderNode(int parentIndex, int indexInParent, ref SweepResources leaves, int start, int count) { bool nodesInvalidated; var nodeIndex = AllocateNode(out nodesInvalidated); Debug.Assert(!nodesInvalidated, "The sweep builder should have allocated enough nodes ahead of time."); var node = nodes + nodeIndex; node->Parent = parentIndex; node->IndexInParent = indexInParent; if (count <= ChildrenCapacity) { //No need to do any sorting. This node can fit every remaining subtree. node->ChildCount = count; var bounds = &node->A; var children = &node->ChildA; var leafCounts = &node->LeafCountA; for (int i = 0; i < count; ++i) { var index = leaves.IndexMap[i + start]; bool leavesInvalidated; var leafIndex = AddLeaf(leaves.Ids[index], nodeIndex, i, out leavesInvalidated); Debug.Assert(!leavesInvalidated); bounds[i] = leaves.Bounds[index]; children[i] = Encode(leafIndex); leafCounts[i] = 1; } return nodeIndex; } const int recursionDepth = ChildrenCapacity == 32 ? 4 : ChildrenCapacity == 16 ? 3 : ChildrenCapacity == 8 ? 2 : ChildrenCapacity == 4 ? 1 : 0; SplitLeavesIntoChildren(recursionDepth, ref leaves, start, count, nodeIndex); return nodeIndex; }