public void RayCast(b2WorldRayCastWrapper callback, b2RayCastInput input) { b2Vec2 p1 = input.p1; b2Vec2 p2 = input.p2; b2Vec2 r = p2 - p1; Debug.Assert(r.LengthSquared() > 0.0f); r.Normalize(); // v is perpendicular to the segment. b2Vec2 v = b2Math.b2Cross(1.0f, r); b2Vec2 abs_v = b2Math.b2Abs(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. b2AABB segmentAABB = new b2AABB(); { b2Vec2 t = p1 + maxFraction * (p2 - p1); segmentAABB.lowerBound = b2Math.b2Min(p1, t); segmentAABB.upperBound = b2Math.b2Max(p1, t); } Stack<int> stack = new Stack<int>(); stack.Push(m_root); while (stack.Count > 0) { int nodeId = stack.Pop(); if (nodeId == b2TreeNode.b2_nullNode) { continue; } b2TreeNode node = m_nodes[nodeId]; if (b2Collision.b2TestOverlap(ref node.aabb, ref 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 = b2Math.b2Abs(b2Math.b2Dot(v, p1 - c)) - b2Math.b2Dot(abs_v, h); if (separation > 0.0f) { continue; } if (node.IsLeaf()) { b2RayCastInput subInput = new b2RayCastInput(); subInput.p1 = input.p1; subInput.p2 = input.p2; subInput.maxFraction = maxFraction; float value = callback.RayCastCallback(subInput, nodeId); if (value == 0.0f) { // The client has terminated the ray cast. return; } if (value > 0.0f) { // Update segment bounding box. maxFraction = value; b2Vec2 t = p1 + maxFraction * (p2 - p1); segmentAABB.lowerBound = b2Math.b2Min(p1, t); segmentAABB.upperBound = b2Math.b2Max(p1, t); } } else { stack.Push(node.child1); stack.Push(node.child2); } } }