/// 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. public void RayCast(IRayCastEnabled callback, RayCastInput input) { Vec2 p1 = input.P1; Vec2 p2 = input.P2; Vec2 r = p2 - p1; Box2DXDebug.Assert(r.LengthSquared() > 0.0f); r.Normalize(); // v is perpendicular to the segment. Vec2 v = Vec2.Cross(1.0f, r); Vec2 abs_v = Math.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(); { Vec2 t = p1 + maxFraction * (p2 - p1); segmentAABB.LowerBound = Math.Min(p1, t); segmentAABB.UpperBound = Math.Max(p1, t); } const int k_stackSize = 128; int[] stack = new int[k_stackSize]; int count = 0; stack[count++] = _root; while (count > 0) { int nodeId = stack[--count]; if (nodeId == NullNode) { continue; } DynamicTreeNode node = _nodes[nodeId]; if (Collision.TestOverlap(node.Aabb, segmentAABB) == false) { continue; } // Separating axis for segment (Gino, p80). // |dot(v, p1 - c)| > dot(|v|, h) Vec2 c = node.Aabb.GetCenter(); Vec2 h = node.Aabb.GetExtents(); float separation = Math.Abs(Vec2.Dot(v, p1 - c)) - Vec2.Dot(abs_v, h); if (separation > 0.0f) { continue; } if (node.IsLeaf()) { RayCastInput subInput = new RayCastInput(); subInput.P1 = input.P1; subInput.P2 = input.P2; subInput.MaxFraction = maxFraction; maxFraction = callback.RayCastCallback(subInput, nodeId); if (maxFraction == 0.0f) { return; } // Update segment bounding box. { Vec2 t = p1 + maxFraction * (p2 - p1); segmentAABB.LowerBound = Math.Min(p1, t); segmentAABB.UpperBound = Math.Max(p1, t); } } else { Box2DXDebug.Assert(count + 1 < k_stackSize); stack[count++] = node.Child1; stack[count++] = node.Child2; } } }