/// <summary>
/// Finds the node that has the assigned user data.
/// </summary>
/// <param name="userData"></param>
/// <returns></returns>
private AABBNode FindNode(T userData)
{
AABBNode foundNode = null;
List <AABBNode> nodesToSearch = new List <AABBNode>();
nodesToSearch.Add(m_RootNode);
while (nodesToSearch.Count > 0)
{
AABBNode currentNode = nodesToSearch[0];
nodesToSearch.RemoveAt(0);
if (currentNode.UserData == userData)
{
foundNode = currentNode;
break;
}
else if (!currentNode.IsLeaf)
{
nodesToSearch.AddRange(currentNode.Children);
}
}
return(foundNode);
}
/// <summary>
/// Finds the leaf node that matches bounds.
/// </summary>
/// <param name="bounds"></param>
/// <returns></returns>
private AABBNode FindNode(Bounds bounds)
{
AABBNode foundNode = null;
AABBNode currentNode = m_RootNode;
while (currentNode != null)
{
if (currentNode.IsLeaf)
{
foundNode = currentNode.Bounds == bounds ? currentNode : null;
break;
}
else
{
//Which child node if any would the bounds be in?
if (currentNode.Children[0].Bounds.ContainsBounds(bounds))
{
currentNode = currentNode.Children[0];
}
else if (currentNode.Children[1].Bounds.ContainsBounds(bounds))
{
currentNode = currentNode.Children[1];
}
else
{
currentNode = null;
}
}
}
return(foundNode);
}
//-----------------------------------------------------------------------------------------------------------
#region Private Functions
/// <summary>
/// Recursively Insert the new Node until we hit a leaf node. Then branch and insert both nodes.
/// </summary>
/// <param name="currentNode"></param>
/// <param name="newNode"></param>
private void RecursiveInsert(AABBNode currentNode, AABBNode newNode)
{
AABBNode branch = currentNode;
if (currentNode.IsLeaf)
{
branch = AABBNode.CreateNode(currentNode.Bounds, newNode.Bounds);
branch.Parent = currentNode.Parent;
if (currentNode == m_RootNode)
{
m_RootNode = branch;
}
else
{
branch.Parent.Children[branch.Parent.Children[0] == currentNode ? 0 : 1] = branch;
}
branch.SetChildren(currentNode, newNode);
}
else
{
Bounds withChild1 = branch.Children[0].Bounds.ExpandToContian(newNode.Bounds);
Bounds withChild2 = branch.Children[1].Bounds.ExpandToContian(newNode.Bounds);
float volume1 = withChild1.Volume();
float volume2 = withChild2.Volume();
RecursiveInsert((volume1 <= volume2) ? branch.Children[0] : branch.Children[1], newNode);
}
branch.RebuildBounds();
}
/// <summary>
/// Creates a node containing both bounds.
/// </summary>
/// <param name="rightBounds"></param>
/// <param name="leftBounds"></param>
/// <returns></returns>
public static AABBNode CreateNode(Bounds rightBounds, Bounds leftBounds)
{
AABBNode newNode = new AABBNode();
newNode.Bounds = rightBounds.ExpandToContian(leftBounds);
return(newNode);
}
public void AddNode(AABBNode node)
{
node.UpdateAABB();
if (leaves.Capacity == 0)
{
root = node;
leaves.Add(node);
}
else
{
AABBNode currentNode = root;
//Find a sibling for a new node
while (!currentNode.IsLeaf())
{
currentNode = SearchNode(node, currentNode);
}
currentNode = InsertNode(currentNode, node);
//Check if it's a 2nd iteration and if tree can be balanced
Balance(currentNode);
leaves.Add(node);
}
}
private void MergeIntoBranch(AABBNode myNewNode, AABBNode currentObject)
{
AABBNode myNewBranch = new AABBNode();
BoundingBox.CreateMerged(ref myNewNode.myBox, ref currentObject.myBox, out myNewBranch.myBox);
myNewBranch.Left = currentObject;
myNewBranch.DepthOffset = currentObject.DepthOffset;
currentObject.DepthOffset--;
myNewBranch.Right = myNewNode; //Depth Offset = 0;
myNewNode.Parent = myNewBranch;
if (currentObject.Parent == null)
{
Root = myNewBranch;
}
else if (currentObject.Parent.Right == currentObject)
{
currentObject.Parent.Right = myNewBranch;
}
else if (currentObject.Parent.Left == currentObject)
{
currentObject.Parent.Left = myNewBranch;
}
myNewBranch.Parent = currentObject.Parent;
currentObject.Parent = myNewBranch;
}
private int RemoveInvalidParents(AABBNode NodeBeingRemoved, out AABBNode lastModifiedNode)
{
int Count = 0;
var myParent = NodeBeingRemoved.Parent;
if (NodeBeingRemoved.Parent.Left == NodeBeingRemoved)
{
NodeBeingRemoved.Parent.Left = null;
}
else if (NodeBeingRemoved.Parent.Right == NodeBeingRemoved)
{
NodeBeingRemoved.Parent.Right = null;
}
lastModifiedNode = NodeBeingRemoved.Parent;
if (myParent.Right == null && myParent.Left == null) //case 1 node;
{
var formerChildNode = myParent;
myParent = myParent.Parent;
while (myParent != null)
{
if (myParent.Left == formerChildNode)
{
myParent.Left = null;
if (myParent.Right == null)
{
formerChildNode = myParent;
myParent = myParent.Parent;
Count++;
}
else
{
lastModifiedNode = myParent;
myParent = null;
}
}
else
{
myParent.Right = null;
if (myParent.Left == null)
{
Count++;
formerChildNode = myParent;
myParent = myParent.Parent;
}
else
{
lastModifiedNode = myParent;
myParent = null;
}
}
}
}
else
{
lastModifiedNode = myParent;
}
return(Count);
}
/// <summary>
/// Removes a node from the tree
/// </summary>
/// <param name="node"></param>
private void RemoveNode(AABBNode node)
{
AABBNode nodeParent = node.Parent;
if (node == m_RootNode)
{
m_RootNode = null;
}
else
{
AABBNode otherChild = nodeParent.Children[0] == node ? nodeParent.Children[1] : nodeParent.Children[0];
if (nodeParent.Parent == null)
{
m_RootNode = otherChild;
otherChild.Parent = null;
}
else
{
int childIndex = nodeParent.Parent.Children[0] == nodeParent ? 0 : 1;
nodeParent.Parent.Children[childIndex] = otherChild;
otherChild.Parent = nodeParent.Parent;
}
UpdateNodeBoundUp(otherChild.Parent);
}
}
//Adjust sizes of AABB's after insertion of a new Node
private void AdjustSizeOfOne(AABBNode node)
{
while (node != null)
{
node.UpdateAABB();
node = node.parent;
}
}
//-----------------------------------------------------------------------------------------------------------
#region Public Functions
/// <summary>
/// Sets the children for this node.
/// </summary>
/// <param name="child1"></param>
/// <param name="child2"></param>
public void SetChildren(AABBNode child1, AABBNode child2)
{
child1.Parent = this;
child2.Parent = this;
Children[0] = child1;
Children[1] = child2;
}
private static void ResizeBranch(AABBNode myNewNode, AABBNode CurrentObject)
{
var ContainmentTypeReturned = CurrentObject.myBox.Contains(myNewNode.myBox);
if (ContainmentTypeReturned == ContainmentType.Intersects || ContainmentTypeReturned == ContainmentType.Disjoint)
{
BoundingBox.CreateMerged(ref myNewNode.myBox, ref CurrentObject.myBox, out CurrentObject.myBox);
}
}
/*Branch – Our branches always have exactly two children (known as left and right) and are assigned an AABB that is large enough to contain all of it’s descendants.
* Leaf – Our leaves are associated with a game world object and through that have an AABB. A leaf’s AABB must fit entirely within it’s parents AABB and due to how our branches are defined that means it fits in every ancestors AABB.
* Root – Our root may be a branch or a leaf*/
public void Insert <T>(ref BoundingBox myBox, T input)
{
AABBNode myNewNode = new AABBNodeObject <T>()
{
myBox = myBox, myData = input
};
myNodes.Add(input, myNewNode);
if (Root == null)
{
Root = myNewNode;
return;
}
AABBNode CurrentObject = Root;
while (CurrentObject != null)
{
if (CurrentObject.ContainsData()) //It's a game object
{
MergeIntoBranch(myNewNode, CurrentObject);
return;
}
else //It's just a branch.
{
ResizeBranch(myNewNode, CurrentObject);
ContainmentType leftContainment = ContainmentType.Disjoint;
ContainmentType rightContainment = ContainmentType.Disjoint;
if (CurrentObject.Left != null)
{
leftContainment = CurrentObject.Left.myBox.Contains(myBox);
}
if (CurrentObject.Right != null)
{
rightContainment = CurrentObject.Right.myBox.Contains(myBox);
}
if ((leftContainment == ContainmentType.Contains || leftContainment == ContainmentType.Intersects) && rightContainment == ContainmentType.Disjoint)
{
CurrentObject = CurrentObject.Left;
}
else if ((rightContainment == ContainmentType.Contains || rightContainment == ContainmentType.Intersects) && leftContainment == ContainmentType.Disjoint)
{
CurrentObject = CurrentObject.Right;
}
else if (leftContainment == ContainmentType.Intersects && rightContainment == ContainmentType.Intersects)
{
CurrentObject = GetNextByHeuristic(myNewNode, CurrentObject);
}
else
{
throw new Exception("Remove or Insert failed to clean up AABB tree properly!");
}
}
}
}
/// <summary>
/// Creates a node with the bounds and data.
/// </summary>
/// <param name="bounds"></param>
/// <param name="data"></param>
/// <returns></returns>
public static AABBNode CreateNode(Bounds bounds, T data)
{
AABBNode newNode = new AABBNode();
newNode.Bounds = bounds;
newNode.UserData = data;
// Determine if we want a margin bounds;
return(newNode);
}
/// <summary>
/// Updates the bounds nonleaf node object moving up the Parent tree to Root.
/// </summary>
/// <param name="node"></param>
private void UpdateNodeBoundUp(AABBNode node)
{
if (node != null)
{
if (!node.IsLeaf)
{
node.RebuildBounds();
}
UpdateNodeBoundUp(node.Parent);
}
}
public RayCastHit <T> BoundingPrimativeCast <T>(BoundingBox myTestBox)
{
if (Root == null)
{
return(new RayCastHit <T>(new List <T>()));
}
List <T> myResults = new List <T>();
Stack <AABBNode> NodesToExplore = new Stack <AABBNode>();
AABBNode Current = Root;
while (Current != null)
{
ContainmentType left = ContainmentType.Disjoint;
ContainmentType right = ContainmentType.Disjoint;
if (Current.Left != null)
{
left = myTestBox.Contains(Current.Left.myBox);
}
if (Current.Right != null)
{
right = myTestBox.Contains(Current.Right.myBox);
}
if (left == ContainmentType.Contains || left == ContainmentType.Intersects) //So it hit.
{
if (Current.Left.ContainsData())
{
myResults.Add(((AABBNodeObject <T>)Current.Left).myData);
}
else
{
NodesToExplore.Push(Current.Left);
}
}
if (right == ContainmentType.Contains || right == ContainmentType.Intersects) //So it hit.
{
if (Current.Right.ContainsData())
{
myResults.Add(((AABBNodeObject <T>)Current.Right).myData);
}
else
{
NodesToExplore.Push(Current.Right);
}
}
if (NodesToExplore.Count == 0)
{
break;
}
Current = NodesToExplore.Pop();
}
return(new RayCastHit <T>(myResults));
}
//-----------------------------------------------------------------------------------------------------------
#region Public Functions
/// <summary>
/// Creates a new node with the provided bounds.
/// </summary>
/// <param name="bounds"></param>
/// <param name="data"></param>
public void Insert(Bounds bounds, T data)
{
AABBNode newNode = AABBNode.CreateNode(bounds, data);
if (m_RootNode == null)
{
m_RootNode = newNode;
}
else
{
RecursiveInsert(m_RootNode, newNode);
}
}
//Find deepest node that does not contain placingNode 's bounds and makes smallest AABB with new node
private AABBNode SearchNode(AABBNode placingNode, AABBNode currentNode)
{
if (currentNode.left.nodeBounds.Contains(placingNode.nodeBounds))
{
return(currentNode.left);
}
else if (currentNode.right.nodeBounds.Contains(placingNode.nodeBounds))
{
return(currentNode.right);
}
else
{
return(CheckSize(placingNode, currentNode));
}
}
private void BuildRecursive(int nodeIndex, int start, int numFaces)
{
int MaxFacesPerLeaf = 6;
// a reference to the current node, need to be careful here as this reference may become invalid if array is resized
AABBNode n = GetNode(nodeIndex);
// track max tree depth
++CurrentDepth;
MaxDepth = Math.Max(MaxDepth, CurrentDepth);
int[] faces = GetFaces(start, numFaces);
Vector3 min, max;
CalculateFaceBounds(faces, out min, out max);
n.Bounds = new Box3(min, max);
n.Level = CurrentDepth - 1;
// calculate bounds of faces and add node
if (numFaces <= MaxFacesPerLeaf)
{
n.Faces = faces;
++LeafNodes;
}
else
{
++InnerNodes;
// face counts for each branch
//const uint32_t leftCount = PartitionMedian(n, faces, numFaces);
int leftCount = PartitionSAH(faces);
int rightCount = numFaces - leftCount;
// alloc 2 nodes
Nodes[nodeIndex].Children = FreeNode;
// allocate two nodes
FreeNode += 2;
// split faces in half and build each side recursively
BuildRecursive(GetNode(nodeIndex).Children + 0, start, leftCount);
BuildRecursive(GetNode(nodeIndex).Children + 1, start + leftCount, rightCount);
}
--CurrentDepth;
}
public RayCastHit <T> RaycastReturnFirst <T>(Ray aRay)
{
if (Root == null)
{
return(new RayCastHit <T>(new List <T>()));
}
List <T> myResults = new List <T>();
Stack <AABBNode> NodesToExplore = new Stack <AABBNode>();
AABBNode Current = Root;
while (Current != null)
{
if (aRay.Intersects(Current.Left.myBox).HasValue) //So it hit.
{
if (Current.Left.ContainsData() && Current.Left is AABBNodeObject <T> )
{
myResults.Add(((AABBNodeObject <T>)Current.Left).myData);
break;
}
else
{
NodesToExplore.Push(Current.Left);
}
}
if (aRay.Intersects(Current.Right.myBox).HasValue) //So it hit.
{
if (Current.Right.ContainsData() && Current.Right is AABBNodeObject <T> )
{
myResults.Add(((AABBNodeObject <T>)Current.Right).myData);
break;
}
else
{
NodesToExplore.Push(Current.Right);
}
}
if (NodesToExplore.Count == 0)
{
break;
}
Current = NodesToExplore.Pop();
}
return(new RayCastHit <T>(myResults));
}
private AABBNode GetNextByHeuristic(AABBNode myNewNode, AABBNode CurrentObject)
{
float DistanceLeft = -1;
float DistanceRight = -1;
DistanceLeft = DistanceApproximation.DistanceTaxiCab(CurrentObject.Left.myBox.Center, myNewNode.myBox.Center);
DistanceRight = DistanceApproximation.DistanceTaxiCab(CurrentObject.Right.myBox.Center, myNewNode.myBox.Center);
AABBNode Closest;
if (DistanceLeft >= DistanceRight)
{
CurrentObject = CurrentObject.Right;
}
else
{
CurrentObject = CurrentObject.Left;
}
return(CurrentObject);
}
//Insert new nodeToAdd to node tree, creating new parent if needed and reassigning hierarchy
private AABBNode InsertNode(AABBNode currentNode, AABBNode nodeToAdd)
{
AABBNode newParent = new AABBNode(new AABB(currentNode.nodeBounds, nodeToAdd.nodeBounds));
AABBNode oldParent = null;
if (currentNode.parent != null)
{
oldParent = currentNode.parent;
if (currentNode == currentNode.parent.left)
{
currentNode.parent.left = newParent;
}
else
{
currentNode.parent.right = newParent;
}
}
else
{
root = newParent;
}
newParent.left = currentNode;
newParent.right = nodeToAdd;
currentNode.parent = newParent;
nodeToAdd.parent = newParent;
if (oldParent != null)
{
newParent.parent = oldParent;
}
AdjustSizeOfOne(newParent);
return(newParent);
//MassAdjustment
}
//check if newParents sibling makes smaller AABB with newParents parent's sibling than current newParents parent AABB, and if it does restructure tree accordingly
private void Balance(AABBNode newParent)
{
if (leaves.Count < 4)
{
return;
}
while (AABB.GetSize(newParent.GetSibling().nodeBounds, newParent.parent.GetSibling().nodeBounds) < AABB.GetSize(newParent.parent.nodeBounds))
{
InsertNode(newParent.parent.GetSibling(), newParent.GetSibling());
if (newParent.parent.parent.left == newParent.parent)
{
newParent.parent.parent.left = newParent;
}
else
{
newParent.parent.parent.right = newParent;
}
newParent.parent = newParent.parent.parent;
newParent = newParent.parent;
}
}
private void TraceRecursive(int nodeIndex, Vector3 start, Vector3 dir, ref MeshRay ray)
{
AABBNode node = Nodes[nodeIndex];
if (node.Faces == null)
{
// find closest node
AABBNode leftChild = Nodes[node.Children + 0];
AABBNode rightChild = Nodes[node.Children + 1];
float[] dist = new float[] { float.PositiveInfinity, float.PositiveInfinity };
IntersectRayAABB(start, dir, leftChild.Bounds.Min, leftChild.Bounds.Max, out dist[0]);
IntersectRayAABB(start, dir, rightChild.Bounds.Min, rightChild.Bounds.Max, out dist[1]);
int closest = 0;
int furthest = 1;
if (dist[1] < dist[0])
{
closest = 1;
furthest = 0;
}
if (dist[closest] < ray.distance)
{
TraceRecursive(node.Children + closest, start, dir, ref ray);
}
if (dist[furthest] < ray.distance)
{
TraceRecursive(node.Children + furthest, start, dir, ref ray);
}
}
else
{
float t, u, v, w, s;
for (int i = 0; i < node.Faces.Length; ++i)
{
int indexStart = node.Faces[i] * 3;
Vector3 a = Vertices[Indices[indexStart + 0]];
Vector3 b = Vertices[Indices[indexStart + 1]];
Vector3 c = Vertices[Indices[indexStart + 2]];
if (IntersectRayTriTwoSided(start, dir, a, b, c, out t, out u, out v, out w, out s))
{
if (t < ray.distance)
{
ray.distance = t;
ray.u = u;
ray.v = v;
ray.w = w;
ray.faceSign = s;
ray.faceIndex = node.Faces[i];
}
}
}
}
}
//return out of children of currentNode node that makes smallest AABB with nodeToAdd
public AABBNode CheckSize(AABBNode nodeToAdd, AABBNode currentNode)
{
return(AABB.GetSize(nodeToAdd.nodeBounds, currentNode.left.nodeBounds) < AABB.GetSize(nodeToAdd.nodeBounds, currentNode.right.nodeBounds) ? currentNode.left : currentNode.right);
}
internal void Clear()
{
myNodes.Clear();
Root = null;
}
/// <summary>
/// Removes the node containing data.
/// </summary>
/// <param name="data"></param>
public void Remove(T data)
{
AABBNode node = FindNode(data);
RemoveNode(node);
}
/// <summary>
/// Removes the node with the bounds. If two nodes have the exact same bounds only the first one found will be removed.
/// </summary>
/// <param name="bounds"></param>
public void Remove(Bounds bounds)
{
AABBNode node = FindNode(bounds);
RemoveNode(node);
}
/// <summary>
/// Destroys all nodes in the tree.
/// </summary>
public void Clear()
{
// All we need to do is remove the root node reference. The garbage collector will do the rest.
m_RootNode = null;
}
