/**
* @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);
}
