internal void SolveTOI(ref TimeStep subStep) { _contactSolver.Reset(_contacts, ContactCount, subStep.dtRatio, false); // Solve position constraints. const float kTOIBaumgarte = 0.75f; for (int i = 0; i < Settings.TOIPositionIterations; ++i) { bool contactsOkay = _contactSolver.SolvePositionConstraints(kTOIBaumgarte); if (contactsOkay) { break; } if (i == Settings.TOIPositionIterations - 1) { i += 0; } } // Leap of faith to new safe state. for (int i = 0; i < BodyCount; ++i) { PhysicsBody body = Bodies[i]; body.Sweep.A0 = body.Sweep.A; body.Sweep.C0 = body.Sweep.C; } // No warm starting is needed for TOI events because warm // starting impulses were applied in the discrete solver. _contactSolver.InitializeVelocityConstraints(); // Solve velocity constraints. for (int i = 0; i < Settings.TOIVelocityIterations; ++i) { _contactSolver.SolveVelocityConstraints(); } // Don't store the TOI contact forces for warm starting // because they can be quite large. // Integrate positions. for (int i = 0; i < BodyCount; ++i) { PhysicsBody b = Bodies[i]; if (b.BodyType == BodyType.Static) { continue; } // Check for large velocities. float translationx = subStep.dt * b.LinearVelocityInternal.X; float translationy = subStep.dt * b.LinearVelocityInternal.Y; float dot = translationx * translationx + translationy * translationy; if (dot > Settings.MaxTranslationSquared) { float norm = 1f / (float)Math.Sqrt(dot); float value = Settings.MaxTranslation * subStep.inv_dt; b.LinearVelocityInternal.X = value * (translationx * norm); b.LinearVelocityInternal.Y = value * (translationy * norm); } float rotation = subStep.dt * b.AngularVelocity; if (rotation * rotation > Settings.MaxRotationSquared) { if (rotation < 0.0) { b.AngularVelocityInternal = -subStep.inv_dt * Settings.MaxRotation; } else { b.AngularVelocityInternal = subStep.inv_dt * Settings.MaxRotation; } } // Integrate b.Sweep.C.X += subStep.dt * b.LinearVelocityInternal.X; b.Sweep.C.Y += subStep.dt * b.LinearVelocityInternal.Y; b.Sweep.A += subStep.dt * b.AngularVelocityInternal; // Compute new transform b.SynchronizeTransform(); // Note: shapes are synchronized later. } Report(_contactSolver.Constraints); }
/// <summary> /// Find TOI contacts and solve them. /// </summary> /// <param name="step">The step.</param> private void SolveTOI(ref TimeStep step) { Island.Reset(2 * Settings.MaxTOIContacts, Settings.MaxTOIContacts, 0, ContactManager); if (_stepComplete) { for (int i = 0; i < BodyList.Count; i++) { BodyList[i].Flags &= ~BodyFlags.Island; BodyList[i].Sweep.Alpha0 = 0.0f; } for (int i = 0; i < ContactManager.ContactList.Count; i++) { Contact c = ContactManager.ContactList[i]; // Invalidate TOI c.Flags &= ~(ContactFlags.TOI | ContactFlags.Island); c.TOICount = 0; c.TOI = 1.0f; } } // Find TOI events and solve them. for (; ; ) { // Find the first TOI. Contact minContact = null; float minAlpha = 1.0f; for (int i = 0; i < ContactManager.ContactList.Count; i++) { Contact c = ContactManager.ContactList[i]; // Is this contact disabled? if (c.Enabled == false) { continue; } // Prevent excessive sub-stepping. if (c.TOICount > Settings.MaxSubSteps) { continue; } float alpha; if ((c.Flags & ContactFlags.TOI) == ContactFlags.TOI) { // This contact has a valid cached TOI. alpha = c.TOI; } else { Fixture fA = c.FixtureA; Fixture fB = c.FixtureB; // Is there a sensor? if (fA.IsSensor || fB.IsSensor) { continue; } PhysicsBody bA = fA.Body; PhysicsBody bB = fB.Body; BodyType typeA = bA.BodyType; BodyType typeB = bB.BodyType; bool awakeA = bA.Awake && typeA != BodyType.Static; bool awakeB = bB.Awake && typeB != BodyType.Static; // Is at least one body awake? if (awakeA == false && awakeB == false) { continue; } bool collideA = (bA.IsBullet || typeA != BodyType.Dynamic) && !bA.IgnoreCCD; bool collideB = (bB.IsBullet || typeB != BodyType.Dynamic) && !bB.IgnoreCCD; // 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.Sweep.Alpha0; if (bA.Sweep.Alpha0 < bB.Sweep.Alpha0) { alpha0 = bB.Sweep.Alpha0; bA.Sweep.Advance(alpha0); } else if (bB.Sweep.Alpha0 < bA.Sweep.Alpha0) { alpha0 = bA.Sweep.Alpha0; bB.Sweep.Advance(alpha0); } Debug.Assert(alpha0 < 1.0f); // Compute the time of impact in interval [0, minTOI] _input.ProxyA.Set(fA.Shape, c.ChildIndexA); _input.ProxyB.Set(fB.Shape, c.ChildIndexB); _input.SweepA = bA.Sweep; _input.SweepB = bB.Sweep; _input.TMax = 1.0f; TOIOutput output; TimeOfImpact.CalculateTimeOfImpact(out output, _input); // Beta is the fraction of the remaining portion of the . float beta = output.T; if (output.State == TOIOutputState.Touching) { alpha = Math.Min(alpha0 + (1.0f - alpha0) * beta, 1.0f); } else { alpha = 1.0f; } c.TOI = alpha; c.Flags |= ContactFlags.TOI; } if (alpha < minAlpha) { // This is the minimum TOI found so far. minContact = c; minAlpha = alpha; } } if (minContact == null || 1.0f - 10.0f * Settings.Epsilon < minAlpha) { // No more TOI events. Done! _stepComplete = true; break; } // Advance the bodies to the TOI. Fixture fA1 = minContact.FixtureA; Fixture fB1 = minContact.FixtureB; PhysicsBody bA1 = fA1.Body; PhysicsBody bB1 = fB1.Body; Sweep backup1 = bA1.Sweep; Sweep backup2 = bB1.Sweep; bA1.Advance(minAlpha); bB1.Advance(minAlpha); // The TOI contact likely has some new contact points. minContact.Update(ContactManager); minContact.Flags &= ~ContactFlags.TOI; ++minContact.TOICount; // Is the contact solid? if (minContact.Enabled == false || minContact.IsTouching() == false) { // Restore the sweeps. minContact.Enabled = false; bA1.Sweep = backup1; bB1.Sweep = backup2; bA1.SynchronizeTransform(); bB1.SynchronizeTransform(); continue; } bA1.Awake = true; bB1.Awake = true; // Build the island Island.Clear(); Island.Add(bA1); Island.Add(bB1); Island.Add(minContact); bA1.Flags |= BodyFlags.Island; bB1.Flags |= BodyFlags.Island; minContact.Flags |= ContactFlags.Island; // Get contacts on bodyA and bodyB. PhysicsBody[] bodies = { bA1, bB1 }; for (int i = 0; i < 2; ++i) { PhysicsBody body = bodies[i]; if (body.BodyType == BodyType.Dynamic) { // for (ContactEdge ce = body.ContactList; ce && Island.BodyCount < Settings.MaxTOIContacts; ce = ce.Next) for (ContactEdge ce = body.ContactList; ce != null; ce = ce.Next) { Contact contact = ce.Contact; // Has this contact already been added to the island? if ((contact.Flags & ContactFlags.Island) == ContactFlags.Island) { continue; } // Only add static, kinematic, or bullet bodies. PhysicsBody other = ce.Other; if (other.BodyType == BodyType.Dynamic && body.IsBullet == false && other.IsBullet == false) { continue; } // Skip sensors. if (contact.FixtureA.IsSensor || contact.FixtureB.IsSensor) { continue; } // Tentatively advance the body to the TOI. Sweep backup = other.Sweep; if ((other.Flags & BodyFlags.Island) == 0) { other.Advance(minAlpha); } // Update the contact points contact.Update(ContactManager); // Was the contact disabled by the user? if (contact.Enabled == false) { other.Sweep = backup; other.SynchronizeTransform(); continue; } // Are there contact points? if (contact.IsTouching() == false) { other.Sweep = backup; other.SynchronizeTransform(); continue; } // Add the contact to the island contact.Flags |= ContactFlags.Island; Island.Add(contact); // Has the other body already been added to the island? if ((other.Flags & BodyFlags.Island) == BodyFlags.Island) { continue; } // Add the other body to the island. other.Flags |= BodyFlags.Island; if (other.BodyType != BodyType.Static) { other.Awake = 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(ref subStep); // Reset island flags and synchronize broad-phase proxies. for (int i = 0; i < Island.BodyCount; ++i) { PhysicsBody body = Island.Bodies[i]; body.Flags &= ~BodyFlags.Island; if (body.BodyType != BodyType.Dynamic) { continue; } body.SynchronizeFixtures(); // Invalidate all contact TOIs on this displaced body. for (ContactEdge ce = body.ContactList; ce != null; ce = ce.Next) { ce.Contact.Flags &= ~(ContactFlags.TOI | ContactFlags.Island); } } // Commit fixture proxy movements to the broad-phase so that new contacts are created. // Also, some contacts can be destroyed. ContactManager.FindNewContacts(); if (EnableSubStepping) { _stepComplete = false; break; } } }
public void Solve(ref TimeStep step, ref Vector2 gravity) { // Integrate velocities and apply damping. for (int i = 0; i < BodyCount; ++i) { PhysicsBody b = Bodies[i]; if (b.BodyType != BodyType.Dynamic) { continue; } // Integrate velocities. // FPE 3 only - Only apply gravity if the body wants it. if (b.IgnoreGravity) { b.LinearVelocityInternal.X += step.dt * (b.InvMass * b.Force.X); b.LinearVelocityInternal.Y += step.dt * (b.InvMass * b.Force.Y); b.AngularVelocityInternal += step.dt * b.InvI * b.Torque; } else { b.LinearVelocityInternal.X += step.dt * (gravity.X + b.InvMass * b.Force.X); b.LinearVelocityInternal.Y += step.dt * (gravity.Y + b.InvMass * b.Force.Y); b.AngularVelocityInternal += step.dt * b.InvI * b.Torque; } // Apply damping. // ODE: dv/dt + c * v = 0 // Solution: v(t) = v0 * exp(-c * t) // Time step: v(t + dt) = v0 * exp(-c * (t + dt)) = v0 * exp(-c * t) * exp(-c * dt) = v * exp(-c * dt) // v2 = exp(-c * dt) * v1 // Taylor expansion: // v2 = (1.0f - c * dt) * v1 b.LinearVelocityInternal *= MathUtils.Clamp(1.0f - step.dt * b.LinearDamping, 0.0f, 1.0f); b.AngularVelocityInternal *= MathUtils.Clamp(1.0f - step.dt * b.AngularDamping, 0.0f, 1.0f); } // Partition contacts so that contacts with static bodies are solved last. int i1 = -1; for (int i2 = 0; i2 < ContactCount; ++i2) { Fixture fixtureA = _contacts[i2].FixtureA; Fixture fixtureB = _contacts[i2].FixtureB; PhysicsBody bodyA = fixtureA.Body; PhysicsBody bodyB = fixtureB.Body; bool nonStatic = bodyA.BodyType != BodyType.Static && bodyB.BodyType != BodyType.Static; if (nonStatic) { ++i1; //TODO: Only swap if they are not the same? see http://code.google.com/p/box2d/issues/detail?id=162 Contact tmp = _contacts[i1]; _contacts[i1] = _contacts[i2]; _contacts[i2] = tmp; } } // Initialize velocity constraints. _contactSolver.Reset(_contacts, ContactCount, step.dtRatio, Settings.EnableWarmstarting); _contactSolver.InitializeVelocityConstraints(); if (Settings.EnableWarmstarting) { _contactSolver.WarmStart(); } #if (!SILVERLIGHT) if (Settings.EnableDiagnostics) { _watch.Start(); _tmpTime = 0; } #endif for (int i = 0; i < JointCount; ++i) { if (_joints[i].Enabled) _joints[i].InitVelocityConstraints(ref step); } #if (!SILVERLIGHT) if (Settings.EnableDiagnostics) { _tmpTime += _watch.ElapsedTicks; } #endif // Solve velocity constraints. for (int i = 0; i < Settings.VelocityIterations; ++i) { #if (!SILVERLIGHT) if (Settings.EnableDiagnostics) _watch.Start(); #endif for (int j = 0; j < JointCount; ++j) { PhysicsJoint joint = _joints[j]; if (!joint.Enabled) continue; joint.SolveVelocityConstraints(ref step); joint.Validate(step.inv_dt); } #if (!SILVERLIGHT) if (Settings.EnableDiagnostics) { _watch.Stop(); _tmpTime += _watch.ElapsedTicks; _watch.Reset(); } #endif _contactSolver.SolveVelocityConstraints(); } // Post-solve (store impulses for warm starting). _contactSolver.StoreImpulses(); // Integrate positions. for (int i = 0; i < BodyCount; ++i) { PhysicsBody b = Bodies[i]; if (b.BodyType == BodyType.Static) { continue; } // Check for large velocities. float translationX = step.dt * b.LinearVelocityInternal.X; float translationY = step.dt * b.LinearVelocityInternal.Y; float result = translationX * translationX + translationY * translationY; if (result > Settings.MaxTranslationSquared) { float sq = (float)Math.Sqrt(result); float ratio = Settings.MaxTranslation / sq; b.LinearVelocityInternal.X *= ratio; b.LinearVelocityInternal.Y *= ratio; } float rotation = step.dt * b.AngularVelocityInternal; if (rotation * rotation > Settings.MaxRotationSquared) { float ratio = Settings.MaxRotation / Math.Abs(rotation); b.AngularVelocityInternal *= ratio; } // Store positions for continuous collision. b.Sweep.C0.X = b.Sweep.C.X; b.Sweep.C0.Y = b.Sweep.C.Y; b.Sweep.A0 = b.Sweep.A; // Integrate b.Sweep.C.X += step.dt * b.LinearVelocityInternal.X; b.Sweep.C.Y += step.dt * b.LinearVelocityInternal.Y; b.Sweep.A += step.dt * b.AngularVelocityInternal; // Compute new transform b.SynchronizeTransform(); // Note: shapes are synchronized later. } // Iterate over constraints. for (int i = 0; i < Settings.PositionIterations; ++i) { bool contactsOkay = _contactSolver.SolvePositionConstraints(Settings.ContactBaumgarte); bool jointsOkay = true; #if (!SILVERLIGHT) if (Settings.EnableDiagnostics) _watch.Start(); #endif for (int j = 0; j < JointCount; ++j) { PhysicsJoint joint = _joints[j]; if (!joint.Enabled) continue; bool jointOkay = joint.SolvePositionConstraints(); jointsOkay = jointsOkay && jointOkay; } #if (!SILVERLIGHT) if (Settings.EnableDiagnostics) { _watch.Stop(); _tmpTime += _watch.ElapsedTicks; _watch.Reset(); } #endif if (contactsOkay && jointsOkay) { // Exit early if the position errors are small. break; } } #if (!SILVERLIGHT) if (Settings.EnableDiagnostics) { JointUpdateTime = _tmpTime; } #endif Report(_contactSolver.Constraints); if (Settings.AllowSleep) { float minSleepTime = Settings.MaxFloat; for (int i = 0; i < BodyCount; ++i) { PhysicsBody b = Bodies[i]; if (b.BodyType == BodyType.Static) { continue; } if ((b.Flags & BodyFlags.AutoSleep) == 0) { b.SleepTime = 0.0f; minSleepTime = 0.0f; } if ((b.Flags & BodyFlags.AutoSleep) == 0 || b.AngularVelocityInternal * b.AngularVelocityInternal > AngTolSqr || Vector2.Dot(b.LinearVelocityInternal, b.LinearVelocityInternal) > LinTolSqr) { b.SleepTime = 0.0f; minSleepTime = 0.0f; } else { b.SleepTime += step.dt; minSleepTime = Math.Min(minSleepTime, b.SleepTime); } } if (minSleepTime >= Settings.TimeToSleep) { for (int i = 0; i < BodyCount; ++i) { PhysicsBody b = Bodies[i]; b.Awake = false; } } } }
private void Solve(ref TimeStep step) { // Size the island for the worst case. Island.Reset(BodyList.Count, ContactManager.ContactList.Count, JointList.Count, ContactManager); // Clear all the island flags. foreach (PhysicsBody b in BodyList) { b.Flags &= ~BodyFlags.Island; } for (int i = 0; i < ContactManager.ContactList.Count; i++) { Contact c = ContactManager.ContactList[i]; c.Flags &= ~ContactFlags.Island; } foreach (PhysicsJoint j in JointList) { j.IslandFlag = false; } // Build and simulate all awake islands. int stackSize = BodyList.Count; if (stackSize > _stack.Length) _stack = new PhysicsBody[Math.Max(_stack.Length * 2, stackSize)]; for (int index = BodyList.Count - 1; index >= 0; index--) { PhysicsBody seed = BodyList[index]; if ((seed.Flags & (BodyFlags.Island)) != BodyFlags.None) { continue; } if (seed.Awake == false || seed.Enabled == false) { continue; } // The seed can be dynamic or kinematic. if (seed.BodyType == BodyType.Static) { continue; } // Reset island and stack. Island.Clear(); int stackCount = 0; _stack[stackCount++] = seed; seed.Flags |= BodyFlags.Island; // 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. PhysicsBody b = _stack[--stackCount]; Debug.Assert(b.Enabled); Island.Add(b); // Make sure the body is awake. b.Awake = true; // To keep islands as small as possible, we don't // propagate islands across static bodies. if (b.BodyType == BodyType.Static) { continue; } // Search all contacts connected to this body. for (ContactEdge ce = b.ContactList; ce != null; ce = ce.Next) { Contact contact = ce.Contact; // Has this contact already been added to an island? if ((contact.Flags & ContactFlags.Island) != ContactFlags.None) { continue; } // Is this contact solid and touching? if (!ce.Contact.Enabled || !ce.Contact.IsTouching()) { continue; } // Skip sensors. bool sensorA = contact.FixtureA.IsSensor; bool sensorB = contact.FixtureB.IsSensor; if (sensorA || sensorB) { continue; } Island.Add(contact); contact.Flags |= ContactFlags.Island; PhysicsBody other = ce.Other; // Was the other body already added to this island? if ((other.Flags & BodyFlags.Island) != BodyFlags.None) { continue; } Debug.Assert(stackCount < stackSize); _stack[stackCount++] = other; other.Flags |= BodyFlags.Island; } // Search all joints connect to this body. for (JointEdge je = b.JointList; je != null; je = je.Next) { if (je.Joint.IslandFlag) { continue; } PhysicsBody other = je.Other; // WIP David //Enter here when it's a non-fixed joint. Non-fixed joints have a other body. if (other != null) { // Don't simulate joints connected to inactive bodies. if (other.Enabled == false) { continue; } Island.Add(je.Joint); je.Joint.IslandFlag = true; if ((other.Flags & BodyFlags.Island) != BodyFlags.None) { continue; } Debug.Assert(stackCount < stackSize); _stack[stackCount++] = other; other.Flags |= BodyFlags.Island; } else { Island.Add(je.Joint); je.Joint.IslandFlag = true; } } } Island.Solve(ref step, ref Gravity); // Post solve cleanup. for (int i = 0; i < Island.BodyCount; ++i) { // Allow static bodies to participate in other islands. PhysicsBody b = Island.Bodies[i]; if (b.BodyType == BodyType.Static) { b.Flags &= ~BodyFlags.Island; } } } // Synchronize fixtures, check for out of range bodies. foreach (PhysicsBody b in BodyList) { // If a body was not in an island then it did not move. if ((b.Flags & BodyFlags.Island) != BodyFlags.Island) { continue; } if (b.BodyType == BodyType.Static) { continue; } // Update fixtures (for broad-phase). b.SynchronizeFixtures(); } // Look for new contacts. ContactManager.FindNewContacts(); }