public void Destroy(Contact c) { Fixture fixtureA = c.FixtureA; Fixture fixtureB = c.FixtureB; Body bodyA = fixtureA.GetBody(); Body bodyB = fixtureB.GetBody(); if (m_contactListener != null && c.IsTouching()) { m_contactListener.EndContact(c); } // Remove from the world. m_contactList.Remove(c); // Remove from body 1 c.m_nodeA.other.m_contactList.Remove(c.m_nodeA); // Remove from body 2 c.m_nodeB.other.m_contactList.Remove(c.m_nodeA); // Call the factory. }
internal void Destroy(Contact c) { Fixture fixtureA = c.GetFixtureA(); Fixture fixtureB = c.GetFixtureB(); Body bodyA = fixtureA.GetBody(); Body bodyB = fixtureB.GetBody(); if (ContactListener != null && c.IsTouching()) { ContactListener.EndContact(c); } // Remove from the world. if (c._prev != null) { c._prev._next = c._next; } if (c._next != null) { c._next._prev = c._prev; } if (c == _contactList) { _contactList = c._next; } // Remove from body 1 if (c._nodeA.Prev != null) { c._nodeA.Prev.Next = c._nodeA.Next; } if (c._nodeA.Next != null) { c._nodeA.Next.Prev = c._nodeA.Prev; } if (c._nodeA == bodyA._contactList) { bodyA._contactList = c._nodeA.Next; } // Remove from body 2 if (c._nodeB.Prev != null) { c._nodeB.Prev.Next = c._nodeB.Next; } if (c._nodeB.Next != null) { c._nodeB.Next.Prev = c._nodeB.Prev; } if (c._nodeB == bodyB._contactList) { bodyB._contactList = c._nodeB.Next; } c.Destroy(); --_contactCount; }
// 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; } }
private void Solve(TimeStep step) { m_profile.solveInit = 0.0f; m_profile.solveVelocity = 0.0f; m_profile.solvePosition = 0.0f; // Size the island for the worst case. Island island = new Island(m_contactManager.m_contactListener); // Clear all the island flags. foreach (Body b in m_bodyList) { b.m_flags &= ~Body.BodyFlags.e_islandFlag; } foreach (Contact c in m_contactManager.m_contactList) { c.m_flags &= ~ContactFlags.e_islandFlag; } foreach (Joint j in m_jointList) { j.m_islandFlag = false; } // Build and simulate all awake islands. List <Body> stack = new List <Body>(m_bodyList.Count()); foreach (Body seed in m_bodyList) { if (seed.m_flags.HasFlag(Body.BodyFlags.e_islandFlag)) { continue; } if (seed.IsAwake() == false || seed.IsActive() == false) { continue; } // The seed can be dynamic or kinematic. if (seed.GetBodyType() == BodyType._staticBody) { continue; } // Reset island and stack. island.Clear(); int stackCount = 0; stack.Add(seed); stackCount++; seed.m_flags |= Body.BodyFlags.e_islandFlag; // Perform a depth first search (DFS) on the constraint graph. while (stackCount > 0) { // Grab the next body off the stack and add it to the island. Body b = stack[--stackCount]; Utilities.Assert(b.IsActive() == true); island.Add(b); // Make sure the body is awake. b.SetAwake(true); // To keep islands as small as possible, we don't // propagate islands across static bodies. if (b.GetBodyType() == BodyType._staticBody) { continue; } // Search all contacts connected to this body. foreach (ContactEdge ce in b.m_contactList) { Contact contact = ce.contact; // Has this contact already been added to an island? if (contact.m_flags.HasFlag(ContactFlags.e_islandFlag)) { continue; } // Is this contact solid and touching? if (contact.IsEnabled() == false || contact.IsTouching() == false) { continue; } // Skip sensors. bool sensorA = contact.m_fixtureA.m_isSensor; bool sensorB = contact.m_fixtureB.m_isSensor; if (sensorA || sensorB) { continue; } island.Add(contact); contact.m_flags |= ContactFlags.e_islandFlag; Body other = ce.other; // Was the other body already added to this island? if (other.m_flags.HasFlag(Body.BodyFlags.e_islandFlag)) { continue; } Utilities.Assert(stackCount < m_bodyList.Count()); stack.Add(other); stackCount++; other.m_flags |= Body.BodyFlags.e_islandFlag; } // Search all joints connect to this body. foreach (JointEdge je in b.m_jointList) { if (je.joint.m_islandFlag == true) { continue; } Body other = je.other; // Don't simulate joints connected to inactive bodies. if (other.IsActive() == false) { continue; } island.Add(je.joint); je.joint.m_islandFlag = true; if (other.m_flags.HasFlag(Body.BodyFlags.e_islandFlag)) { continue; } stack.Add(other); stackCount++; other.m_flags |= Body.BodyFlags.e_islandFlag; } } Profile profile = new Profile(); island.Solve(profile, step, m_gravity, m_allowSleep); m_profile.solveInit += profile.solveInit; m_profile.solveVelocity += profile.solveVelocity; m_profile.solvePosition += profile.solvePosition; // Post solve cleanup. for (int i = 0; i < island.m_bodies.Count(); ++i) { // Allow static bodies to participate in other islands. Body b = island.m_bodies[i]; if (b.GetBodyType() == BodyType._staticBody) { b.m_flags &= ~Body.BodyFlags.e_islandFlag; } } } { Timer timer = new Timer(); // Synchronize fixtures, check for out of range bodies. foreach (Body b in m_bodyList) { // If a body was not in an island then it did not move. if ((b.m_flags & Body.BodyFlags.e_islandFlag) == 0) { continue; } if (b.GetBodyType() == BodyType._staticBody) { continue; } // Update fixtures (for broad-phase). b.SynchronizeFixtures(); } // Look for new contacts. m_contactManager.FindNewContacts(); m_profile.broadphase = timer.GetMilliseconds(); } }
private void SolveTOI(TimeStep step) { Island island = new Island(m_contactManager.m_contactListener); if (m_stepComplete) { foreach (Body b in m_bodyList) { b.m_flags &= ~Body.BodyFlags.e_islandFlag; b.m_sweep.alpha0 = 0.0f; } foreach (Contact c in m_contactManager.m_contactList) { // Invalidate TOI c.m_flags &= ~(ContactFlags.e_toiFlag | ContactFlags.e_islandFlag); c.m_toiCount = 0; c.m_toi = 1.0f; } } Fixture fA = null; Fixture fB = null; Body bA = null; Body bB = null; // Find TOI events and solve them. for (;;) { // Find the first TOI. Contact minContact = null; float minAlpha = 1.0f; foreach (Contact c in m_contactManager.m_contactList) { // Is this contact disabled? if (c.IsEnabled() == false) { continue; } // Prevent excessive sub-stepping. if (c.m_toiCount > Settings._maxSubSteps) { continue; } float alpha = 1.0f; if (c.m_flags.HasFlag(ContactFlags.e_toiFlag)) { // This contact has a valid cached TOI. alpha = c.m_toi; } else { fA = c.FixtureA; fB = c.FixtureB; // Is there a sensor? if (fA.IsSensor || fB.IsSensor) { continue; } bA = fA.GetBody(); bB = fB.GetBody(); BodyType typeA = bA.m_type; BodyType typeB = bB.m_type; Utilities.Assert(typeA == BodyType._dynamicBody || typeB == BodyType._dynamicBody); bool activeA = bA.IsAwake() && typeA != BodyType._staticBody; bool activeB = bB.IsAwake() && typeB != BodyType._staticBody; // Is at least one body active (awake and dynamic or kinematic)? if (activeA == false && activeB == false) { continue; } bool collideA = bA.IsBullet() || typeA != BodyType._dynamicBody; bool collideB = bB.IsBullet() || typeB != BodyType._dynamicBody; // Are these two non-bullet dynamic bodies? if (collideA == false && collideB == false) { continue; } // Compute the TOI for this contact. // Put the sweeps onto the same time interval. float alpha0 = bA.m_sweep.alpha0; if (bA.m_sweep.alpha0 < bB.m_sweep.alpha0) { alpha0 = bB.m_sweep.alpha0; bA.m_sweep.Advance(alpha0); } else if (bB.m_sweep.alpha0 < bA.m_sweep.alpha0) { alpha0 = bA.m_sweep.alpha0; bB.m_sweep.Advance(alpha0); } Utilities.Assert(alpha0 < 1.0f); int indexA = c.GetChildIndexA(); int indexB = c.GetChildIndexB(); // Compute the time of impact in interval [0, minTOI] TOIInput input = new TOIInput(); input.proxyA.Set(fA.GetShape(), indexA); input.proxyB.Set(fB.GetShape(), indexB); input.sweepA = bA.m_sweep; input.sweepB = bB.m_sweep; input.tMax = 1.0f; TOIOutput output; Utilities.TimeOfImpact(out output, input); // Beta is the fraction of the remaining portion of the . float beta = output.t; if (output.state == TOIOutput.State.e_touching) { alpha = Math.Min(alpha0 + (1.0f - alpha0) * beta, 1.0f); } else { alpha = 1.0f; } c.m_toi = alpha; c.m_flags |= ContactFlags.e_toiFlag; } if (alpha < minAlpha) { // This is the minimum TOI found so far. minContact = c; minAlpha = alpha; } } if (minContact == null || 1.0f - 10.0f * Single.Epsilon < minAlpha) { // No more TOI events. Done! m_stepComplete = true; break; } // Advance the bodies to the TOI. fA = minContact.FixtureA; fB = minContact.FixtureB; bA = fA.GetBody(); bB = fB.GetBody(); Sweep backup1 = bA.m_sweep; Sweep backup2 = bB.m_sweep; bA.Advance(minAlpha); bB.Advance(minAlpha); // The TOI contact likely has some new contact points. minContact.Update(m_contactManager.m_contactListener); minContact.m_flags &= ~ContactFlags.e_toiFlag; ++minContact.m_toiCount; // Is the contact solid? if (minContact.IsEnabled() == false || minContact.IsTouching() == false) { // Restore the sweeps. minContact.SetEnabled(false); bA.m_sweep = backup1; bB.m_sweep = backup2; bA.SynchronizeTransform(); bB.SynchronizeTransform(); continue; } bA.SetAwake(true); bB.SetAwake(true); // Build the island island.Clear(); island.Add(bA); island.Add(bB); island.Add(minContact); bA.m_flags |= Body.BodyFlags.e_islandFlag; bB.m_flags |= Body.BodyFlags.e_islandFlag; minContact.m_flags |= ContactFlags.e_islandFlag; // Get contacts on bodyA and bodyB. Body[] bodies = { bA, bB }; for (int i = 0; i < 2; ++i) { Body body = bodies[i]; if (body.m_type == BodyType._dynamicBody) { foreach (ContactEdge ce in body.m_contactList) { throw new NotImplementedException(); //if (island.m_bodies.Count() == island.m_bodyCapacity) //{ // break; //} //if (island.m_bodies.Count() == island.m_contactCapacity) //{ // break; //} //Contact* contact = ce.contact; //// Has this contact already been added to the island? //if (contact.m_flags & ContactFlags.e_islandFlag) //{ // continue; //} //// Only add static, kinematic, or bullet bodies. //Body* other = ce.other; //if (other.m_type == _dynamicBody && // body.IsBullet() == false && other.IsBullet() == false) //{ // continue; //} //// Skip sensors. //bool sensorA = contact.m_fixtureA.m_isSensor; //bool sensorB = contact.m_fixtureB.m_isSensor; //if (sensorA || sensorB) //{ // continue; //} //// Tentatively advance the body to the TOI. //Sweep backup = other.m_sweep; //if ((other.m_flags & Body.BodyFlags.e_islandFlag) == 0) //{ // other.Advance(minAlpha); //} //// Update the contact points //contact.Update(m_contactManager.m_contactListener); //// Was the contact disabled by the user? //if (contact.IsEnabled() == false) //{ // other.m_sweep = backup; // other.SynchronizeTransform(); // continue; //} //// Are there contact points? //if (contact.IsTouching() == false) //{ // other.m_sweep = backup; // other.SynchronizeTransform(); // continue; //} //// Add the contact to the island //contact.m_flags |= ContactFlags.e_islandFlag; //island.Add(contact); //// Has the other body already been added to the island? //if (other.m_flags & Body.BodyFlags.e_islandFlag) //{ // continue; //} //// Add the other body to the island. //other.m_flags |= Body.BodyFlags.e_islandFlag; //if (other.m_type != _staticBody) //{ // other.SetAwake(true); //} //island.Add(other); } } } TimeStep subStep; subStep.dt = (1.0f - minAlpha) * step.dt; subStep.inv_dt = 1.0f / subStep.dt; subStep.dtRatio = 1.0f; subStep.positionIterations = 20; subStep.velocityIterations = step.velocityIterations; subStep.warmStarting = false; island.SolveTOI(subStep, bA.m_islandIndex, bB.m_islandIndex); // Reset island flags and synchronize broad-phase proxies. for (int i = 0; i < island.m_bodies.Count(); ++i) { throw new NotImplementedException(); //Body* body = island.m_bodies[i]; //body.m_flags &= ~Body.BodyFlags.e_islandFlag; //if (body.m_type != _dynamicBody) //{ // continue; //} //body.SynchronizeFixtures(); //// Invalidate all contact TOIs on this displaced body. //for (ContactEdge* ce = body.m_contactList; ce; ce = ce.next) //{ // ce.contact.m_flags &= ~(ContactFlags.e_toiFlag | ContactFlags.e_islandFlag); //} } // Commit fixture proxy movements to the broad-phase so that new contacts are created. // Also, some contacts can be destroyed. m_contactManager.FindNewContacts(); if (m_subStepping) { m_stepComplete = false; break; } } }