/// <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="input">the ray-cast input data. The ray extends from p1 to p1 + maxFraction * (p2 - p1).</param>
/// <param name="callback">a callback class that is called for each proxy that is hit by the ray.</param>
public virtual void raycast(TreeRayCastCallback callback, RayCastInput input)
{
Vec2 p1 = input.p1;
Vec2 p2 = input.p2;
r.set_Renamed(p2).subLocal(p1);
Debug.Assert(r.lengthSquared() > 0f);
r.normalize();
// v is perpendicular to the segment.
Vec2.crossToOutUnsafe(1f, r, v);
absV.set_Renamed(v).absLocal();
// 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 segAABB = aabb;
// Vec2 t = p1 + maxFraction * (p2 - p1);
temp.set_Renamed(p2).subLocal(p1).mulLocal(maxFraction).addLocal(p1);
Vec2.minToOut(p1, temp, segAABB.lowerBound);
Vec2.maxToOut(p1, temp, segAABB.upperBound);
intStack.push(m_root);
while (intStack.Count > 0)
{
int nodeId = intStack.pop();
if (nodeId == TreeNode.NULL_NODE)
{
continue;
}
TreeNode node = m_nodes[nodeId];
if (!AABB.testOverlap(node.aabb, segAABB))
{
continue;
}
// Separating axis for segment (Gino, p80).
// |dot(v, p1 - c)| > dot(|v|, h)
node.aabb.getCenterToOut(c);
node.aabb.getExtentsToOut(h);
temp.set_Renamed(p1).subLocal(c);
float separation = MathUtils.abs(Vec2.dot(v, temp)) - Vec2.dot(absV, h);
if (separation > 0.0f)
{
continue;
}
if (node.Leaf)
{
subInput.p1.set_Renamed(input.p1);
subInput.p2.set_Renamed(input.p2);
subInput.maxFraction = maxFraction;
float value_Renamed = callback.raycastCallback(subInput, nodeId);
if (value_Renamed == 0.0f)
{
// The client has terminated the ray cast.
return;
}
if (value_Renamed > 0.0f)
{
// Update segment bounding box.
maxFraction = value_Renamed;
t.set_Renamed(p2).subLocal(p1).mulLocal(maxFraction).addLocal(p1);
Vec2.minToOut(p1, t, segAABB.lowerBound);
Vec2.maxToOut(p1, t, segAABB.upperBound);
}
}
else
{
intStack.push(node.child1);
intStack.push(node.child2);
}
}
}
public void raycast(TreeRayCastCallback callback, RayCastInput input)
{
m_tree.raycast(callback, input);
}
public void raycast(TreeRayCastCallback callback, RayCastInput input)
{
Vec2 p1 = input.p1;
Vec2 p2 = input.p2;
double p1x = p1.x, p2x = p2.x, p1y = p1.y, p2y = p2.y;
double vx, vy;
double rx, ry;
double absVx, absVy;
double cx, cy;
double hx, hy;
double tempx, tempy;
r.x = p2x - p1x;
r.y = p2y - p1y;
r.normalize();
rx = r.x;
ry = r.y;
// v is perpendicular to the segment.
vx = -1d * ry;
vy = 1d * rx;
absVx = MathUtils.abs(vx);
absVy = MathUtils.abs(vy);
// Separating axis for segment (Gino, p80).
// |dot(v, p1 - c)| > dot(|v|, h)
double maxFraction = input.maxFraction;
// Build a bounding box for the segment.
AABB segAABB = aabb;
// Vec2 t = p1 + maxFraction * (p2 - p1);
// before inline
// temp.set(p2).subLocal(p1).mulLocal(maxFraction).addLocal(p1);
// Vec2.minToOut(p1, temp, segAABB.lowerBound);
// Vec2.maxToOut(p1, temp, segAABB.upperBound);
tempx = (p2x - p1x) * maxFraction + p1x;
tempy = (p2y - p1y) * maxFraction + p1y;
segAABB.lowerBound.x = p1x < tempx ? p1x : tempx;
segAABB.lowerBound.y = p1y < tempy ? p1y : tempy;
segAABB.upperBound.x = p1x > tempx ? p1x : tempx;
segAABB.upperBound.y = p1y > tempy ? p1y : tempy;
// end inline
nodeStack.reset();
nodeStack.push(m_root);
while (nodeStack.getCount() > 0)
{
DynamicTreeNode node = nodeStack.pop();
if (node == null)
{
continue;
}
AABB nodeAABB = node.aabb;
if (!AABB.testOverlap(nodeAABB, segAABB))
{
continue;
}
// Separating axis for segment (Gino, p80).
// |dot(v, p1 - c)| > dot(|v|, h)
// node.aabb.getCenterToOut(c);
// node.aabb.getExtentsToOut(h);
cx = (nodeAABB.lowerBound.x + nodeAABB.upperBound.x) * .5d;
cy = (nodeAABB.lowerBound.y + nodeAABB.upperBound.y) * .5d;
hx = (nodeAABB.upperBound.x - nodeAABB.lowerBound.x) * .5d;
hy = (nodeAABB.upperBound.y - nodeAABB.lowerBound.y) * .5d;
tempx = p1x - cx;
tempy = p1y - cy;
double separation = MathUtils.abs(vx * tempx + vy * tempy) - (absVx * hx + absVy * hy);
if (separation > 0.0d)
{
continue;
}
if (node.isLeaf())
{
subInput.p1.x = p1x;
subInput.p1.y = p1y;
subInput.p2.x = p2x;
subInput.p2.y = p2y;
subInput.maxFraction = maxFraction;
double value = callback.raycastCallback(subInput, node.id);
if (value == 0.0d)
{
// The client has terminated the ray cast.
return;
}
if (value > 0.0d)
{
// Update segment bounding box.
maxFraction = value;
// temp.set(p2).subLocal(p1).mulLocal(maxFraction).addLocal(p1);
// Vec2.minToOut(p1, temp, segAABB.lowerBound);
// Vec2.maxToOut(p1, temp, segAABB.upperBound);
tempx = (p2x - p1x) * maxFraction + p1x;
tempy = (p2y - p1y) * maxFraction + p1y;
segAABB.lowerBound.x = p1x < tempx ? p1x : tempx;
segAABB.lowerBound.y = p1y < tempy ? p1y : tempy;
segAABB.upperBound.x = p1x > tempx ? p1x : tempx;
segAABB.upperBound.y = p1y > tempy ? p1y : tempy;
}
}
else
{
nodeStack.push(node.child1);
nodeStack.push(node.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 : from p1 to p1 + maxFraction * (p2 - p1).
* @param callback a callback class that is called for each proxy that is hit by the ray.
*/
public void raycast(TreeRayCastCallback callback, RayCastInput input)
{
m_tree.raycast(callback, input);
}
public void raycast(TreeRayCastCallback callback, RayCastInput input)
{
Vec2 p1 = input.p1;
Vec2 p2 = input.p2;
float p1x = p1.x, p2x = p2.x, p1y = p1.y, p2y = p2.y;
float vx, vy;
float rx, ry;
float absVx, absVy;
float cx, cy;
float hx, hy;
float tempx, tempy;
r.x = p2x - p1x;
r.y = p2y - p1y;
Debug.Assert((r.x*r.x + r.y*r.y) > 0f);
r.normalize();
rx = r.x;
ry = r.y;
// v is perpendicular to the segment.
vx = -1f*ry;
vy = 1f*rx;
absVx = MathUtils.abs(vx);
absVy = MathUtils.abs(vy);
// 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 segAABB = aabb;
// Vec2 t = p1 + maxFraction * (p2 - p1);
// before inline
// temp.set(p2).subLocal(p1).mulLocal(maxFraction).addLocal(p1);
// Vec2.minToOut(p1, temp, segAABB.lowerBound);
// Vec2.maxToOut(p1, temp, segAABB.upperBound);
tempx = (p2x - p1x)*maxFraction + p1x;
tempy = (p2y - p1y)*maxFraction + p1y;
segAABB.lowerBound.x = p1x < tempx ? p1x : tempx;
segAABB.lowerBound.y = p1y < tempy ? p1y : tempy;
segAABB.upperBound.x = p1x > tempx ? p1x : tempx;
segAABB.upperBound.y = p1y > tempy ? p1y : tempy;
// end inline
nodeStackIndex = 0;
nodeStack[nodeStackIndex++] = m_root;
while (nodeStackIndex > 0)
{
DynamicTreeNode node = nodeStack[--nodeStackIndex];
if (node == null)
{
continue;
}
AABB nodeAABB = node.aabb;
if (!AABB.testOverlap(nodeAABB, segAABB))
{
continue;
}
// Separating axis for segment (Gino, p80).
// |dot(v, p1 - c)| > dot(|v|, h)
// node.aabb.getCenterToOut(c);
// node.aabb.getExtentsToOut(h);
cx = (nodeAABB.lowerBound.x + nodeAABB.upperBound.x)*.5f;
cy = (nodeAABB.lowerBound.y + nodeAABB.upperBound.y)*.5f;
hx = (nodeAABB.upperBound.x - nodeAABB.lowerBound.x)*.5f;
hy = (nodeAABB.upperBound.y - nodeAABB.lowerBound.y)*.5f;
tempx = p1x - cx;
tempy = p1y - cy;
float separation = MathUtils.abs(vx*tempx + vy*tempy) - (absVx*hx + absVy*hy);
if (separation > 0.0f)
{
continue;
}
if (node.child1 == null)
{
subInput.p1.x = p1x;
subInput.p1.y = p1y;
subInput.p2.x = p2x;
subInput.p2.y = p2y;
subInput.maxFraction = maxFraction;
float value = callback.raycastCallback(subInput, node.id);
if (value == 0.0f)
{
// The client has terminated the ray cast.
return;
}
if (value > 0.0f)
{
// Update segment bounding box.
maxFraction = value;
// temp.set(p2).subLocal(p1).mulLocal(maxFraction).addLocal(p1);
// Vec2.minToOut(p1, temp, segAABB.lowerBound);
// Vec2.maxToOut(p1, temp, segAABB.upperBound);
tempx = (p2x - p1x)*maxFraction + p1x;
tempy = (p2y - p1y)*maxFraction + p1y;
segAABB.lowerBound.x = p1x < tempx ? p1x : tempx;
segAABB.lowerBound.y = p1y < tempy ? p1y : tempy;
segAABB.upperBound.x = p1x > tempx ? p1x : tempx;
segAABB.upperBound.y = p1y > tempy ? p1y : tempy;
}
}
else
{
if (nodeStack.Length - nodeStackIndex - 2 <= 0)
{
DynamicTreeNode[] newBuffer = new DynamicTreeNode[nodeStack.Length*2];
Array.Copy(nodeStack, 0, newBuffer, 0, nodeStack.Length);
nodeStack = newBuffer;
}
nodeStack[nodeStackIndex++] = node.child1;
nodeStack[nodeStackIndex++] = node.child2;
}
}
}
