private int AllocateNode() { if (NextFreeNodeIndex == null) { NodeCapacity += GrowthSize; for (int nodeIndex = AllocateNodeCount; nodeIndex < NodeCapacity; nodeIndex++) { Nodes.Add(new AABBNode()); Nodes[nodeIndex].NextNodeIndex = nodeIndex + 1; } Nodes[NodeCapacity - 1].NextNodeIndex = null; NextFreeNodeIndex = AllocateNodeCount; } int nodeIndexv = NextFreeNodeIndex.Value; AABBNode allocatedNode = Nodes[nodeIndexv]; allocatedNode.ParentNodeIndex = null; allocatedNode.LeftNodeIndex = null; allocatedNode.RightNodeIndex = null; NextFreeNodeIndex = allocatedNode.NextNodeIndex; AllocateNodeCount++; return(nodeIndexv); }
public List <IAABB> QueryOverlaps(IAABB obj) { List <IAABB> overlaps = new List <IAABB>(); Stack <int?> stack = new Stack <int?>(); AABB testAabb = obj.GetAABB(); stack.Push(RootNodeIndex); while (stack.Any()) { int?nodeIndex = stack.Peek(); stack.Pop(); if (!nodeIndex.HasValue) { continue; } AABBNode node = Nodes[nodeIndex.Value].Clone() as AABBNode; if (node.aabb.Intersect(testAabb)) { if (node.IsLeaf() && node.Obj != obj) { overlaps.Add((IAABB)node.Obj); } else { stack.Push(node.LeftNodeIndex); stack.Push(node.RightNodeIndex); } } } return(overlaps); }
private void DeallocateNode(int nodeIndex) { AABBNode deallocatedNode = Nodes[nodeIndex]; deallocatedNode.NextNodeIndex = NextFreeNodeIndex; NextFreeNodeIndex = nodeIndex; AllocateNodeCount--; }
public void InsertObject(IAABB obj) { int nodeIndex = AllocateNode(); AABBNode node = Nodes[nodeIndex]; node.aabb = obj.GetAABB(); node.Obj = obj; InsertLeaf(nodeIndex); objectNodeIndexMap.Add(obj, nodeIndex); }
private void FixUpwardsTree(int?treeNodeIndex) { while (treeNodeIndex != null) { AABBNode treeNode = Nodes[treeNodeIndex.Value]; AABBNode leftNode = Nodes[treeNode.LeftNodeIndex.Value].Clone() as AABBNode; AABBNode rightNode = Nodes[treeNode.RightNodeIndex.Value].Clone() as AABBNode; treeNode.aabb = leftNode.aabb.Merge(rightNode.aabb); treeNodeIndex = treeNode.ParentNodeIndex; } }
private void UpdateLeaf(int leafNodeIndex, AABB newAABB) { AABBNode node = Nodes[leafNodeIndex]; if (node.aabb.Contains(newAABB)) { return; } RemoveLeaf(leafNodeIndex); node.aabb = newAABB; InsertLeaf(leafNodeIndex); }
private void RemoveLeaf(int leafNodeIndex) { if (leafNodeIndex == RootNodeIndex) { RootNodeIndex = null; return; } AABBNode leafNode = Nodes[leafNodeIndex]; int parentNodeIndex = leafNode.ParentNodeIndex.Value; AABBNode parentNode = Nodes[parentNodeIndex].Clone() as AABBNode; int? grandParentNodeIndex = parentNode.ParentNodeIndex; int? siblingNodeIndex = parentNode.LeftNodeIndex == leafNodeIndex ? parentNode.RightNodeIndex : parentNode.LeftNodeIndex; AABBNode siblingNode = Nodes[siblingNodeIndex.Value]; if (grandParentNodeIndex != null) { AABBNode grandParentNode = Nodes[grandParentNodeIndex.Value]; if (grandParentNode.LeftNodeIndex == parentNodeIndex) { grandParentNode.LeftNodeIndex = siblingNodeIndex; } else { grandParentNode.RightNodeIndex = siblingNodeIndex; } siblingNode.ParentNodeIndex = grandParentNodeIndex; DeallocateNode(parentNodeIndex); FixUpwardsTree(grandParentNodeIndex.Value); } else { RootNodeIndex = siblingNodeIndex; siblingNode.ParentNodeIndex = null; DeallocateNode(parentNodeIndex); } leafNode.ParentNodeIndex = null; }
private int GetMaxDepth(AABBNode node) { if (node == null) { return(0); } else { if (node.LeftNodeIndex == null && node.RightNodeIndex == null) { return(GetMaxDepth(Nodes[node.ParentNodeIndex.Value])); } if (node.LeftNodeIndex == null) { return(GetMaxDepth(Nodes[node.RightNodeIndex.Value])); } if (node.RightNodeIndex == null) { return(GetMaxDepth(Nodes[node.LeftNodeIndex.Value])); } int lDepth = GetMaxDepth(Nodes[node.LeftNodeIndex.Value]); int rDepth = GetMaxDepth(Nodes[node.RightNodeIndex.Value]); if (lDepth > rDepth) { return(lDepth + 1); } else { return(rDepth + 1); } } }
private void InsertLeaf(int leafNodeIndex) { if (RootNodeIndex == null) { RootNodeIndex = leafNodeIndex; return; } int treeNodeIndex = RootNodeIndex.Value; AABBNode leafNode = Nodes[leafNodeIndex]; while (!Nodes[treeNodeIndex].IsLeaf()) { AABBNode treeNode = Nodes[treeNodeIndex].Clone() as AABBNode; int leftNodeIndex = treeNode.LeftNodeIndex.Value; int rightNodeIndex = treeNode.RightNodeIndex.Value; AABBNode leftNode = Nodes[leftNodeIndex].Clone() as AABBNode; AABBNode rightNode = Nodes[rightNodeIndex].Clone() as AABBNode; AABB combinedAabb = treeNode.aabb.Merge(leafNode.aabb); double newParentNodeCost = 2.0 * combinedAabb.SurfaceArea; double minimumPushDownCost = 2.0 * (combinedAabb.SurfaceArea - treeNode.aabb.SurfaceArea); double costLeft; double costRight; if (leftNode.IsLeaf()) { costLeft = leafNode.aabb.Merge(leftNode.aabb).SurfaceArea + minimumPushDownCost; } else { AABB newLeftAabb = leafNode.aabb.Merge(leftNode.aabb); costLeft = (newLeftAabb.SurfaceArea - leftNode.aabb.SurfaceArea) + minimumPushDownCost; } if (rightNode.IsLeaf()) { costRight = leafNode.aabb.Merge(rightNode.aabb).SurfaceArea + minimumPushDownCost; } else { AABB newRightAabb = leafNode.aabb.Merge(rightNode.aabb); costRight = (newRightAabb.SurfaceArea - rightNode.aabb.SurfaceArea) + minimumPushDownCost; } if (newParentNodeCost < costRight) { treeNodeIndex = leftNodeIndex; } else { treeNodeIndex = rightNodeIndex; } } int leafSiblingIndex = treeNodeIndex; AABBNode leafSibling = Nodes[leafSiblingIndex]; int? oldParentIndex = leafSibling.ParentNodeIndex; int newParentIndex = AllocateNode(); AABBNode newParent = Nodes[newParentIndex]; newParent.ParentNodeIndex = oldParentIndex; newParent.aabb = leafNode.aabb.Merge(leafSibling.aabb); newParent.LeftNodeIndex = leafSiblingIndex; newParent.RightNodeIndex = leafNodeIndex; leafNode.ParentNodeIndex = newParentIndex; leafSibling.ParentNodeIndex = newParentIndex; if (oldParentIndex == null) { RootNodeIndex = newParentIndex; } else { AABBNode oldParent = Nodes[oldParentIndex.Value]; if (oldParent.LeftNodeIndex == leafSiblingIndex) { oldParent.LeftNodeIndex = newParentIndex; } else { oldParent.RightNodeIndex = newParentIndex; } } treeNodeIndex = leafNode.ParentNodeIndex.Value; FixUpwardsTree(treeNodeIndex); }