/// <summary> /// Query an AABB for overlapping proxies. The callback class is called for each proxy that overlaps the supplied /// AABB. /// </summary> /// <param name="callback">The callback.</param> /// <param name="aabb">The AABB.</param> public void Query(Func <int, bool> callback, ref Aabb aabb) { queryStack.Clear(); queryStack.Push(root); while (queryStack.Count > 0) { int nodeId = queryStack.Pop(); if (nodeId == NullNode) { continue; } TreeNode <T> node = nodes[nodeId]; if (Aabb.TestOverlap(ref node.Aabb, ref aabb)) { if (node.IsLeaf()) { bool proceed = callback(nodeId); if (!proceed) { return; } } else { queryStack.Push(node.Child1); queryStack.Push(node.Child2); } } } }
/// <summary>Test overlap of fat AABBs.</summary> /// <param name="proxyIdA">The proxy id A.</param> /// <param name="proxyIdB">The proxy id B.</param> /// <returns></returns> public bool TestOverlap(int proxyIdA, int proxyIdB) { tree.GetFatAabb(proxyIdA, out Aabb aabbA); tree.GetFatAabb(proxyIdB, out Aabb aabbB); return(Aabb.TestOverlap(ref aabbA, ref aabbB)); }
/// <summary> /// 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. /// </summary> /// <param name="callback">A callback class that is called for each proxy that is hit by the ray.</param> /// <param name="input">The ray-cast input data. The ray extends from p1 to p1 + maxFraction * (p2 - p1).</param> public void RayCast(Func <RayCastInput, int, float> callback, ref RayCastInput input) { Vector2 p1 = input.Point1; Vector2 p2 = input.Point2; Vector2 r = p2 - p1; Debug.Assert(r.LengthSquared() > 0.0f); r = Vector2.Normalize(r); // v is perpendicular to the segment. Vector2 absV = MathUtils.Abs(new Vector2(-r.Y, r.X)); //Velcro: Inlined the 'v' variable // 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); } raycastStack.Clear(); raycastStack.Push(root); while (raycastStack.Count > 0) { int nodeId = raycastStack.Pop(); if (nodeId == NullNode) { continue; } TreeNode <T> node = nodes[nodeId]; if (!Aabb.TestOverlap(ref node.Aabb, ref segmentAabb)) { continue; } // Separating axis for segment (Gino, p80). // |dot(v, p1 - c)| > dot(|v|, h) Vector2 c = node.Aabb.Center; Vector2 h = node.Aabb.Extents; float separation = Math.Abs(Vector2.Dot(new Vector2(-r.Y, r.X), p1 - c)) - Vector2.Dot(absV, h); if (separation > 0.0f) { continue; } if (node.IsLeaf()) { RayCastInput subInput; subInput.Point1 = input.Point1; subInput.Point2 = input.Point2; subInput.MaxFraction = maxFraction; float value = callback(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 { raycastStack.Push(node.Child1); raycastStack.Push(node.Child2); } } }