/// <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); } } }