public void RayCast(Ib2RayCastCallback 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 = r.NegUnitCross(); // 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 = b2AABB.Default; { b2Vec2 t = p1 + maxFraction * (p2 - p1); segmentAABB.Set(b2Math.b2Min(p1, t), 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.Center; b2Vec2 h = node.aabb.Extents; 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.Set(b2Math.b2Min(p1, t), b2Math.b2Max(p1, t)); } } else { stack.Push(node.child1); stack.Push(node.child2); } } }
public void RayCast(Ib2RayCastCallback 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 = r.NegUnitCross(); // 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 = b2AABB.Default; { b2Vec2 t = p1 + maxFraction * (p2 - p1); segmentAABB.Set(b2Math.b2Min(p1, t), 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.Center; b2Vec2 h = node.aabb.Extents; 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.Set(b2Math.b2Min(p1, t),b2Math.b2Max(p1, t)); } } else { stack.Push(node.child1); stack.Push(node.child2); } } }