/// <summary>
/// Query an AABB for overlapping proxies. The callback class is called for each proxy that overlaps the supplied
/// AABB.
/// </summary>
/// <param name="callback">The callback.</param>
/// <param name="aabb">The AABB.</param>
public void Query(Func <int, bool> callback, ref Aabb aabb)
{
queryStack.Clear();
queryStack.Push(root);
while (queryStack.Count > 0)
{
int nodeId = queryStack.Pop();
if (nodeId == NullNode)
{
continue;
}
TreeNode <T> node = nodes[nodeId];
if (Aabb.TestOverlap(ref node.Aabb, ref aabb))
{
if (node.IsLeaf())
{
bool proceed = callback(nodeId);
if (!proceed)
{
return;
}
}
else
{
queryStack.Push(node.Child1);
queryStack.Push(node.Child2);
}
}
}
}
/// <summary>Test overlap of fat AABBs.</summary>
/// <param name="proxyIdA">The proxy id A.</param>
/// <param name="proxyIdB">The proxy id B.</param>
/// <returns></returns>
public bool TestOverlap(int proxyIdA, int proxyIdB)
{
tree.GetFatAabb(proxyIdA, out Aabb aabbA);
tree.GetFatAabb(proxyIdB, out Aabb aabbB);
return(Aabb.TestOverlap(ref aabbA, ref aabbB));
}
/// <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(Func <RayCastInput, int, float> 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)); //Velcro: 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 node.Aabb, ref segmentAabb))
{
continue;
}
// Separating axis for segment (Gino, p80).
// |dot(v, p1 - c)| > dot(|v|, h)
Vector2 c = node.Aabb.Center;
Vector2 h = node.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 (node.IsLeaf())
{
RayCastInput subInput;
subInput.Point1 = input.Point1;
subInput.Point2 = input.Point2;
subInput.MaxFraction = maxFraction;
float value = callback(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(node.Child1);
raycastStack.Push(node.Child2);
}
}
}