// Advance a dynamic body to its first time of contact // and adjust the position to ensure clearance. void SolveTOI(Body body) { // Find the minimum contact. Contact toiContact = null; float toi = 1.0f; Body toiOther = null; bool found; int count; int iter = 0; bool bullet = body.IsBullet; // Iterate until all contacts agree on the minimum TOI. We have // to iterate because the TOI algorithm may skip some intermediate // collisions when objects rotate through each other. do { count = 0; found = false; for (ContactEdge ce = body._contactList; ce != null; ce = ce.Next) { if (ce.Contact == toiContact) { continue; } Body other = ce.Other; BodyType type = other.GetType(); // Only bullets perform TOI with dynamic bodies. if (bullet == true) { // Bullets only perform TOI with bodies that have their TOI resolved. if ((other._flags & BodyFlags.Toi) == 0) { continue; } // No repeated hits on non-static bodies if (type != BodyType.Static && (ce.Contact._flags & ContactFlags.BulletHit) != 0) { continue; } } else if (type == BodyType.Dynamic) { continue; } // Check for a disabled contact. Contact contact = ce.Contact; if (contact.IsEnabled() == false) { continue; } // Prevent infinite looping. if (contact._toiCount > 10) { continue; } Fixture fixtureA = contact._fixtureA; Fixture fixtureB = contact._fixtureB; int indexA = contact._indexA; int indexB = contact._indexB; // Cull sensors. if (fixtureA.IsSensor() || fixtureB.IsSensor()) { continue; } Body bodyA = fixtureA._body; Body bodyB = fixtureB._body; // Compute the time of impact in interval [0, minTOI] TOIInput input = new TOIInput(); input.proxyA.Set(fixtureA.GetShape(), indexA); input.proxyB.Set(fixtureB.GetShape(), indexB); input.sweepA = bodyA._sweep; input.sweepB = bodyB._sweep; input.tMax = toi; TOIOutput output; TimeOfImpact.CalculateTimeOfImpact(out output, ref input); if (output.State == TOIOutputState.Touching && output.t < toi) { toiContact = contact; toi = output.t; toiOther = other; found = true; } ++count; } ++iter; } while (found && count > 1 && iter < 50); if (toiContact == null) { body.Advance(1.0f); return; } Sweep backup = body._sweep; body.Advance(toi); toiContact.Update(_contactManager.ContactListener); if (toiContact.IsEnabled() == false) { // Contact disabled. Backup and recurse. body._sweep = backup; SolveTOI(body); } ++toiContact._toiCount; // Update all the valid contacts on this body and build a contact island. count = 0; for (ContactEdge ce = body._contactList; (ce != null) && (count < Settings.b2_maxTOIContacts); ce = ce.Next) { Body other = ce.Other; BodyType type = other.GetType(); // Only perform correction with static bodies, so the // body won't get pushed out of the world. if (type == BodyType.Dynamic) { continue; } // Check for a disabled contact. Contact contact = ce.Contact; if (contact.IsEnabled() == false) { continue; } Fixture fixtureA = contact._fixtureA; Fixture fixtureB = contact._fixtureB; // Cull sensors. if (fixtureA.IsSensor() || fixtureB.IsSensor()) { continue; } // The contact likely has some new contact points. The listener // gives the user a chance to disable the contact. if (contact != toiContact) { contact.Update(_contactManager.ContactListener); } // Did the user disable the contact? if (contact.IsEnabled() == false) { // Skip this contact. continue; } if (contact.IsTouching() == false) { continue; } _toiContacts[count] = contact; ++count; } // Reduce the TOI body's overlap with the contact island. _toiSolver.Initialize(_toiContacts, count, body); float k_toiBaumgarte = 0.75f; //bool solved = false; for (int i = 0; i < 20; ++i) { bool contactsOkay = _toiSolver.Solve(k_toiBaumgarte); if (contactsOkay) { //solved = true; break; } } if (toiOther.GetType() != BodyType.Static) { toiContact._flags |= ContactFlags.BulletHit; } }