/** * Create a proxy with an initial AABB. Pairs are not reported until * UpdatePairs is called. */ public object CreateProxy(b2AABB aabb, object userData) { b2DynamicTreeNode proxy = m_tree.CreateProxy(aabb, userData); ++m_proxyCount; BufferMove(proxy); return(proxy); }
private void RemoveLeaf(b2DynamicTreeNode leaf) { if (leaf == m_root) { m_root = null; return; } b2DynamicTreeNode node2 = leaf.parent; b2DynamicTreeNode node1 = node2.parent; b2DynamicTreeNode sibling; if (node2.child1 == leaf) { sibling = node2.child2; } else { sibling = node2.child1; } if (node1 != null) { // Destroy node2 and connect node1 to sibling if (node1.child1 == node2) { node1.child1 = sibling; } else { node1.child2 = sibling; } sibling.parent = node1; FreeNode(node2); // Adjust the ancestor bounds while (node1 != null) { b2AABB oldAABB = node1.aabb; node1.aabb = b2AABB.CombineStatic(node1.child1.aabb, node1.child2.aabb); if (oldAABB.Contains(node1.aabb)) { break; } node1 = node1.parent; } } else { m_root = sibling; sibling.parent = null; FreeNode(node2); } }
/** * Query an AABB for overlapping proxies. The callback * is called for each proxy that overlaps the supplied AABB. * The callback should match function signature * <code>fuction callback(proxy:b2DynamicTreeNode):Boolean</code> * and should return false to trigger premature termination. */ public void Query(BroadPhaseQueryCallback callback, b2AABB aabb) { if (m_root == null) { return; } Dictionary <int, b2DynamicTreeNode> stack = new Dictionary <int, b2DynamicTreeNode>(); int count = 0; stack.Add(count++, m_root); while (count > 0) { b2DynamicTreeNode node = stack[--count]; if (node.aabb.TestOverlap(aabb)) { if (node.IsLeaf()) { bool proceed = callback(node); if (!proceed) { return; } } else { // No stack limit, so no assert int key; key = count++; if (stack.ContainsKey(key)) { stack.Remove(key); } stack.Add(key, node.child1); key = count++; if (stack.ContainsKey(key)) { stack.Remove(key); } stack.Add(key, node.child2); } } } }
private b2DynamicTreeNode AllocateNode() { // Peel a node off the free list if (m_freeList != null) { b2DynamicTreeNode node = m_freeList; m_freeList = node.parent; node.parent = null; node.child1 = null; node.child2 = null; return(node); } // Ignore length pool expansion and relocation found in the C++ // As we are using heap allocation return(new b2DynamicTreeNode()); }
/* * public function Dump(node:b2DynamicTreeNode=null, depth:int=0):void * { * if (!node) * { * node = m_root; * } * if (!node) return; * for (var i:int = 0; i < depth; i++) s += " "; * if (node.userData) * { * var ud:* = (node.userData as b2Fixture).GetBody().GetUserData(); * trace(s + ud); * }else { * trace(s + "-"); * } * if (node.child1) * Dump(node.child1, depth + 1); * if (node.child2) * Dump(node.child2, depth + 1); * } */ /** * Create a proxy. Provide a tight fitting AABB and a userData. */ public b2DynamicTreeNode CreateProxy(b2AABB aabb, object userData) { b2DynamicTreeNode node = AllocateNode(); // Fatten the aabb. float extendX = b2Settings.b2_aabbExtension; float extendY = b2Settings.b2_aabbExtension; node.aabb.lowerBound.x = aabb.lowerBound.x - extendX; node.aabb.lowerBound.y = aabb.lowerBound.y - extendY; node.aabb.upperBound.x = aabb.upperBound.x + extendX; node.aabb.upperBound.y = aabb.upperBound.y + extendY; node.userData = userData; InsertLeaf(node); return(node); }
/** * Perform some iterations to re-balance the tree. */ public void Rebalance(int iterations) { if (m_root == null) { return; } for (int i = 0; i < iterations; i++) { b2DynamicTreeNode node = m_root; uint bit = 0; while (node.IsLeaf() == false) { node = (((int)m_path >> (int)bit) & 1) > 0 ? node.child2 : node.child1; bit = (bit + 1) & 31; // 0-31 bits in a uint } ++m_path; RemoveLeaf(node); InsertLeaf(node); } }
/** * Move a proxy with a swept AABB. If the proxy has moved outside of its fattened AABB, * then the proxy is removed from the tree and re-inserted. Otherwise * the function returns immediately. */ public bool MoveProxy(b2DynamicTreeNode proxy, b2AABB aabb, b2Vec2 displacement) { b2Settings.b2Assert(proxy.IsLeaf()); if (proxy.aabb.Contains(aabb)) { return(false); } RemoveLeaf(proxy); // Extend AABB float extendX = b2Settings.b2_aabbExtension + b2Settings.b2_aabbMultiplier * (displacement.x > 0?displacement.x: -displacement.x); float extendY = b2Settings.b2_aabbExtension + b2Settings.b2_aabbMultiplier * (displacement.y > 0?displacement.y: -displacement.y); proxy.aabb.lowerBound.x = aabb.lowerBound.x - extendX; proxy.aabb.lowerBound.y = aabb.lowerBound.y - extendY; proxy.aabb.upperBound.x = aabb.upperBound.x + extendX; proxy.aabb.upperBound.y = aabb.upperBound.y + extendY; InsertLeaf(proxy); return(true); }
private void InsertLeaf(b2DynamicTreeNode leaf) { ++m_insertionCount; if (m_root == null) { m_root = leaf; m_root.parent = null; return; } b2Vec2 center = leaf.aabb.GetCenter(); b2DynamicTreeNode sibling = m_root; if (sibling.IsLeaf() == false) { do { b2DynamicTreeNode child1 = sibling.child1; b2DynamicTreeNode child2 = sibling.child2; //b2Vec2 delta1 = b2Abs(m_nodes[child1].aabb.GetCenter() - center); //b2Vec2 delta2 = b2Abs(m_nodes[child2].aabb.GetCenter() - center); //float32 norm1 = delta1.x + delta1.y; //float32 norm2 = delta2.x + delta2.y; float norm1 = Mathf.Abs((child1.aabb.lowerBound.x + child1.aabb.upperBound.x) / 2.0f - center.x) + Mathf.Abs((child1.aabb.lowerBound.y + child1.aabb.upperBound.y) / 2.0f - center.y); float norm2 = Mathf.Abs((child2.aabb.lowerBound.x + child2.aabb.upperBound.x) / 2.0f - center.x) + Mathf.Abs((child2.aabb.lowerBound.y + child2.aabb.upperBound.y) / 2.0f - center.y); if (norm1 < norm2) { sibling = child1; } else { sibling = child2; } }while (sibling.IsLeaf() == false); } // Create a parent for the siblings b2DynamicTreeNode node1 = sibling.parent; b2DynamicTreeNode node2 = AllocateNode(); node2.parent = node1; node2.userData = null; node2.aabb.Combine(leaf.aabb, sibling.aabb); if (node1 != null) { if (sibling.parent.child1 == sibling) { node1.child1 = node2; } else { node1.child2 = node2; } node2.child1 = sibling; node2.child2 = leaf; sibling.parent = node2; leaf.parent = node2; do { if (node1.aabb.Contains(node2.aabb)) { break; } node1.aabb.Combine(node1.child1.aabb, node1.child2.aabb); node2 = node1; node1 = node1.parent; }while (node1 != null); } else { node2.child1 = sibling; node2.child2 = leaf; sibling.parent = node2; leaf.parent = node2; m_root = node2; } }
private void FreeNode(b2DynamicTreeNode node) { node.parent = m_freeList; m_freeList = node; }
/** * Ray-cast against the proxies in the tree. This relies on the callback * to perform a exact ray-cast in the case were the proxy contains a shape. * The callback also performs the any collision filtering. This has performance * roughly equal to k * log(n), where k is the number of collisions and n is the * number of proxies in the tree. * @param input the ray-cast input data. The ray extends from p1 to p1 + maxFraction * (p2 - p1). * @param callback a callback class that is called for each proxy that is hit by the ray. * It should be of signature: * ----- <code>function callback(input:b2RayCastInput, proxy:*):void</code> * <code>function callback(input:b2RayCastInput, proxy:*):Number</code> */ public void RayCast(BroadPhaseRayCastCallback callback, b2RayCastInput input) { if (m_root == null) { return; } b2Vec2 p1 = input.p1; b2Vec2 p2 = input.p2; b2Vec2 r = b2Math.SubtractVV(p1, p2); //b2Settings.b2Assert(r.LengthSquared() > 0.0); r.Normalize(); // v is perpendicular to the segment b2Vec2 v = b2Math.CrossFV(1.0f, r); b2Vec2 abs_v = b2Math.AbsV(v); float maxFraction = input.maxFraction; // Build a bounding box for the segment b2AABB segmentAABB = new b2AABB(); float tX; float tY; { tX = p1.x + maxFraction * (p2.x - p1.x); tY = p1.y + maxFraction * (p2.y - p1.y); segmentAABB.lowerBound.x = Mathf.Min(p1.x, tX); segmentAABB.lowerBound.y = Mathf.Min(p1.y, tY); segmentAABB.upperBound.x = Mathf.Max(p1.x, tX); segmentAABB.upperBound.y = Mathf.Max(p1.y, tY); } Dictionary <int, b2DynamicTreeNode> stack = new Dictionary <int, b2DynamicTreeNode>(); int count = 0; stack[count++] = m_root; while (count > 0) { b2DynamicTreeNode node = stack[--count]; if (node.aabb.TestOverlap(segmentAABB) == false) { continue; } // Separating axis for segment (Gino, p80) // |dot(v, p1 - c)| > dot(|v|,h) b2Vec2 c = node.aabb.GetCenter(); b2Vec2 h = node.aabb.GetExtents(); float separation = Mathf.Abs(v.x * (p1.x - c.x) + v.y * (p1.y - c.y)) - abs_v.x * h.x - abs_v.y * h.y; if (separation > 0.0f) { continue; } if (node.IsLeaf()) { b2RayCastInput subInput = new b2RayCastInput(); subInput.p1 = input.p1; subInput.p2 = input.p2; //*************by kingBook 2015/10/22 16:17************* subInput.maxFraction = maxFraction; float value = callback(subInput, node); if (value == 0) { return; } if (value > 0) { //Update the segment bounding box maxFraction = value; //****************************************************** tX = p1.x + maxFraction * (p2.x - p1.x); tY = p1.y + maxFraction * (p2.y - p1.y); segmentAABB.lowerBound.x = Mathf.Min(p1.x, tX); segmentAABB.lowerBound.y = Mathf.Min(p1.y, tY); segmentAABB.upperBound.x = Mathf.Max(p1.x, tX); segmentAABB.upperBound.y = Mathf.Max(p1.y, tY); } } else { // No stack limit, so no assert stack[count++] = node.child1; stack[count++] = node.child2; } } }
/** * Get user data from a proxy. Returns null if the proxy is invalid. */ public object GetUserData(b2DynamicTreeNode proxy) { return(proxy.userData); }
public b2AABB GetFatAABB(b2DynamicTreeNode proxy) { return(proxy.aabb); }
/** * Destroy a proxy. This asserts if the id is invalid. */ public void DestroyProxy(b2DynamicTreeNode proxy) { //b2Settings.b2Assert(proxy.IsLeaf()); RemoveLeaf(proxy); FreeNode(proxy); }
private void UnBufferMove(b2DynamicTreeNode proxy) { m_moveBuffer.Remove(proxy); }
// Private /////////////// private void BufferMove(b2DynamicTreeNode proxy) { m_moveBuffer.Add(proxy); }