public float RayCastCallback(b2RayCastInput input, int proxyId) { b2FixtureProxy proxy = (b2FixtureProxy)broadPhase.GetUserData(proxyId); b2Fixture fixture = proxy.fixture; int index = proxy.childIndex; b2RayCastOutput output; bool hit = fixture.RayCast(out output, input, index); if (hit) { float fraction = output.fraction; b2Vec2 point = (1.0f - fraction) * input.p1 + fraction * input.p2; return callback.ReportFixture(fixture, point, output.normal, fraction); } return input.maxFraction; }
public bool RayCast(out b2RayCastOutput output, b2RayCastInput input) { float tmin = -float.MaxValue; float tmax = float.MaxValue; b2Vec2 p = input.p1; b2Vec2 d = input.p2 - input.p1; b2Vec2 absD = b2Math.b2Abs(d); b2Vec2 normal = new b2Vec2(0,0); for (int i = 0; i < 2; ++i) { float p_i, lb, ub, d_i, absd_i; p_i = (i == 0 ? p.x : p.y); lb = (i == 0 ? m_lowerBound.x : m_lowerBound.y); ub = (i == 0 ? m_upperBound.x : m_upperBound.y); absd_i = (i == 0 ? absD.x : absD.y); d_i = (i == 0 ? d.x : d.y); if (absd_i < b2Settings.b2_epsilon) { // Parallel. if (p_i < lb || ub < p_i) { output.fraction = 0f; output.normal = new b2Vec2(0, 0); return false; } } else { float inv_d = 1.0f / d_i; float t1 = (lb - p_i) * inv_d; float t2 = (ub - p_i) * inv_d; // Sign of the normal vector. float s = -1.0f; if (t1 > t2) { b2Math.b2Swap(t1, t2); s = 1.0f; } // Push the min up if (t1 > tmin) { normal.SetZero(); if (i == 0) { normal.x = s; } else { normal.y = s; } tmin = t1; } // Pull the max down tmax = Math.Min(tmax, t2); if (tmin > tmax) { output.fraction = 0f; output.normal = new b2Vec2(0, 0); return false; } } } // Does the ray start inside the box? // Does the ray intersect beyond the max fraction? if (tmin < 0.0f || input.maxFraction < tmin) { output.fraction = 0f; output.normal = new b2Vec2(0, 0); return false; } // Intersection. output.fraction = tmin; output.normal = normal; return true; }
public virtual bool RayCast(out b2RayCastOutput output, b2RayCastInput input, int childIndex) { return m_shape.RayCast(out output, input, m_body.Transform, childIndex); }
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); } } }
public bool RayCast(out b2RayCastOutput output, b2RayCastInput input) { float tmin = -float.MaxValue; float tmax = float.MaxValue; b2Vec2 p = input.p1; b2Vec2 d = input.p2 - input.p1; b2Vec2 absD = b2Math.b2Abs(d); b2Vec2 normal = new b2Vec2(0, 0); for (int i = 0; i < 2; ++i) { float p_i, lb, ub, d_i, absd_i; p_i = (i == 0 ? p.x : p.y); lb = (i == 0 ? m_lowerBound.x : m_lowerBound.y); ub = (i == 0 ? m_upperBound.x : m_upperBound.y); absd_i = (i == 0 ? absD.x : absD.y); d_i = (i == 0 ? d.x : d.y); if (absd_i < b2Settings.b2_epsilon) { // Parallel. if (p_i < lb || ub < p_i) { output.fraction = 0f; output.normal = new b2Vec2(0, 0); return(false); } } else { float inv_d = 1.0f / d_i; float t1 = (lb - p_i) * inv_d; float t2 = (ub - p_i) * inv_d; // Sign of the normal vector. float s = -1.0f; if (t1 > t2) { b2Math.b2Swap(t1, t2); s = 1.0f; } // Push the min up if (t1 > tmin) { normal.SetZero(); if (i == 0) { normal.x = s; } else { normal.y = s; } tmin = t1; } // Pull the max down tmax = Math.Min(tmax, t2); if (tmin > tmax) { output.fraction = 0f; output.normal = new b2Vec2(0, 0); return(false); } } } // Does the ray start inside the box? // Does the ray intersect beyond the max fraction? if (tmin < 0.0f || input.maxFraction < tmin) { output.fraction = 0f; output.normal = new b2Vec2(0, 0); return(false); } // Intersection. output.fraction = tmin; output.normal = normal; return(true); }
public virtual void RayCast(b2WorldRayCastWrapper w, b2RayCastInput input) { m_tree.RayCast(w, input); }
public float RayCastCallback(ref b2RayCastInput input, int proxyId) { Actor actor = (Actor) m_tree.GetUserData(proxyId); b2RayCastOutput output = new b2RayCastOutput(); bool hit = actor.aabb.RayCast(out output, input); if (hit) { m_rayCastOutput = output; m_rayActor = actor; m_rayActor.fraction = output.fraction; return output.fraction; } return input.maxFraction; }
/** * @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; }
public void RayCast <T>(T callback, b2RayCastInput input) { m_tree.RayCast(callback, input); }
/** * 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; } } }
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 = 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); } } }
/** * @inheritDoc */ public void RayCast(BroadPhaseRayCastCallback callback, b2RayCastInput input) { m_tree.RayCast(callback, input); }
// From Real-time Collision Detection, p179. /** * Perform a precise raycast against the AABB. */ public bool RayCast(b2RayCastOutput output, b2RayCastInput input) { float tmin = -float.MaxValue; //float tmax = float.MaxValue; float pX = input.p1.x; float pY = input.p1.y; float dX = input.p2.x - input.p1.x; float dY = input.p2.y - input.p1.y; float absDX = Mathf.Abs(dX); float absDY = Mathf.Abs(dY); b2Vec2 normal = output.normal; float inv_d; float t1; float t2; float t3; float s; //x if (absDX < float.MinValue) { // Parallel. if (pX < lowerBound.x || upperBound.x < pX) { return(false); } } else { inv_d = 1.0f / dX; t1 = (lowerBound.x - pX) * inv_d; t2 = (upperBound.x - pX) * inv_d; // Sign of the normal vector s = -1.0f; if (t1 > t2) { t3 = t1; t1 = t2; t2 = t3; s = 1.0f; } // Push the min up if (t1 > tmin) { normal.x = s; normal.y = 0.0f; tmin = t1; } // Pull the max down float tmax = Mathf.Min(float.MaxValue, t2); if (tmin > tmax) { return(false); } } //y if (absDY < float.MinValue) { // Parallel. if (pY < lowerBound.y || upperBound.y < pY) { return(false); } } else { inv_d = 1.0f / dY; t1 = (lowerBound.y - pY) * inv_d; t2 = (upperBound.y - pY) * inv_d; // Sign of the normal vector s = -1.0f; if (t1 > t2) { t3 = t1; t1 = t2; t2 = t3; s = 1.0f; } // Push the min up if (t1 > tmin) { normal.y = s; normal.x = 0.0f; tmin = t1; } // Pull the max down float tmax = Mathf.Min(float.MaxValue, t2); if (tmin > tmax) { return(false); } } output.fraction = tmin; return(true); }