/// Query an AABB for overlapping proxies. The callback class /// is called for each proxy that overlaps the supplied AABB. public void Query(Func <int, bool> callback, ref AABB aabb) { stack.Clear(); stack.Push(_root); while (stack.Count > 0) { int nodeId = stack.Pop(); if (nodeId == NullNode) { continue; } DynamicTreeNode node = _nodes[nodeId]; if (AABB.TestOverlap(ref node.aabb, ref aabb)) { if (node.IsLeaf()) { bool proceed = callback(nodeId); if (!proceed) { return; } } else { stack.Push(node.child1); stack.Push(node.child2); } } } }
private int CountLeaves(int nodeId) { if (nodeId == NullNode) { return(0); } //Debug.Assert(0 <= nodeId && nodeId < _nodeCapacity); DynamicTreeNode node = _nodes[nodeId]; if (node.IsLeaf()) { //Debug.Assert(node.leafCount == 1); return(1); } int count1 = CountLeaves(node.child1); int count2 = CountLeaves(node.child2); int count = count1 + count2; //Debug.Assert(count == node.leafCount); return(count); }
/// 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. internal void RayCast(RayCastCallbackInternal callback, ref RayCastInput input) { Vector2 p1 = input.p1; Vector2 p2 = input.p2; Vector2 r = p2 - p1; //Debug.Assert(r.sqrMagnitude > 0.0f); r.Normalize(); // v is perpendicular to the segment. Vector2 v = MathUtils.Cross(1.0f, r); Vector2 abs_v = MathUtils.Abs(v); // Separating axis for segment (Gino, p80). // |dot(v, p1 - c)| > dot(|v|, h) float maxFraction = input.maxFraction; // Build a bounding box for the segment. AABB segmentAABB = new AABB(); { Vector2 t = p1 + maxFraction * (p2 - p1); segmentAABB.lowerBound = Vector2.Min(p1, t); segmentAABB.upperBound = Vector2.Max(p1, t); } stack.Clear(); stack.Push(_root); while (stack.Count > 0) { int nodeId = stack.Pop(); if (nodeId == NullNode) { continue; } DynamicTreeNode node = _nodes[nodeId]; if (AABB.TestOverlap(ref node.aabb, ref segmentAABB) == false) { continue; } // Separating axis for segment (Gino, p80). // |dot(v, p1 - c)| > dot(|v|, h) Vector2 c = node.aabb.GetCenter(); Vector2 h = node.aabb.GetExtents(); float separation = Math.Abs(Vector2.Dot(v, p1 - c)) - Vector2.Dot(abs_v, h); if (separation > 0.0f) { continue; } if (node.IsLeaf()) { RayCastInput subInput; subInput.p1 = input.p1; subInput.p2 = input.p2; subInput.maxFraction = maxFraction; float value = callback(ref subInput, nodeId); if (value == 0.0f) { // the client has terminated the raycast. return; } if (value > 0.0f) { // Update segment bounding box. maxFraction = value; Vector2 t = p1 + maxFraction * (p2 - p1); segmentAABB.lowerBound = Vector2.Min(p1, t); segmentAABB.upperBound = Vector2.Max(p1, t); } } else { stack.Push(node.child1); stack.Push(node.child2); } } }