/// <summary> /// Initializes a new instance of the <see cref="World"/> class. /// </summary> /// <param name="gravity">The gravity.</param> public World(Vector2 gravity) { ContactManager = new ContactManager(); Gravity = gravity; Flags = WorldFlags.ClearForces; _queryAABBCallbackWrapper = QueryAABBCallbackWrapper; _rayCastCallbackWrapper = RayCastCallbackWrapper; Controllers = new List<Controller>(); BreakableBodyList = new List<BreakableBody>(); BodyList = new List<Body>(32); JointList = new List<Joint>(32); }
public void Reset(int bodyCapacity, int contactCapacity, int jointCapacity, ContactManager contactManager) { _bodyCapacity = bodyCapacity; _contactCapacity = contactCapacity; _jointCapacity = jointCapacity; BodyCount = 0; ContactCount = 0; JointCount = 0; _contactManager = contactManager; if (Bodies == null || Bodies.Length < bodyCapacity) { Bodies = new Body[bodyCapacity]; } if (_contacts == null || _contacts.Length < contactCapacity) { _contacts = new Contact[contactCapacity*2]; } if (_joints == null || _joints.Length < jointCapacity) { _joints = new Joint[jointCapacity*2]; } }
/// <summary> /// Take a time step. This performs collision detection, integration, /// and consraint solution. /// </summary> /// <param name="dt">The amount of time to simulate, this should not vary.</param> public void Step(float dt) { #if (!SILVERLIGHT) if (Settings.EnableDiagnostics) { _watch.Start(); } #endif ProcessChanges(); #if (!SILVERLIGHT) if (Settings.EnableDiagnostics) { AddRemoveTime = _watch.ElapsedTicks; } #endif //If there is no change in time, no need to calculate anything. if (dt == 0 || !Enabled) { #if (!SILVERLIGHT) if (Settings.EnableDiagnostics) { _watch.Stop(); _watch.Reset(); } #endif return; } // If new fixtures were added, we need to find the new contacts. if ((Flags & WorldFlags.NewFixture) == WorldFlags.NewFixture) { ContactManager.FindNewContacts(); Flags &= ~WorldFlags.NewFixture; } TimeStep step; step.inv_dt = 1.0f / dt; step.dt = dt; step.dtRatio = _invDt0 * dt; //Update controllers for (int i = 0; i < Controllers.Count; i++) { Controllers[i].Update(dt); } #if (!SILVERLIGHT) if (Settings.EnableDiagnostics) { ControllersUpdateTime = _watch.ElapsedTicks - AddRemoveTime; } #endif // Update contacts. This is where some contacts are destroyed. ContactManager.Collide(); #if (!SILVERLIGHT) if (Settings.EnableDiagnostics) { ContactsUpdateTime = _watch.ElapsedTicks - (AddRemoveTime + ControllersUpdateTime); } #endif // Integrate velocities, solve velocity raints, and integrate positions. Solve(ref step); #if (!SILVERLIGHT) if (Settings.EnableDiagnostics) { SolveUpdateTime = _watch.ElapsedTicks - (AddRemoveTime + ControllersUpdateTime + ContactsUpdateTime); } #endif // Handle TOI events. if (Settings.ContinuousPhysics) { SolveTOI(ref step); } #if (!SILVERLIGHT) if (Settings.EnableDiagnostics) { ContinuousPhysicsTime = _watch.ElapsedTicks - (AddRemoveTime + ControllersUpdateTime + ContactsUpdateTime + SolveUpdateTime); } #endif _invDt0 = step.inv_dt; if ((Flags & WorldFlags.ClearForces) != 0) { ClearForces(); } for (int i = 0; i < BreakableBodyList.Count; i++) { BreakableBodyList[i].Update(); } #if (!SILVERLIGHT) if (Settings.EnableDiagnostics) { _watch.Stop(); //AddRemoveTime = 1000 * AddRemoveTime / Stopwatch.Frequency; UpdateTime = _watch.ElapsedTicks; _watch.Reset(); } #endif }
private void Solve(ref TimeStep step) { // Size the island for the worst case. Island.Reset(BodyList.Count, ContactManager.ContactCount, JointList.Count, ContactManager); // Clear all the island flags. foreach (Body b in BodyList) { b.Flags &= ~BodyFlags.Island; } for (Contact c = ContactManager.ContactList; c != null; c = c.Next) { c.Flags &= ~ContactFlags.Island; } foreach (Joint j in JointList) { j.IslandFlag = false; } // Build and simulate all awake islands. int stackSize = BodyList.Count; if (stackSize > _stack.Length) { _stack = new Body[Math.Max(_stack.Length * 2, stackSize)]; } for (int index = BodyList.Count - 1; index >= 0; index--) { Body 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. Body 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; Body 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; } Body 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. Body b = Island.Bodies[i]; if (b.BodyType == BodyType.Static) { b.Flags &= ~BodyFlags.Island; } } } // Synchronize fixtures, check for out of range bodies. foreach (Body 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(); }
/// <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 (Contact c = ContactManager.ContactList; c != null; c = c.Next) { // 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 (Contact c = ContactManager.ContactList; c != null; c = c.Next) { // 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; } Body bA = fA.Body; Body bB = fB.Body; BodyType typeA = bA.BodyType; BodyType typeB = bB.BodyType; Debug.Assert(typeA == BodyType.Dynamic || typeB == BodyType.Dynamic); 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; bool collideB = bB.IsBullet || typeB != BodyType.Dynamic; // 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] TOIInput input = new TOIInput(); 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, ref 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; Body bA1 = fA1.Body; Body 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. Body[] bodies = { bA1, bB1 }; for (int i = 0; i < 2; ++i) { Body 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. Body 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) { Body 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 (_subStepping) { _stepComplete = false; break; } } }