protected void RemoveNode(IAABBNode nodeData)
{
IAABBNode LastModified;
if (nodeData != Root)
{
if (nodeData.Parent.Left == nodeData)
{
nodeData.Parent.Left = null;
}
else if (nodeData.Parent.Right == nodeData)
{
nodeData.Parent.Right = null;
}
var Parent = nodeData.Parent;
RemoveInvalidParents(ref Parent, out LastModified);
if (LastModified != Root && !CheckValidNode(LastModified))
{
throw new Exception("AABBTree Node became invalid after adding and remove.");
}
else if (LastModified == Root && !CheckValidNode(LastModified))
{
Root = null;
}
}
else
{
RemoveInvalidParents(ref Root, out LastModified);
if (Root != null && !CheckValidNode(Root))
{
throw new Exception("AABBTree Node became invalid after adding and remove.");
}
}
}
private int RemoveInvalidParents(IAABBNode NodeBeingRemoved, out IAABBNode 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);
}
private void MergeIntoBranch(IAABBNode myNewNode, IAABBNode currentObject)
{
IAABBNode myNewBranch = new AABBStaticNode();
myNewBranch.Bounds = BoundingBox.CreateMerged(myNewNode.Bounds, currentObject.Bounds);
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;
}
public SharedAABBTree GetAreaAsChildTree(BoundingBox Areabounds)
{
IAABBNode CurrentObject = Root;
IAABBNode MinimalSharedBranch = GetMinimalBranch(Areabounds);
return(new SharedAABBTree(this, MinimalSharedBranch));
}
public RayCastHit <T> BoundingPrimativeCast <T>(IBoundingShape myTestBox)
{
if (Root == null)
{
return(new RayCastHit <T>(new List <T>()));
}
List <T> myResults = new List <T>();
Stack <IAABBNode> NodesToExplore = new Stack <IAABBNode>();
IAABBNode Current = Root;
if (Current.ContainsData())
{
if (Current.Bounds.Contains(myTestBox) != ContainmentType.Disjoint)
{
TryDataConvert <T>(myResults, Current);
}
return(new RayCastHit <T>(myResults));
}
while (Current != null)
{
ContainmentType left = ContainmentType.Disjoint;
ContainmentType right = ContainmentType.Disjoint;
if (Current.Left != null)
{
left = myTestBox.Contains(Current.Left.Bounds);
}
if (Current.Right != null)
{
right = myTestBox.Contains(Current.Right.Bounds);
}
if (left == ContainmentType.Contains || left == ContainmentType.Intersects) //So it hit.
{
if (Current.Left.ContainsData())
{
TryDataConvert <T>(myResults, Current.Left);
}
else
{
NodesToExplore.Push(Current.Left);
}
}
if (right == ContainmentType.Contains || right == ContainmentType.Intersects) //So it hit.
{
if (Current.Right.ContainsData())
{
TryDataConvert <T>(myResults, Current.Right);
}
else
{
NodesToExplore.Push(Current.Right);
}
}
if (NodesToExplore.Count == 0)
{
break;
}
Current = NodesToExplore.Pop();
}
return(new RayCastHit <T>(myResults));
}
public T GetObjectDataContainingPoint <T>(Vector3 Point)
{
Stack <IAABBNode> NodesToExplore = new Stack <IAABBNode>();
IAABBNode Current = Root;
if (Current.ContainsData())
{
if (Current.Bounds.Contains(Point) == ContainmentType.Contains)
{
return((T)((IAABBTreeDataHolder)Current).GetInternalData());
}
}
while (Current != null)
{
if (Current.Bounds.Contains(Point) == ContainmentType.Contains)//So it hit.
{
if (Current.Left.ContainsData() && Current.Left is IAABBTreeDataHolder)
{
if (Current.Left.Bounds.Center == Point)
{
return((T)((IAABBTreeDataHolder)Current.Left).GetInternalData());
}
else
{
return(default(T));
}
}
else
{
NodesToExplore.Push(Current.Left);
}
}
if (Current.Bounds.Contains(Point) == ContainmentType.Contains) //So it hit.
{
if (Current.Right.ContainsData() && Current.Right is IAABBTreeDataHolder)
{
if (Current.Right.Bounds.Center == Point)
{
return((T)((IAABBTreeDataHolder)Current.Right).GetInternalData());
}
else
{
return(default(T));
}
}
else
{
NodesToExplore.Push(Current.Right);
}
}
if (NodesToExplore.Count == 0)
{
break;
}
Current = NodesToExplore.Pop();
}
return(default(T));
}
private static void ResizeBranch(IAABBNode myNewNode, IAABBNode CurrentObject)
{
var ContainmentTypeReturned = CurrentObject.Bounds.Contains(myNewNode.Bounds);
if (ContainmentTypeReturned == ContainmentType.Intersects || ContainmentTypeReturned == ContainmentType.Disjoint)
{
CurrentObject.Bounds = BoundingBox.CreateMerged(myNewNode.Bounds, CurrentObject.Bounds);
}
}
public RayCastHit <T> RaycastReturnFirst <T>(Ray aRay)
{
if (Root == null)
{
return(new RayCastHit <T>(new List <T>()));
}
List <T> myResults = new List <T>();
Stack <IAABBNode> NodesToExplore = new Stack <IAABBNode>();
IAABBNode Current = Root;
if (Current.ContainsData())
{
if (aRay.Intersects(Current.Bounds).HasValue)
{
TryDataConvert <T>(myResults, Current);
return(new RayCastHit <T>(myResults));
}
}
while (Current != null)
{
if (aRay.Intersects(Current.Left.Bounds).HasValue) //So it hit.
{
if (Current.Left.ContainsData() && Current.Left is IAABBTreeDataHolder)
{
TryDataConvert <T>(myResults, Current.Left);
break;
}
else
{
NodesToExplore.Push(Current.Left);
}
}
if (aRay.Intersects(Current.Right.Bounds).HasValue) //So it hit.
{
if (Current.Right.ContainsData() && Current.Right is IAABBTreeDataHolder)
{
TryDataConvert <T>(myResults, Current.Right);
break;
}
else
{
NodesToExplore.Push(Current.Right);
}
}
if (NodesToExplore.Count == 0)
{
break;
}
Current = NodesToExplore.Pop();
}
return(new RayCastHit <T>(myResults));
}
private int RemoveInvalidParents(ref IAABBNode NodeParent, out IAABBNode lastModifiedNode)
{
int Count = 0;
lastModifiedNode = NodeParent;
while (!CheckValidNode(NodeParent) && NodeParent.Parent != null)
{
lastModifiedNode = NodeParent;
NodeParent = NodeParent.Parent;
Count++;
}
return(Count);
}
protected IAABBNode GetMinimalBranch(IBoundingShape areabounds)
{
if (Root == null)
{
return(null);
}
IAABBNode CurrentMinimalObject = Root;
Stack <IAABBNode> LastChecked = new Stack <IAABBNode>();
while (CurrentMinimalObject != null)
{
ContainmentType leftContainment = ContainmentType.Disjoint;
ContainmentType rightContainment = ContainmentType.Disjoint;
if (CurrentMinimalObject.Left != null)
{
leftContainment = CurrentMinimalObject.Left.Bounds.Contains(areabounds);
}
if (CurrentMinimalObject.Right != null)
{
rightContainment = CurrentMinimalObject.Right.Bounds.Contains(areabounds);
}
else if (CurrentMinimalObject.Left == null)
{
break;
}
if (leftContainment == ContainmentType.Contains || (leftContainment == ContainmentType.Intersects && rightContainment == ContainmentType.Disjoint))
{
CurrentMinimalObject = CurrentMinimalObject.Left;
}
else if (rightContainment == ContainmentType.Contains || (rightContainment == ContainmentType.Intersects && leftContainment == ContainmentType.Disjoint))
{
CurrentMinimalObject = CurrentMinimalObject.Right;
}
else if (leftContainment == ContainmentType.Intersects && rightContainment == ContainmentType.Intersects)
{
break;
}
else
{
throw new Exception("Tree was left in unclean state.");
}
}
return(CurrentMinimalObject);
}
private void Insert(IAABBNode myNewNode)
{
if (Root == null)
{
Root = myNewNode;
return;
}
IAABBNode 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 = null;
ContainmentType?rightContainment = null;
if (CurrentObject.Left != null)
{
leftContainment = CurrentObject.Left.Bounds.Contains(myNewNode.Bounds);
}
if (CurrentObject.Right != null)
{
rightContainment = CurrentObject.Right.Bounds.Contains(myNewNode.Bounds);
}
if (leftContainment == ContainmentType.Contains || !rightContainment.HasValue || (leftContainment == ContainmentType.Intersects && rightContainment == ContainmentType.Disjoint))
{
CurrentObject = CurrentObject.Left;
}
else if (rightContainment == ContainmentType.Contains || !leftContainment.HasValue || (rightContainment == ContainmentType.Intersects && leftContainment == ContainmentType.Disjoint))
{
CurrentObject = CurrentObject.Right;
}
else if (CurrentObject.Left != null && CurrentObject.Right != null)
{
CurrentObject = GetNextByHeuristic(myNewNode, CurrentObject);
}
}
}
}
private bool CheckValidNode(IAABBNode lastModified)
{
if (lastModified.ContainsData())
{
if (lastModified.Left != null || lastModified.Right != null)
{
return(false);
}
}
else
{
if (lastModified.Left == null && lastModified.Right == null)
{
return(false);
}
}
return(true);
}
private static IAABBNode GetNextByHeuristice(IAABBNode CurrentObject, Vector3 areabounds)
{
float DistanceLeft = -1;//TODO: Refactor this
float DistanceRight = -1;
DistanceLeft = DistanceApproximation.DistanceTaxiCab(CurrentObject.Left.Bounds.Center, areabounds);
DistanceRight = DistanceApproximation.DistanceTaxiCab(CurrentObject.Right.Bounds.Center, areabounds);
if (DistanceLeft >= DistanceRight)
{
CurrentObject = CurrentObject.Right;
}
else
{
CurrentObject = CurrentObject.Left;
}
return(CurrentObject);
}
protected void ReinitializeTreeFromRootItem(bool DynamicOnly = false)
{
if (IsReadonly)
{
throw new Exception("Someone was either stuck processing or didn't properly dispose of Enumerator for AABBTree.");
}
RebuildDynamicOnly = DynamicOnly;
if (DynamicOnly)
{
foreach (var A in DynamicNodeLookup)
{
RemoveNode(A.Value);
}
DynamicNodeLookup.Clear();
}
IAABBNode Current = Root;
Queue <IAABBNode> NextToExamine = new Queue <IAABBNode>();
while (Current != null)
{
Current.Visit(this);
if (Current.Left != null)
{
NextToExamine.Enqueue(Current.Left);
}
if (Current.Right != null)
{
NextToExamine.Enqueue(Current.Right);
}
if (NextToExamine.Count != 0)
{
Current = NextToExamine.Dequeue();
}
else
{
break;
}
}
}
private IEnumerable <IAABBNode> IterateDataNodes(DataPredicateTest myTestPredicate, IAABBNode Current)
{
Stack <IAABBNode> NodesToExplore = new Stack <IAABBNode>();
if (Current.ContainsData())
{
if (myTestPredicate(Current))
{
yield return(Current);
}
yield break;
}
while (Current != null)
{
if (myTestPredicate(Current.Left)) //So it hit.
{
if (Current.Left.ContainsData() && Current.Left is IAABBTreeDataHolder)
{
yield return(Current.Left);
}
else
{
NodesToExplore.Push(Current.Left);
}
}
if (myTestPredicate(Current.Right)) //So it hit.
{
if (Current.Right.ContainsData() && Current.Right is IAABBTreeDataHolder)
{
yield return(Current.Right);
}
else
{
NodesToExplore.Push(Current.Right);
}
}
if (NodesToExplore.Count == 0)
{
break;
}
Current = NodesToExplore.Pop();
}
}
private static void TryDataConvert <T>(List <T> myResults, IAABBNode Current)
{
var Test = (T)((IAABBTreeDataHolder)Current).GetInternalData();
myResults.Add(Test);
}
private void TryDataConvert <T>(Stack <T> mostClosestItem, IAABBNode Current)
{
var Test = (T)((IAABBTreeDataHolder)Current).GetInternalData();
mostClosestItem.Push(Test);
}
public List <T> GetNearest <T>(Vector3 position, uint Count, float MaxDistance)
{
if (Count == 0)
{
throw new Exception("Attempted to perform get nearest search with a return count set to 0");
}
if (Root == null)
{
return(new List <T>());
}
List <T> myResults = new List <T>((int)Count);
Stack <IAABBNode> NodesToExplore = new Stack <IAABBNode>();
Stack <T> MostClosestItem = new Stack <T>();
IAABBNode Current = Root;
if (Current == null)
{
return(new List <T>());
}
if (Current.ContainsData())
{
if (IsWithinRangeOfPosition(position, MaxDistance, Current.Bounds))
{
TryDataConvert(myResults, Current);
}
else
{
return(null);
}
}
while (Current != null)
{
if (Current.Left != null && (Current.Right == null || !IsWithinRangeOfPosition(position, MaxDistance, Current.Right.Bounds)))
{
if (IsWithinRangeOfPosition(position, MaxDistance, Current.Left.Bounds))
{
if ((Current.Left.ContainsData())) //this only happens if we're at the bottom of the tree.
{
TryDataConvert <T>(MostClosestItem, Current.Left);
}
else
{
NodesToExplore.Push(Current.Left);
}
}
}
else if (Current.Right != null && (Current.Left == null || !IsWithinRangeOfPosition(position, MaxDistance, Current.Left.Bounds)))
{
if (IsWithinRangeOfPosition(position, MaxDistance, Current.Right.Bounds))
{
if ((Current.Right.ContainsData())) //this only happens if we're at the bottom of the tree.
{
TryDataConvert <T>(MostClosestItem, Current.Right);
}
else
{
NodesToExplore.Push(Current.Right);
}
}
}
else if (Current.Left != null && Current.Right != null)
{
if (Current.Left.ContainsData() && Current.Right.ContainsData())
{
TryDataConvert <T>(MostClosestItem, Current.Left);
TryDataConvert <T>(MostClosestItem, Current.Right);
}
else if (Current.Left.ContainsData())
{
TryDataConvert <T>(MostClosestItem, Current.Left);
NodesToExplore.Push(Current.Right);
}
else if (Current.Right.ContainsData())
{
TryDataConvert <T>(MostClosestItem, Current.Right);
NodesToExplore.Push(Current.Left);
}
else
{ //pushes nearest node into the stack first so it pop first.
if (Vector3.Distance(Current.Right.Bounds.Center, position) > Vector3.Distance(Current.Left.Bounds.Center, position))
{
NodesToExplore.Push(Current.Right);
NodesToExplore.Push(Current.Left);
}
else
{
NodesToExplore.Push(Current.Left);
NodesToExplore.Push(Current.Right);
}
}
}
if (NodesToExplore.Count == 0 || (MostClosestItem.Count == Count))
{
break;
}
Current = NodesToExplore.Pop();
}
if (Count > 0)
{
int Counter = (int)Math.Min(Count, MostClosestItem.Count);
for (int i = 0; i < Counter; i++)
{
myResults.Add(MostClosestItem.Pop());
}
}
else
{
int Counter = MostClosestItem.Count;
for (int i = 0; i < Counter; i++)
{
myResults.Add(MostClosestItem.Pop());
}
}
return(myResults);
}
private IAABBNode GetNextByHeuristic(IAABBNode myNewNode, IAABBNode CurrentObject)
{
var areabounds = myNewNode.Bounds.Center;
return(GetNextByHeuristice(CurrentObject, areabounds));
}
public StaticAABBTree(IAABBNode RootNode, BoundingBox areabounds, Predicate <object> shouldEliminateItemCheck = null) : base(RootNode)
{
this.areabounds = areabounds;
RebuildStaticObjects = true;
ReinitializeTreeFromRootItem();
}
public SimpleAABBTree(IAABBNode minimalSharedBranch) : base(minimalSharedBranch)
{
ReinitializeTreeFromRootItem();
}
protected AABBTreeBase(IAABBNode RootNode)
{
this.Root = RootNode;
}
public SharedAABBTree(SimpleAABBTree ParentObject, IAABBNode minimalSharedBranch) : base(minimalSharedBranch)
{
this.ParentObject = ParentObject;
}
