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);
}
