/// <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 == false)
{
return;
}
}
else
{
_queryStack.Push(node.Child1);
_queryStack.Push(node.Child2);
}
}
}
}
public void Query(Func <int, bool> callback, ref AABB aabb)
{
this._queryStack.Clear();
this._queryStack.Push(this._root);
while (this._queryStack.Count > 0)
{
int num = this._queryStack.Pop();
bool flag = num == -1;
if (!flag)
{
TreeNode <T> treeNode = this._nodes[num];
bool flag2 = AABB.TestOverlap(ref treeNode.AABB, ref aabb);
if (flag2)
{
bool flag3 = treeNode.IsLeaf();
if (flag3)
{
bool flag4 = callback(num);
bool flag5 = !flag4;
if (flag5)
{
break;
}
}
else
{
this._queryStack.Push(treeNode.Child1);
this._queryStack.Push(treeNode.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)
{
AABB aabbA, aabbB;
_tree.GetFatAABB(proxyIdA, out aabbA);
_tree.GetFatAABB(proxyIdB, out aabbB);
return(AABB.TestOverlap(ref aabbA, ref aabbB));
}
public bool TestOverlap(int proxyIdA, int proxyIdB)
{
AABB aABB;
this._tree.GetFatAABB(proxyIdA, out aABB);
AABB aABB2;
this._tree.GetFatAABB(proxyIdB, out aABB2);
return AABB.TestOverlap(ref aABB, ref aABB2);
}
/// <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, FP> callback, ref RayCastInput input)
{
TSVector2 p1 = input.Point1;
TSVector2 p2 = input.Point2;
TSVector2 r = p2 - p1;
Debug.Assert(r.LengthSquared() > 0.0f);
r.Normalize();
// v is perpendicular to the segment.
TSVector2 absV = MathUtils.Abs(new TSVector2(-r.y, r.x)); //FPE: Inlined the 'v' variable
// Separating axis for segment (Gino, p80).
// |dot(v, p1 - c)| > dot(|v|, h)
FP maxFraction = input.MaxFraction;
// Build a bounding box for the segment.
AABB segmentAABB = new AABB();
{
TSVector2 t = p1 + maxFraction * (p2 - p1);
TSVector2.Min(ref p1, ref t, out segmentAABB.LowerBound);
TSVector2.Max(ref p1, ref t, out segmentAABB.UpperBound);
}
_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) == false)
{
continue;
}
// Separating axis for segment (Gino, p80).
// |dot(v, p1 - c)| > dot(|v|, h)
TSVector2 c = node.AABB.Center;
TSVector2 h = node.AABB.Extents;
FP separation = FP.Abs(TSVector2.Dot(new TSVector2(-r.y, r.x), p1 - c)) - TSVector2.Dot(absV, h);
if (separation > 0.0f)
{
continue;
}
if (node.IsLeaf())
{
RayCastInput subInput;
subInput.Point1 = input.Point1;
subInput.Point2 = input.Point2;
subInput.MaxFraction = maxFraction;
FP 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;
TSVector2 t = p1 + maxFraction * (p2 - p1);
segmentAABB.LowerBound = TSVector2.Min(p1, t);
segmentAABB.UpperBound = TSVector2.Max(p1, t);
}
}
else
{
_raycastStack.Push(node.Child1);
_raycastStack.Push(node.Child2);
}
}
}
public void RayCast(Func <RayCastInput, int, FP> callback, ref RayCastInput input)
{
TSVector2 point = input.Point1;
TSVector2 point2 = input.Point2;
TSVector2 tSVector = point2 - point;
Debug.Assert(tSVector.LengthSquared() > 0f);
tSVector.Normalize();
TSVector2 value = MathUtils.Abs(new TSVector2(-tSVector.y, tSVector.x));
FP fP = input.MaxFraction;
AABB aABB = default(AABB);
TSVector2 tSVector2 = point + fP * (point2 - point);
TSVector2.Min(ref point, ref tSVector2, out aABB.LowerBound);
TSVector2.Max(ref point, ref tSVector2, out aABB.UpperBound);
this._raycastStack.Clear();
this._raycastStack.Push(this._root);
while (this._raycastStack.Count > 0)
{
int num = this._raycastStack.Pop();
bool flag = num == -1;
if (!flag)
{
TreeNode <T> treeNode = this._nodes[num];
bool flag2 = !AABB.TestOverlap(ref treeNode.AABB, ref aABB);
if (!flag2)
{
TSVector2 center = treeNode.AABB.Center;
TSVector2 extents = treeNode.AABB.Extents;
FP x = FP.Abs(TSVector2.Dot(new TSVector2(-tSVector.y, tSVector.x), point - center)) - TSVector2.Dot(value, extents);
bool flag3 = x > 0f;
if (!flag3)
{
bool flag4 = treeNode.IsLeaf();
if (flag4)
{
RayCastInput arg;
arg.Point1 = input.Point1;
arg.Point2 = input.Point2;
arg.MaxFraction = fP;
FP fP2 = callback(arg, num);
bool flag5 = fP2 == 0f;
if (flag5)
{
break;
}
bool flag6 = fP2 > 0f;
if (flag6)
{
fP = fP2;
TSVector2 value2 = point + fP * (point2 - point);
aABB.LowerBound = TSVector2.Min(point, value2);
aABB.UpperBound = TSVector2.Max(point, value2);
}
}
else
{
this._raycastStack.Push(treeNode.Child1);
this._raycastStack.Push(treeNode.Child2);
}
}
}
}
}
}