public void Solve(ref TimeStep step, Vector2 gravity, bool allowSleep) { // Integrate velocities and apply damping. for (int i = 0; i < _bodyCount; ++i) { Body b = _bodies[i]; if (b.GetType() != BodyType.Dynamic) { continue; } // Integrate velocities. b._linearVelocity += step.dt * (gravity + b._invMass * b._force); b._angularVelocity += 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._linearVelocity *= MathUtils.Clamp(1.0f - step.dt * b._linearDamping, 0.0f, 1.0f); b._angularVelocity *= 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].GetFixtureA(); Fixture fixtureB = _contacts[i2].GetFixtureB(); Body bodyA = fixtureA.GetBody(); Body bodyB = fixtureB.GetBody(); bool nonStatic = bodyA.GetType() != BodyType.Static && bodyB.GetType() != BodyType.Static; if (nonStatic) { ++i1; //b2Swap(_contacts[i1], _contacts[i2]); Contact temp = _contacts[i1]; _contacts[i1] = _contacts[i2]; _contacts[i2] = temp; } } // Initialize velocity constraints. _contactSolver.Reset(_contacts, _contactCount, step.dtRatio); _contactSolver.WarmStart(); for (int i = 0; i < _jointCount; ++i) { _joints[i].InitVelocityConstraints(ref step); } // Solve velocity constraints. for (int i = 0; i < step.velocityIterations; ++i) { for (int j = 0; j < _jointCount; ++j) { _joints[j].SolveVelocityConstraints(ref step); } _contactSolver.SolveVelocityConstraints(); } // Post-solve (store impulses for warm starting). _contactSolver.StoreImpulses(); // Integrate positions. for (int i = 0; i < _bodyCount; ++i) { Body b = _bodies[i]; if (b.GetType() == BodyType.Static) { continue; } // Check for large velocities. Vector2 translation = step.dt * b._linearVelocity; if (Vector2.Dot(translation, translation) > Settings.b2_maxTranslationSquared) { float ratio = Settings.b2_maxTranslation / translation.magnitude; b._linearVelocity *= ratio; } float rotation = step.dt * b._angularVelocity; if (rotation * rotation > Settings.b2_maxRotationSquared) { float ratio = Settings.b2_maxRotation / Math.Abs(rotation); b._angularVelocity *= ratio; } // Store positions for continuous collision. b._sweep.c0 = b._sweep.c; b._sweep.a0 = b._sweep.a; // Integrate b._sweep.c += step.dt * b._linearVelocity; b._sweep.a += step.dt * b._angularVelocity; // Compute new transform b.SynchronizeTransform(); // Note: shapes are synchronized later. } // Iterate over constraints. for (int i = 0; i < step.positionIterations; ++i) { bool contactsOkay = _contactSolver.SolvePositionConstraints(Settings.b2_contactBaumgarte); bool jointsOkay = true; for (int j = 0; j < _jointCount; ++j) { bool jointOkay = _joints[j].SolvePositionConstraints(Settings.b2_contactBaumgarte); jointsOkay = jointsOkay && jointOkay; } if (contactsOkay && jointsOkay) { // Exit early if the position errors are small. break; } } Report(_contactSolver._constraints); if (allowSleep) { float minSleepTime = Settings.b2_maxFloat; const float linTolSqr = Settings.b2_linearSleepTolerance * Settings.b2_linearSleepTolerance; const float angTolSqr = Settings.b2_angularSleepTolerance * Settings.b2_angularSleepTolerance; for (int i = 0; i < _bodyCount; ++i) { Body b = _bodies[i]; if (b.GetType() == BodyType.Static) { continue; } if ((b._flags & BodyFlags.AutoSleep) == 0) { b._sleepTime = 0.0f; minSleepTime = 0.0f; } if ((b._flags & BodyFlags.AutoSleep) == 0 || b._angularVelocity * b._angularVelocity > angTolSqr || Vector2.Dot(b._linearVelocity, b._linearVelocity) > linTolSqr) { b._sleepTime = 0.0f; minSleepTime = 0.0f; } else { b._sleepTime += step.dt; minSleepTime = Math.Min(minSleepTime, b._sleepTime); } } if (minSleepTime >= Settings.b2_timeToSleep) { for (int i = 0; i < _bodyCount; ++i) { Body b = _bodies[i]; b.SetAwake(false); } } } }
// 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; } }
void Solve(ref TimeStep step) { // Size the island for the worst case. _island.Reset(_bodyCount, _contactManager._contactCount, _jointCount, _contactManager.ContactListener); // Clear all the island flags. for (Body b = _bodyList; b != null; b = b._next) { b._flags &= ~BodyFlags.Island; } for (Contact c = _contactManager._contactList; c != null; c = c._next) { c._flags &= ~ContactFlags.Island; } for (Joint j = _jointList; j != null; j = j._next) { j._islandFlag = false; } // Build and simulate all awake islands. int stackSize = _bodyCount; if (stackSize > stack.Length) { stack = new Body[Math.Max(stack.Length * 2, stackSize)]; } for (Body seed = _bodyList; seed != null; seed = seed._next) { if ((seed._flags & (BodyFlags.Island)) != BodyFlags.None) { continue; } if (seed.IsAwake() == false || seed.IsActive() == false) { continue; } // The seed can be dynamic or kinematic. if (seed.GetType() == 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 raint graph. while (stackCount > 0) { // Grab the next body off the stack and add it to the island. Body b = stack[--stackCount]; //Debug.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.GetType() == 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.IsEnabled() || !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 == 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._islandFlag = true; if ((other._flags & BodyFlags.Island) != BodyFlags.None) { continue; } //Debug.Assert(stackCount < stackSize); stack[stackCount++] = other; other._flags |= BodyFlags.Island; } } _island.Solve(ref step, Gravity, _allowSleep); // 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.GetType() == BodyType.Static) { b._flags &= ~BodyFlags.Island; } } } // Synchronize fixtures, check for out of range bodies. for (Body b = _bodyList; b != null; b = b.GetNext()) { // If a body was not in an island then it did not move. if ((b._flags & BodyFlags.Island) != BodyFlags.Island) { continue; } if (b.GetType() == BodyType.Static) { continue; } // Update fixtures (for broad-phase). b.SynchronizeFixtures(); } // Look for new contacts. _contactManager.FindNewContacts(); }
/// Call this to draw shapes and other debug draw data. public void DrawDebugData() { if (DebugDraw == null) { return; } DebugDrawFlags flags = DebugDraw.Flags; if ((flags & DebugDrawFlags.Shape) == DebugDrawFlags.Shape) { for (Body b = _bodyList; b != null; b = b.GetNext()) { Transform xf; b.GetTransform(out xf); for (Fixture f = b.GetFixtureList(); f != null; f = f.GetNext()) { if (b.IsActive() == false) { DrawShape(f, xf, new Color(0.5f, 0.5f, 0.3f)); } else if (b.GetType() == BodyType.Static) { DrawShape(f, xf, new Color(0.5f, 0.9f, 0.5f)); } else if (b.GetType() == BodyType.Kinematic) { DrawShape(f, xf, new Color(0.5f, 0.5f, 0.9f)); } else if (b.IsAwake() == false) { DrawShape(f, xf, new Color(0.6f, 0.6f, 0.6f)); } else { DrawShape(f, xf, new Color(0.9f, 0.7f, 0.7f)); } } } } if ((flags & DebugDrawFlags.Joint) == DebugDrawFlags.Joint) { for (Joint j = _jointList; j != null; j = j.GetNext()) { DrawJoint(j); } } if ((flags & DebugDrawFlags.Pair) == DebugDrawFlags.Pair) { Color color = new Color(0.3f, 0.9f, 0.9f); for (Contact c = _contactManager._contactList; c != null; c = c.GetNext()) { /* * Fixture fixtureA = c.GetFixtureA(); * Fixture fixtureB = c.GetFixtureB(); * * AABB aabbA; * AABB aabbB; * fixtureA.GetAABB(out aabbA); * fixtureB.GetAABB(out aabbB); * * Vector2 cA = aabbA.GetCenter(); * Vector2 cB = aabbB.GetCenter(); * * DebugDraw.DrawSegment(cA, cB, color); */ } } if ((flags & DebugDrawFlags.AABB) == DebugDrawFlags.AABB) { Color color = new Color(0.9f, 0.3f, 0.9f); BroadPhase bp = _contactManager._broadPhase; for (Body b = _bodyList; b != null; b = b.GetNext()) { if (b.IsActive() == false) { continue; } for (Fixture f = b.GetFixtureList(); f != null; f = f.GetNext()) { for (int i = 0; i < f._proxyCount; ++i) { FixtureProxy proxy = f._proxies[i]; AABB aabb; bp.GetFatAABB(proxy.proxyId, out aabb); FixedArray8 <Vector2> vs = new FixedArray8 <Vector2>(); vs[0] = new Vector2(aabb.lowerBound.x, aabb.lowerBound.y); vs[1] = new Vector2(aabb.upperBound.x, aabb.lowerBound.y); vs[2] = new Vector2(aabb.upperBound.x, aabb.upperBound.y); vs[3] = new Vector2(aabb.lowerBound.x, aabb.upperBound.y); DebugDraw.DrawPolygon(ref vs, 4, color); } } } } if ((flags & DebugDrawFlags.CenterOfMass) == DebugDrawFlags.CenterOfMass) { for (Body b = _bodyList; b != null; b = b.GetNext()) { Transform xf; b.GetTransform(out xf); xf.Position = b.GetWorldCenter(); DebugDraw.DrawTransform(ref xf); } } }
/// Create a joint to rain bodies together. No reference to the definition /// is retained. This may cause the connected bodies to cease colliding. /// @warning This function is locked during callbacks. public Joint CreateJoint(JointDef def) { //Debug.Assert(!IsLocked); if (IsLocked) { return(null); } Joint j = Joint.Create(def); // Connect to the world list. j._prev = null; j._next = _jointList; if (_jointList != null) { _jointList._prev = j; } _jointList = j; ++_jointCount; // Connect to the bodies' doubly linked lists. j._edgeA.Joint = j; j._edgeA.Other = j._bodyB; j._edgeA.Prev = null; j._edgeA.Next = j._bodyA._jointList; if (j._bodyA._jointList != null) { j._bodyA._jointList.Prev = j._edgeA; } j._bodyA._jointList = j._edgeA; j._edgeB.Joint = j; j._edgeB.Other = j._bodyA; j._edgeB.Prev = null; j._edgeB.Next = j._bodyB._jointList; if (j._bodyB._jointList != null) { j._bodyB._jointList.Prev = j._edgeB; } j._bodyB._jointList = j._edgeB; Body bodyA = def.bodyA; Body bodyB = def.bodyB; bool staticA = bodyA.GetType() == BodyType.Static; bool staticB = bodyB.GetType() == BodyType.Static; // If the joint prevents collisions, then flag any contacts for filtering. if (def.collideConnected == false) { ContactEdge edge = bodyB.GetContactList(); 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; } } // Note: creating a joint doesn't wake the bodies. return(j); }
// Sequentially solve TOIs for each body. We bring each // body to the time of contact and perform some position correction. // Time is not conserved. void SolveTOI() { // Prepare all contacts. for (Contact c = _contactManager._contactList; c != null; c = c._next) { // Enable the contact c._flags |= ContactFlags.Enabled; // Set the number of TOI events for this contact to zero. c._toiCount = 0; } // Initialize the TOI flag. for (Body body = _bodyList; body != null; body = body._next) { // Kinematic, and static bodies will not be affected by the TOI event. // If a body was not in an island then it did not move. if ((body._flags & BodyFlags.Island) == 0 || body.GetType() == BodyType.Kinematic || body.GetType() == BodyType.Static) { body._flags |= BodyFlags.Toi; } else { body._flags &= ~BodyFlags.Toi; } } // Collide non-bullets. for (Body body = _bodyList; body != null; body = body._next) { if ((body._flags & BodyFlags.Toi) != BodyFlags.None) { continue; } if (body.IsBullet == true) { continue; } SolveTOI(body); body._flags |= BodyFlags.Toi; } // Collide bullets. for (Body body = _bodyList; body != null; body = body._next) { if ((body._flags & BodyFlags.Toi) != BodyFlags.None) { continue; } if (body.IsBullet == false) { continue; } SolveTOI(body); body._flags |= BodyFlags.Toi; } }