public unsafe void Insert(T leaf)
{
int levelIndex = 0;
int nodeIndex = 0;
BoundingBox aosBox;
leaf.GetBoundingBox(out aosBox);
var box = new BoundingBoxWide(ref aosBox);
#if OUTPUT
List <int> choices = new List <int>();
#endif
while (true)
{
var level = Levels[levelIndex];
//Which child should the leaf belong to?
Vector <float> originalVolumes;
BoundingBoxWide.ComputeVolume(ref level.Nodes[nodeIndex].BoundingBoxes, out originalVolumes);
originalVolumes = Vector.Max(originalVolumes, Vector <float> .Zero);
BoundingBoxWide merged;
BoundingBoxWide.Merge(ref level.Nodes[nodeIndex].BoundingBoxes, ref box, out merged);
Vector <float> mergedVolumes;
BoundingBoxWide.ComputeVolume(ref merged, out mergedVolumes);
//Give the leaf to whichever node had the least volume change.
var volumeIncreases = mergedVolumes - originalVolumes;
int minimumIndex = 0;
var minimum = volumeIncreases[0];
for (int i = 1; i < Vector <float> .Count; ++i)
{
if (volumeIncreases[i] < minimum)
{
minimumIndex = i;
minimum = volumeIncreases[i];
}
}
//var volumeIncreases = mergedVolumes - originalVolumes;
//int minimumIndex = 0;
//var minimum = float.MaxValue;
//for (int i = 0; i < Vector<float>.Count; ++i)
//{
// if (level.Nodes[nodeIndex].Children[i] == -1)
// {
// minimumIndex = i;
// break;
// }
// if (volumeIncreases[i] < minimum)
// {
// minimumIndex = i;
// minimum = volumeIncreases[i];
// }
//}
#if OUTPUT
Console.WriteLine($"Minimum index: {minimumIndex}, minimum volume increase: {minimum}");
choices.Add(minimumIndex);
#endif
var childIndex = level.Nodes[nodeIndex].Children[minimumIndex];
if (childIndex < -1)
{
//It's a leaf node.
//Create a new internal node with the new leaf and the old leaf as children.
var nextLevel = levelIndex + 1;
//this is the only place where a new level could potentially be created.
EnsureLevel(nextLevel);
Node newNode;
InitializeNode(out newNode);
//The first child of the new node is the old leaf. Insert its bounding box.
//Since we don't have a great way of shuffling yet, just let it be in the same index.
BoundingBoxWide.ConditionalSelect(
ref singleMasks[minimumIndex],
ref level.Nodes[nodeIndex].BoundingBoxes,
ref newNode.BoundingBoxes,
out newNode.BoundingBoxes);
newNode.Children = Vector.ConditionalSelect(singleMasks[minimumIndex], level.Nodes[nodeIndex].Children, newNode.Children);
//Insert the new leaf into the second child slot.
//Just put it in the next slot over from the minimum.
var newLeafChildIndex = (minimumIndex + 1) & (Vector <float> .Count - 1);
BoundingBoxWide.ConditionalSelect(
ref singleMasks[newLeafChildIndex],
ref box,
ref newNode.BoundingBoxes,
out newNode.BoundingBoxes);
var newNodeIndex = Levels[nextLevel].Add(ref newNode);
var leafIndex = AddLeaf(leaf, nextLevel, newNodeIndex, newLeafChildIndex);
var leafIndexVector = new Vector <int>(Encode(leafIndex));
Levels[nextLevel].Nodes[newNodeIndex].Children = Vector.ConditionalSelect(singleMasks[newLeafChildIndex], leafIndexVector, newNode.Children);
//Update the old leaf node with the new index information.
var oldLeafIndex = Encode(childIndex);
leaves[oldLeafIndex].LevelIndex = nextLevel;
leaves[oldLeafIndex].NodeIndex = newNodeIndex;
//Since we inserted it into the same slot of the new node as it was in in the old node, there is no change to the child index.
//Update the original node's child pointer and bounding box.
var newNodeIndexVector = new Vector <int>(newNodeIndex);
level.Nodes[nodeIndex].Children = Vector.ConditionalSelect(singleMasks[minimumIndex], newNodeIndexVector, level.Nodes[nodeIndex].Children);
BoundingBoxWide.ConditionalSelect(ref singleMasks[minimumIndex], ref merged, ref level.Nodes[nodeIndex].BoundingBoxes, out level.Nodes[nodeIndex].BoundingBoxes);
#if OUTPUT
Console.WriteLine($"Leaf {leafIndex} merged with existing leaf.");// New Node Children: {newNode.Children}, Old Node children: {level.Nodes[nodeIndex].Children}");
Console.WriteLine($"Choices: {GetChoiceList(choices)}");
#endif
break;
}
if (childIndex == -1)
{
//There is no child at all.
//Put the new leaf here.
var leafIndex = AddLeaf(leaf, levelIndex, nodeIndex, minimumIndex);
var leafIndexVector = new Vector <int>(Encode(leafIndex));
level.Nodes[nodeIndex].Children = Vector.ConditionalSelect(singleMasks[minimumIndex], leafIndexVector, level.Nodes[nodeIndex].Children);
BoundingBoxWide.ConditionalSelect(ref singleMasks[minimumIndex], ref merged, ref level.Nodes[nodeIndex].BoundingBoxes, out level.Nodes[nodeIndex].BoundingBoxes);
#if OUTPUT
Console.WriteLine($"Leaf {leafIndex} inserted in empty slot.");
Console.WriteLine($"Choices: {GetChoiceList(choices)}");
#endif
break;
}
//It's an internal node. Traverse to the next node.
BoundingBoxWide.ConditionalSelect(ref singleMasks[minimumIndex], ref merged, ref level.Nodes[nodeIndex].BoundingBoxes, out level.Nodes[nodeIndex].BoundingBoxes);
nodeIndex = level.Nodes[nodeIndex].Children[minimumIndex];
++levelIndex;
}
}
