/// Destroy a joint. This may cause the connected bodies to begin colliding. /// @warning This function is locked during callbacks. public void DestroyJoint(Joint j) { Debug.Assert(!IsLocked); if (IsLocked) { return; } bool collideConnected = j._collideConnected; // Remove from the doubly linked list. if (j._prev != null) { j._prev._next = j._next; } if (j._next != null) { j._next._prev = j._prev; } if (j == _jointList) { _jointList = j._next; } // Disconnect from island graph. Body bodyA = j._bodyA; Body bodyB = j._bodyB; // Wake up connected bodies. bodyA.WakeUp(); bodyB.WakeUp(); // Remove from body 1. if (j._edgeA.Prev != null) { j._edgeA.Prev.Next = j._edgeA.Next; } if (j._edgeA.Next != null) { j._edgeA.Next.Prev = j._edgeA.Prev; } if (j._edgeA == bodyA._jointList) { bodyA._jointList = j._edgeA.Next; } j._edgeA.Prev = null; j._edgeA.Next = null; // Remove from body 2 if (j._edgeB.Prev != null) { j._edgeB.Prev.Next = j._edgeB.Next; } if (j._edgeB.Next != null) { j._edgeB.Next.Prev = j._edgeB.Prev; } if (j._edgeB == bodyB._jointList) { bodyB._jointList = j._edgeB.Next; } j._edgeB.Prev = null; j._edgeB.Next = null; Debug.Assert(_jointCount > 0); --_jointCount; // If the joint prevents collisions, then flag any contacts for filtering. if (collideConnected == false) { ContactEdge edge = bodyB.GetConactList(); while (edge != null) { if (edge.Other == bodyA) { // Flag the contact for filtering at the next time step (where either // body is awake). edge.Contact.FlagForFiltering(); } edge = edge.Next; } } }
void SolveTOI(ref TimeStep step) { // Reserve an island and a queue for TOI island solution. _island.Reset(_bodyCount, Settings.b2_maxTOIContactsPerIsland, Settings.b2_maxTOIJointsPerIsland, _contactManager.ContactListener); //Simple one pass queue //Relies on the fact that we're only making one pass //through and each body can only be pushed/popped once. //To push: // queue[queueStart+queueSize++] = newElement; //To pop: // poppedElement = queue[queueStart++]; // --queueSize; //#warning More Body array Allocs int queueCapacity = _bodyCount; Body[] queue = new Body[_bodyCount]; for (Body b = _bodyList; b != null; b = b._next) { b._flags &= ~BodyFlags.Island; b._sweep.t0 = 0.0f; } for (Contact c = _contactManager._contactList; c != null; c = c._next) { // Invalidate TOI c._flags &= ~(ContactFlags.Toi | ContactFlags.Island); } for (Joint j = _jointList; j != null; j = j._next) { j._islandFlag = false; } // Find TOI events and solve them. for (;;) { // Find the first TOI. Contact minContact = null; float minTOI = 1.0f; for (Contact c = _contactManager._contactList; c != null; c = c._next) { if ((c._flags & (ContactFlags.Slow | ContactFlags.NonSolid)) != ContactFlags.None) { continue; } // TODO_ERIN keep a counter on the contact, only respond to M TOIs per contact. float toi = 1.0f; if ((c._flags & ContactFlags.Toi) != ContactFlags.None) { // This contact has a valid cached TOI. toi = c._toi; } else { // Compute the TOI for this contact. Fixture s1 = c.GetFixtureA(); Fixture s2 = c.GetFixtureB(); Body b1 = s1.GetBody(); Body b2 = s2.GetBody(); if ((b1.IsStatic || b1.IsSleeping) && (b2.IsStatic || b2.IsSleeping)) { continue; } // Put the sweeps onto the same time interval. float t0 = b1._sweep.t0; if (b1._sweep.t0 < b2._sweep.t0) { t0 = b2._sweep.t0; b1._sweep.Advance(t0); } else if (b2._sweep.t0 < b1._sweep.t0) { t0 = b1._sweep.t0; b2._sweep.Advance(t0); } Debug.Assert(t0 < 1.0f); // Compute the time of impact. toi = c.ComputeTOI(ref b1._sweep, ref b2._sweep); //CalculateTimeOfImpact(c._fixtureA.GetShape(), b1._sweep, c._fixtureB.GetShape(), b2._sweep); Debug.Assert(0.0f <= toi && toi <= 1.0f); // If the TOI is in range ... if (0.0f < toi && toi < 1.0f) { // Interpolate on the actual range. toi = Math.Min((1.0f - toi) * t0 + toi, 1.0f); } c._toi = toi; c._flags |= ContactFlags.Toi; } if (Settings.b2_FLT_EPSILON < toi && toi < minTOI) { // This is the minimum TOI found so far. minContact = c; minTOI = toi; } } if (minContact == null || 1.0f - 100.0f * Settings.b2_FLT_EPSILON < minTOI) { // No more TOI events. Done! break; } // Advance the bodies to the TOI. Fixture s1_2 = minContact.GetFixtureA(); Fixture s2_2 = minContact.GetFixtureB(); Body b1_2 = s1_2.GetBody(); Body b2_2 = s2_2.GetBody(); b1_2.Advance(minTOI); b2_2.Advance(minTOI); // The TOI contact likely has some new contact points. minContact.Update(_contactManager.ContactListener); minContact._flags &= ~ContactFlags.Toi; if ((minContact._flags & ContactFlags.Touch) == 0) { // This shouldn't happen. Numerical error? //Debug.Assert(false); continue; } // Build the TOI island. We need a dynamic seed. Body seed = b1_2; if (seed.IsStatic) { seed = b2_2; } // Reset island and queue. _island.Clear(); int queueStart = 0; // starting index for queue int queueSize = 0; // elements in queue queue[queueStart + queueSize++] = seed; seed._flags |= BodyFlags.Island; // Perform a breadth first search (BFS) on the contact/joint graph. while (queueSize > 0) { // Grab the next body off the stack and add it to the island. Body b = queue[queueStart++]; --queueSize; _island.Add(b); // Make sure the body is awake. b._flags &= ~BodyFlags.Sleep; // To keep islands as small as possible, we don't // propagate islands across static bodies. if (b.IsStatic) { continue; } // Search all contacts connected to this body. for (ContactEdge cEdge = b._contactList; cEdge != null; cEdge = cEdge.Next) { // Does the TOI island still have space for contacts? if (_island._contacts.Count == _island._contactCapacity) { continue; } // Has this contact already been added to an island? Skip slow or non-solid contacts. if ((cEdge.Contact._flags & (ContactFlags.Island | ContactFlags.Slow | ContactFlags.NonSolid)) != ContactFlags.None) { continue; } // Is this contact touching? For performance we are not updating this contact. if ((cEdge.Contact._flags & ContactFlags.Touch) == 0) { continue; } _island.Add(cEdge.Contact); cEdge.Contact._flags |= ContactFlags.Island; // Update other body. Body other = cEdge.Other; // Was the other body already added to this island? if ((other._flags & BodyFlags.Island) != BodyFlags.None) { continue; } // March forward, this can do no harm since this is the min TOI. if (other.IsStatic == false) { other.Advance(minTOI); other.WakeUp(); } Debug.Assert(queueStart + queueSize < queueCapacity); queue[queueStart + queueSize] = other; ++queueSize; other._flags |= BodyFlags.Island; } for (JointEdge jEdge = b._jointList; jEdge != null; jEdge = jEdge.Next) { if (_island._jointCount == _island._jointCapacity) { continue; } if (jEdge.Joint._islandFlag == true) { continue; } _island.Add(jEdge.Joint); jEdge.Joint._islandFlag = true; Body other = jEdge.Other; if ((other._flags & BodyFlags.Island) != BodyFlags.None) { continue; } if (!other.IsStatic) { other.Advance(minTOI); other.WakeUp(); } Debug.Assert(queueStart + queueSize < queueCapacity); queue[queueStart + queueSize] = other; ++queueSize; other._flags |= BodyFlags.Island; } } TimeStep subStep; subStep.warmStarting = false; subStep.dt = (1.0f - minTOI) * step.dt; subStep.inv_dt = 1.0f / subStep.dt; subStep.dtRatio = 0.0f; subStep.velocityIterations = step.velocityIterations; subStep.positionIterations = step.positionIterations; _island.SolveTOI(ref subStep); // Post solve cleanup. for (int i = 0; i < _island._bodyCount; ++i) { // Allow bodies to participate in future TOI islands. Body b = _island._bodies[i]; b._flags &= ~BodyFlags.Island; if ((b._flags & (BodyFlags.Sleep | BodyFlags.Frozen)) != BodyFlags.None) { continue; } if (b.IsStatic) { continue; } // Update fixtures (for broad-phase). b.SynchronizeFixtures(); // Invalidate all contact TOIs associated with this body. Some of these // may not be in the island because they were not touching. for (ContactEdge ce = b._contactList; ce != null; ce = ce.Next) { ce.Contact._flags &= ~ContactFlags.Toi; } } int contactCount = _island._contacts.Count; for (int i = 0; i < contactCount; ++i) { // Allow contacts to participate in future TOI islands. Contact c = _island._contacts[i]; c._flags &= ~(ContactFlags.Toi | ContactFlags.Island); } for (int i = 0; i < _island._jointCount; ++i) { // Allow joints to participate in future TOI islands. Joint j = _island._joints[i]; j._islandFlag = false; } // Commit fixture proxy movements to the broad-phase so that new contacts are created. // Also, some contacts can be destroyed. _contactManager.FindNewContacts(); } }
internal void Update(IContactListener listener) { Manifold oldManifold = _manifold; Evaluate(); Body bodyA = _fixtureA.GetBody(); Body bodyB = _fixtureB.GetBody(); int oldCount = oldManifold._pointCount; int newCount = _manifold._pointCount; if (newCount == 0 && oldCount > 0) { bodyA.WakeUp(); bodyB.WakeUp(); } // Slow contacts don't generate TOI events. if (bodyA.IsStatic || bodyA.IsBullet || bodyB.IsStatic || bodyB.IsBullet) { _flags &= ~ContactFlags.Slow; } else { _flags |= ContactFlags.Slow; } // 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; 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; break; } } _manifold._points[i] = mp2; } if (oldCount == 0 && newCount > 0) { _flags |= ContactFlags.Touch; listener.BeginContact(this); } if (oldCount > 0 && newCount == 0) { _flags &= ~ContactFlags.Touch; listener.EndContact(this); } if ((_flags & ContactFlags.NonSolid) == 0) { listener.PreSolve(this, ref oldManifold); // The user may have disabled contact. if (_manifold._pointCount == 0) { _flags &= ~ContactFlags.Touch; } } }