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 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;
}
