/// <summary>
/// Tests a physBody against every other registered physBody.
/// </summary>
/// <param name="physBody">Body being tested.</param>
/// <param name="colliderAABB">The AABB of the physBody being tested. This can be IPhysBody.WorldAABB, or a modified version of it.</param>
/// <param name="results">An empty list that the function stores all colliding bodies inside of.</param>
internal void DoCollisionTest(IPhysBody physBody, Box2 colliderAABB, List <IPhysBody> results)
{
// TODO: Terrible hack to fix bullets crashing the server.
// Should be handled with deferred physics events instead.
if (physBody.Owner.Deleted)
{
return;
}
_broadphase.Query((delegate(int id)
{
var body = _broadphase.GetProxy(id).Body;
// TODO: Terrible hack to fix bullets crashing the server.
// Should be handled with deferred physics events instead.
if (body.Owner.Deleted)
{
return(true);
}
if (TestMask(physBody, body))
{
results.Add(body);
}
return(true);
}), colliderAABB);
}
// Broad-phase callback.
private void AddPair(int proxyIdA, int proxyIdB)
{
FixtureProxy proxyA = BroadPhase.GetProxy(proxyIdA);
FixtureProxy proxyB = BroadPhase.GetProxy(proxyIdB);
Fixture fixtureA = proxyA.Fixture;
Fixture fixtureB = proxyB.Fixture;
int indexA = proxyA.ChildIndex;
int indexB = proxyB.ChildIndex;
Body bodyA = fixtureA.Body;
Body bodyB = fixtureB.Body;
// Are the fixtures on the same body?
if (bodyA == bodyB)
{
return;
}
// Does a contact already exist?
for (ContactEdge ceB = bodyB.ContactList; ceB != null; ceB = ceB.Next)
{
if (ceB.Other == bodyA)
{
Fixture fA = ceB.Contact.FixtureA;
Fixture fB = ceB.Contact.FixtureB;
int iA = ceB.Contact.ChildIndexA;
int iB = ceB.Contact.ChildIndexB;
if (fA == fixtureA && fB == fixtureB && iA == indexA && iB == indexB)
{
// A contact already exists.
return;
}
if (fA == fixtureB && fB == fixtureA && iA == indexB && iB == indexA)
{
// A contact already exists.
return;
}
}
}
// Does a joint override collision? Is at least one body dynamic?
if (bodyB.ShouldCollide(bodyA) == false)
{
return;
}
//Check default filter
if (ShouldCollide(fixtureA, fixtureB) == false)
{
return;
}
// Check user filtering.
if (ContactFilter != null && ContactFilter(fixtureA, fixtureB) == false)
{
return;
}
//FPE feature: BeforeCollision delegate
if (fixtureA.BeforeCollision != null && fixtureA.BeforeCollision(fixtureA, fixtureB) == false)
{
return;
}
if (fixtureB.BeforeCollision != null && fixtureB.BeforeCollision(fixtureB, fixtureA) == false)
{
return;
}
// Call the factory.
Contact c = Contact.Create(this, fixtureA, indexA, fixtureB, indexB);
if (c == null)
{
return;
}
// Contact creation may swap fixtures.
fixtureA = c.FixtureA;
fixtureB = c.FixtureB;
bodyA = fixtureA.Body;
bodyB = fixtureB.Body;
// Insert into the world.
c.Prev = ContactList;
c.Next = c.Prev.Next;
c.Prev.Next = c;
c.Next.Prev = c;
ContactCount++;
#if USE_ACTIVE_CONTACT_SET
ActiveContacts.Add(c);
#endif
// Connect to island graph.
// Connect to body A
c._nodeA.Contact = c;
c._nodeA.Other = bodyB;
c._nodeA.Prev = null;
c._nodeA.Next = bodyA.ContactList;
if (bodyA.ContactList != null)
{
bodyA.ContactList.Prev = c._nodeA;
}
bodyA.ContactList = c._nodeA;
// Connect to body B
c._nodeB.Contact = c;
c._nodeB.Other = bodyA;
c._nodeB.Prev = null;
c._nodeB.Next = bodyB.ContactList;
if (bodyB.ContactList != null)
{
bodyB.ContactList.Prev = c._nodeB;
}
bodyB.ContactList = c._nodeB;
// Wake up the bodies
if (fixtureA.IsSensor == false && fixtureB.IsSensor == false)
{
bodyA.Awake = true;
bodyB.Awake = true;
}
}
/// <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>
/// <param name="collisionCategory">The collision categories of the fixtures to raycast against.</param>
public void RayCast(IBroadPhase broadPhase, Func <RayCastInput, FixtureProxy, float> callback, ref RayCastInput input, Category collisionCategory = Category.All)
{
Vector2 p1 = input.Point1;
Vector2 p2 = input.Point2;
Vector2 r = p2 - p1;
Debug.Assert(r.LengthSquared() > 0.0f);
r.Normalize();
// v is perpendicular to the segment.
Vector2 absV = MathUtils.Abs(new Vector2(-r.Y, r.X)); //FPE: 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);
Vector2.Min(ref p1, ref t, out segmentAABB.LowerBound);
Vector2.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 _nodes[nodeId].AABB, ref segmentAABB) == false)
{
continue;
}
// Separating axis for segment (Gino, p80).
// |dot(v, p1 - c)| > dot(|v|, h)
Vector2 c = _nodes[nodeId].AABB.Center;
Vector2 h = _nodes[nodeId].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 (_nodes[nodeId].IsLeaf())
{
FixtureProxy proxy = broadPhase.GetProxy(nodeId);
if (collisionCategory != Category.All &&
//!collisionCategory.HasFlag(proxy.Fixture.CollisionCategories)
(collisionCategory & proxy.Fixture.CollisionCategories) == 0)
{
continue;
}
RayCastInput subInput;
subInput.Point1 = input.Point1;
subInput.Point2 = input.Point2;
subInput.MaxFraction = maxFraction;
float value = callback(subInput, proxy);
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);
Vector2.Min(ref p1, ref t, out segmentAABB.LowerBound);
Vector2.Max(ref p1, ref t, out segmentAABB.UpperBound);
}
}
else
{
_raycastStack.Push(_nodes[nodeId].Child1);
_raycastStack.Push(_nodes[nodeId].Child2);
}
}
}
