/// <summary> /// Describes whether this instance synchronize /// </summary> /// <param name="broadPhase">The broad phase</param> /// <param name="transform1">The transform</param> /// <param name="transform2">The transform</param> /// <returns>The bool</returns> internal bool Synchronize(BroadPhase broadPhase, XForm transform1, XForm transform2) { if (ProxyId == PairManager.NullProxy) { return(false); } // Compute an AABB that covers the swept shape (may miss some rotation effect). Aabb aabb1, aabb2; Shape.ComputeAabb(out aabb1, transform1); Shape.ComputeAabb(out aabb2, transform2); Aabb aabb = new Aabb(); aabb.Combine(aabb1, aabb2); if (broadPhase.InRange(aabb)) { broadPhase.MoveProxy(ProxyId, aabb); return(true); } return(false); }
/// <summary> /// Validates the metrics using the specified index /// </summary> /// <param name="index">The index</param> public void ValidateMetrics(int index) { if (index == NullNode) { return; } TreeNode <T> node = nodes[index]; int child1 = node.Child1; int child2 = node.Child2; if (node.IsLeaf()) { Debug.Assert(child1 == NullNode); Debug.Assert(child2 == NullNode); Debug.Assert(node.Height == 0); return; } Debug.Assert(0 <= child1 && child1 < nodeCapacity); Debug.Assert(0 <= child2 && child2 < nodeCapacity); int height1 = nodes[child1].Height; int height2 = nodes[child2].Height; int height = 1 + Math.Max(height1, height2); Debug.Assert(node.Height == height); Aabb aabb = new Aabb(); aabb.Combine(ref nodes[child1].Aabb, ref nodes[child2].Aabb); Debug.Assert(aabb.LowerBound == node.Aabb.LowerBound); Debug.Assert(aabb.UpperBound == node.Aabb.UpperBound); ValidateMetrics(child1); ValidateMetrics(child2); }
/// <summary> /// Inserts the leaf using the specified leaf /// </summary> /// <param name="leaf">The leaf</param> private void InsertLeaf(int leaf) { if (root == NullNode) { root = leaf; nodes[root].ParentOrNext = NullNode; return; } // Find the best sibling for this node Aabb leafAabb = nodes[leaf].Aabb; int index = root; while (!nodes[index].IsLeaf()) { int child1 = nodes[index].Child1; int child2 = nodes[index].Child2; float area = nodes[index].Aabb.Perimeter; Aabb combinedAabb = new Aabb(); combinedAabb.Combine(ref nodes[index].Aabb, ref leafAabb); float combinedArea = combinedAabb.Perimeter; // Cost of creating a new parent for this node and the new leaf float cost = 2.0f * combinedArea; // Minimum cost of pushing the leaf further down the tree float inheritanceCost = 2.0f * (combinedArea - area); // Cost of descending into child1 float cost1; if (nodes[child1].IsLeaf()) { Aabb aabb = new Aabb(); aabb.Combine(ref leafAabb, ref nodes[child1].Aabb); cost1 = aabb.Perimeter + inheritanceCost; } else { Aabb aabb = new Aabb(); aabb.Combine(ref leafAabb, ref nodes[child1].Aabb); float oldArea = nodes[child1].Aabb.Perimeter; float newArea = aabb.Perimeter; cost1 = newArea - oldArea + inheritanceCost; } // Cost of descending into child2 float cost2; if (nodes[child2].IsLeaf()) { Aabb aabb = new Aabb(); aabb.Combine(ref leafAabb, ref nodes[child2].Aabb); cost2 = aabb.Perimeter + inheritanceCost; } else { Aabb aabb = new Aabb(); aabb.Combine(ref leafAabb, ref nodes[child2].Aabb); float oldArea = nodes[child2].Aabb.Perimeter; float newArea = aabb.Perimeter; cost2 = newArea - oldArea + inheritanceCost; } // Descend according to the minimum cost. if (cost < cost1 && cost1 < cost2) { break; } // Descend if (cost1 < cost2) { index = child1; } else { index = child2; } } int sibling = index; // Create a new parent. int oldParent = nodes[sibling].ParentOrNext; int newParent = AllocateNode(); nodes[newParent].ParentOrNext = oldParent; nodes[newParent].UserData = default(T?); nodes[newParent].Aabb.Combine(ref leafAabb, ref nodes[sibling].Aabb); nodes[newParent].Height = nodes[sibling].Height + 1; if (oldParent != NullNode) { // The sibling was not the root. if (nodes[oldParent].Child1 == sibling) { nodes[oldParent].Child1 = newParent; } else { nodes[oldParent].Child2 = newParent; } nodes[newParent].Child1 = sibling; nodes[newParent].Child2 = leaf; nodes[sibling].ParentOrNext = newParent; nodes[leaf].ParentOrNext = newParent; } else { // The sibling was the root. nodes[newParent].Child1 = sibling; nodes[newParent].Child2 = leaf; nodes[sibling].ParentOrNext = newParent; nodes[leaf].ParentOrNext = newParent; root = newParent; } // Walk back up the tree fixing heights and AABBs index = nodes[leaf].ParentOrNext; while (index != NullNode) { index = Balance(index); int child1 = nodes[index].Child1; int child2 = nodes[index].Child2; Debug.Assert(child1 != NullNode); Debug.Assert(child2 != NullNode); nodes[index].Height = 1 + Math.Max(nodes[child1].Height, nodes[child2].Height); nodes[index].Aabb.Combine(ref nodes[child1].Aabb, ref nodes[child2].Aabb); index = nodes[index].ParentOrNext; } //Validate(); }
/// <summary>Build an optimal tree. Very expensive. For testing.</summary> public void RebuildBottomUp() { int[] nodes = new int[nodeCount]; int count = 0; // Build array of leaves. Free the rest. for (int i = 0; i < nodeCapacity; ++i) { if (this.nodes[i].Height < 0) { // free node in pool continue; } if (this.nodes[i].IsLeaf()) { this.nodes[i].ParentOrNext = NullNode; nodes[count] = i; ++count; } else { FreeNode(i); } } while (count > 1) { float minCost = MathConstants.MaxFloat; int iMin = -1, jMin = -1; for (int i = 0; i < count; ++i) { Aabb aabBi = this.nodes[nodes[i]].Aabb; for (int j = i + 1; j < count; ++j) { Aabb aabBj = this.nodes[nodes[j]].Aabb; Aabb b = new Aabb(); b.Combine(ref aabBi, ref aabBj); float cost = b.Perimeter; if (cost < minCost) { iMin = i; jMin = j; minCost = cost; } } } int index1 = nodes[iMin]; int index2 = nodes[jMin]; TreeNode <T> child1 = this.nodes[index1]; TreeNode <T> child2 = this.nodes[index2]; int parentIndex = AllocateNode(); TreeNode <T> parent = this.nodes[parentIndex]; parent.Child1 = index1; parent.Child2 = index2; parent.Height = 1 + Math.Max(child1.Height, child2.Height); parent.Aabb.Combine(ref child1.Aabb, ref child2.Aabb); parent.ParentOrNext = NullNode; child1.ParentOrNext = parentIndex; child2.ParentOrNext = parentIndex; nodes[jMin] = nodes[count - 1]; nodes[iMin] = parentIndex; --count; } root = nodes[0]; Validate(); }