/// <summary> Query an AABB for overlapping proxies. The callback class
/// is called for each proxy that overlaps the supplied AABB.
/// </summary>
public void Query(Func <int, bool> callback, ref AABB aabb)
{
stack.Clear();
stack.Push(_root);
while (stack.Count > 0)
{
int nodeId = stack.Pop();
if (nodeId == NullNode)
{
continue;
}
DynamicTreeNode node = _nodes[nodeId];
if (AABB.TestOverlap(ref node.aabb, ref aabb))
{
if (node.IsLeaf())
{
bool proceed = callback(nodeId);
if (!proceed)
{
return;
}
}
else
{
stack.Push(node.child1);
stack.Push(node.child2);
}
}
}
}
/// <summary>
/// Test overlap of fat AABBs.
/// </summary>
/// <param name="proxyIdA"></param>
/// <param name="proxyIdB"></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));
}
/// Query an AABB for overlapping proxies. The callback class
/// is called for each proxy that overlaps the supplied AABB.
public void Query(Func <int, bool> callback, ref AABB aabb)
{
int count = 0;
stack[count++] = _root;
while (count > 0)
{
int nodeId = stack[--count];
if (nodeId == NullNode)
{
continue;
}
DynamicTreeNode node = _nodes[nodeId];
if (AABB.TestOverlap(ref node.aabb, ref aabb))
{
if (node.IsLeaf())
{
bool proceed = callback(nodeId);
if (!proceed)
{
return;
}
}
else
{
if (count < k_stackSize)
{
stack[count++] = node.child1;
}
if (count < k_stackSize)
{
stack[count++] = 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 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.
internal void RayCast(RayCastCallbackInternal callback, ref RayCastInput input)
{
Vector2 p1 = input.p1;
Vector2 p2 = input.p2;
Vector2 r = p2 - p1;
Debug.Assert(r.LengthSquared() > 0.0f);
r.Normalize();
// v is perpendicular to the segment.
Vector2 v = MathUtils.Cross(1.0f, r);
Vector2 abs_v = MathUtils.Abs(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.
AABB segmentAABB = new AABB();
{
Vector2 t = p1 + maxFraction * (p2 - p1);
segmentAABB.lowerBound = Vector2.Min(p1, t);
segmentAABB.upperBound = Vector2.Max(p1, t);
}
int count = 0;
stack[count++] = _root;
while (count > 0)
{
int nodeId = stack[--count];
if (nodeId == NullNode)
{
continue;
}
DynamicTreeNode 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)
Vector2 c = node.aabb.GetCenter();
Vector2 h = node.aabb.GetExtents();
float separation = Math.Abs(Vector2.Dot(v, p1 - c)) - Vector2.Dot(abs_v, h);
if (separation > 0.0f)
{
continue;
}
if (node.IsLeaf())
{
RayCastInput subInput;
subInput.p1 = input.p1;
subInput.p2 = input.p2;
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
{
if (count < k_stackSize)
{
stack[count++] = node.child1;
}
if (count < k_stackSize)
{
stack[count++] = node.child2;
}
}
}
}
// Update the contact manifold and touching status.
// Note: do not assume the fixture AABBs are overlapping or are valid.
internal void Update(IContactListener listener)
{
Manifold oldManifold = _manifold;
// Re-enable this contact.
_flags |= ContactFlags.Enabled;
bool touching = false;
bool wasTouching = (_flags & ContactFlags.Touching) == ContactFlags.Touching;
bool sensorA = _fixtureA.IsSensor();
bool sensorB = _fixtureB.IsSensor();
bool sensor = sensorA || sensorB;
Body bodyA = _fixtureA.GetBody();
Body bodyB = _fixtureB.GetBody();
Transform xfA; bodyA.GetTransform(out xfA);
Transform xfB; bodyB.GetTransform(out xfB);
// Is this contact a sensor?
if (sensor)
{
Shape shapeA = _fixtureA.GetShape();
Shape shapeB = _fixtureB.GetShape();
touching = AABB.TestOverlap(shapeA, _indexA, shapeB, _indexB, ref xfA, ref xfB);
// Sensors don't generate manifolds.
_manifold._pointCount = 0;
}
else
{
Evaluate(ref _manifold, ref xfA, ref xfB);
touching = _manifold._pointCount > 0;
// Match old contact ids to new contact ids and copy the
// stored impulses to warm start the solver.
for (int i = 0; i < _manifold._pointCount; ++i)
{
ManifoldPoint mp2 = _manifold._points[i];
mp2.NormalImpulse = 0.0f;
mp2.TangentImpulse = 0.0f;
ContactID id2 = mp2.Id;
bool found = false;
for (int j = 0; j < oldManifold._pointCount; ++j)
{
ManifoldPoint mp1 = oldManifold._points[j];
if (mp1.Id.Key == id2.Key)
{
mp2.NormalImpulse = mp1.NormalImpulse;
mp2.TangentImpulse = mp1.TangentImpulse;
found = true;
break;
}
}
if (found == false)
{
mp2.NormalImpulse = 0.0f;
mp2.TangentImpulse = 0.0f;
}
_manifold._points[i] = mp2;
}
if (touching != wasTouching)
{
bodyA.SetAwake(true);
bodyB.SetAwake(true);
}
}
if (touching)
{
_flags |= ContactFlags.Touching;
}
else
{
_flags &= ~ContactFlags.Touching;
}
if (wasTouching == false && touching == true && null != listener)
{
listener.BeginContact(this);
}
if (wasTouching == true && touching == false && null != listener)
{
listener.EndContact(this);
}
if (sensor == false && null != listener)
{
listener.PreSolve(this, ref oldManifold);
}
}
