/** * @inheritDoc */ public void RayCast(BroadPhaseRayCastCallback callback, b2RayCastInput input) { b2RayCastInput subInput = new b2RayCastInput(); subInput.p1.SetV(input.p1); subInput.p2.SetV(input.p2); subInput.maxFraction = input.maxFraction; float dx = (input.p2.x - input.p1.x) * m_quantizationFactor.x; float dy = (input.p2.y - input.p1.y) * m_quantizationFactor.y; int sx = dx < -float.MinValue ? -1 : (dx > float.MinValue ? 1 : 0); int sy = dy < -float.MinValue ? -1 : (dy > float.MinValue ? 1 : 0); //b2Settings.b2Assert(sx!=0||sy!=0); float p1x = m_quantizationFactor.x * (input.p1.x - m_worldAABB.lowerBound.x); float p1y = m_quantizationFactor.y * (input.p1.y - m_worldAABB.lowerBound.y); List <uint> startValues = new List <uint>(); List <uint> startValues2 = new List <uint>(); startValues[0] = (uint)(p1x) & (b2Settings.USHRT_MAX - 1); startValues[1] = (uint)(p1y) & (b2Settings.USHRT_MAX - 1); startValues2[0] = startValues[0] + 1; startValues2[1] = startValues[1] + 1; List <uint> startIndices = new List <uint> (); int xIndex; int yIndex; b2Proxy proxy; //First deal with all the proxies that contain segment.p1 uint lowerIndex = 0; uint upperIndex = 0; List <uint> lowerIndexOut = new List <uint>(); lowerIndexOut.Add(lowerIndex); List <uint> upperIndexOut = new List <uint>(); upperIndexOut.Add(upperIndex); QueryAxis(lowerIndexOut, upperIndexOut, startValues[0], startValues2[0], m_bounds[0], (uint)(2 * m_proxyCount), 0); if (sx >= 0) { xIndex = (int)(upperIndexOut[0] - 1); } else { xIndex = (int)(lowerIndexOut[0]); } QueryAxis(lowerIndexOut, upperIndexOut, startValues[1], startValues2[1], m_bounds[1], (uint)(2 * m_proxyCount), 1); if (sy >= 0) { yIndex = (int)(upperIndexOut[0] - 1); } else { yIndex = (int)lowerIndexOut[0]; } // Callback for starting proxies: for (int i = 0; i < m_queryResultCount; i++) { //subInput.maxFraction = callback(m_queryResults[i], subInput); subInput.maxFraction = callback(subInput, m_queryResults[i]); } //Now work through the rest of the segment for (;;) { float xProgress = 0; float yProgress = 0; //Move on to next bound xIndex += sx >= 0?1: -1; if (xIndex < 0 || xIndex >= m_proxyCount * 2) { break; } if (sx != 0) { xProgress = (m_bounds[0][xIndex].value - p1x) / dx; } //Move on to next bound yIndex += sy >= 0?1: -1; if (yIndex < 0 || yIndex >= m_proxyCount * 2) { break; } if (sy != 0) { yProgress = (m_bounds[1][yIndex].value - p1y) / dy; } for (;;) { if (sy == 0 || (sx != 0 && xProgress < yProgress)) { if (xProgress > subInput.maxFraction) { break; } //Check that we are entering a proxy, not leaving if (sx > 0?m_bounds[0][xIndex].IsLower():m_bounds[0][xIndex].IsUpper()) { //Check the other axis of the proxy proxy = m_bounds[0][xIndex].proxy; if (sy >= 0) { if (proxy.lowerBounds[1] <= yIndex - 1 && proxy.upperBounds[1] >= yIndex) { //Add the proxy //subInput.maxFraction = callback(proxy, subInput); subInput.maxFraction = callback(subInput, proxy); } } else { if (proxy.lowerBounds[1] <= yIndex && proxy.upperBounds[1] >= yIndex + 1) { //Add the proxy //subInput.maxFraction = callback(proxy, subInput); subInput.maxFraction = callback(subInput, proxy); } } } //Early out if (subInput.maxFraction == 0) { break; } //Move on to the next bound if (sx > 0) { xIndex++; if (xIndex == m_proxyCount * 2) { break; } } else { xIndex--; if (xIndex < 0) { break; } } xProgress = (m_bounds[0][xIndex].value - p1x) / dx; } else { if (yProgress > subInput.maxFraction) { break; } //Check that we are entering a proxy, not leaving if (sy > 0?m_bounds[1][yIndex].IsLower():m_bounds[1][yIndex].IsUpper()) { //Check the other axis of the proxy proxy = m_bounds[1][yIndex].proxy; if (sx >= 0) { if (proxy.lowerBounds[0] <= xIndex - 1 && proxy.upperBounds[0] >= xIndex) { //Add the proxy //subInput.maxFraction = callback(proxy, subInput); subInput.maxFraction = callback(subInput, proxy); } } else { if (proxy.lowerBounds[0] <= xIndex && proxy.upperBounds[0] >= xIndex + 1) { //Add the proxy //subInput.maxFraction = callback(proxy, subInput); subInput.maxFraction = callback(subInput, proxy); } } } //Early out if (subInput.maxFraction == 0) { break; } //Move on to the next bound if (sy > 0) { yIndex++; if (yIndex == m_proxyCount * 2) { break; } } else { yIndex--; if (yIndex < 0) { break; } } yProgress = (m_bounds[1][yIndex].value - p1y) / dy; } } break; } // Prepare for next query. m_queryResultCount = 0; IncrementTimeStamp(); return; }
/// <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(BroadPhaseRayCastCallback 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)); //FPE: 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 _nodes[nodeId].AABB, ref segmentAABB) == false) { continue; } // Separating axis for segment (Gino, p80). // |dot(v, p1 - c)| > dot(|v|, h) Vector2 c = _nodes[nodeId].AABB.Center; Vector2 h = _nodes[nodeId].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 (_nodes[nodeId].IsLeaf()) { RayCastInput subInput; subInput.Point1 = input.Point1; subInput.Point2 = input.Point2; 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 { _raycastStack.Push(_nodes[nodeId].Child1); _raycastStack.Push(_nodes[nodeId].Child2); } } }
/** * 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. * It should be of signature: * ----- <code>function callback(input:b2RayCastInput, proxy:*):void</code> * <code>function callback(input:b2RayCastInput, proxy:*):Number</code> */ public void RayCast(BroadPhaseRayCastCallback callback, b2RayCastInput input) { if (m_root == null) { return; } b2Vec2 p1 = input.p1; b2Vec2 p2 = input.p2; b2Vec2 r = b2Math.SubtractVV(p1, p2); //b2Settings.b2Assert(r.LengthSquared() > 0.0); r.Normalize(); // v is perpendicular to the segment b2Vec2 v = b2Math.CrossFV(1.0f, r); b2Vec2 abs_v = b2Math.AbsV(v); float maxFraction = input.maxFraction; // Build a bounding box for the segment b2AABB segmentAABB = new b2AABB(); float tX; float tY; { tX = p1.x + maxFraction * (p2.x - p1.x); tY = p1.y + maxFraction * (p2.y - p1.y); segmentAABB.lowerBound.x = Mathf.Min(p1.x, tX); segmentAABB.lowerBound.y = Mathf.Min(p1.y, tY); segmentAABB.upperBound.x = Mathf.Max(p1.x, tX); segmentAABB.upperBound.y = Mathf.Max(p1.y, tY); } Dictionary <int, b2DynamicTreeNode> stack = new Dictionary <int, b2DynamicTreeNode>(); int count = 0; stack[count++] = m_root; while (count > 0) { b2DynamicTreeNode node = stack[--count]; if (node.aabb.TestOverlap(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 = Mathf.Abs(v.x * (p1.x - c.x) + v.y * (p1.y - c.y)) - abs_v.x * h.x - abs_v.y * h.y; if (separation > 0.0f) { continue; } if (node.IsLeaf()) { b2RayCastInput subInput = new b2RayCastInput(); subInput.p1 = input.p1; subInput.p2 = input.p2; //*************by kingBook 2015/10/22 16:17************* subInput.maxFraction = maxFraction; float value = callback(subInput, node); if (value == 0) { return; } if (value > 0) { //Update the segment bounding box maxFraction = value; //****************************************************** tX = p1.x + maxFraction * (p2.x - p1.x); tY = p1.y + maxFraction * (p2.y - p1.y); segmentAABB.lowerBound.x = Mathf.Min(p1.x, tX); segmentAABB.lowerBound.y = Mathf.Min(p1.y, tY); segmentAABB.upperBound.x = Mathf.Max(p1.x, tX); segmentAABB.upperBound.y = Mathf.Max(p1.y, tY); } } else { // No stack limit, so no assert stack[count++] = node.child1; stack[count++] = node.child2; } } }
/// <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(BroadPhaseRayCastCallback callback, ref RayCastInput input) { _tree.RayCast(callback, ref input); }
private Func <RayCastInput, Element <FixtureProxy>, float> TransformRayCallback(BroadPhaseRayCastCallback callback) { Func <RayCastInput, Element <FixtureProxy>, float> newCallback = (input, qtnode) => callback(ref input, qtnode.Value.ProxyId); return(newCallback); }
public void RayCast(BroadPhaseRayCastCallback callback, ref RayCastInput input) { _quadTree.RayCast(TransformRayCallback(callback), ref input); }
/** * @inheritDoc */ public void RayCast(BroadPhaseRayCastCallback callback, b2RayCastInput input) { m_tree.RayCast(callback, input); }