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. #if USE_ISLAND_SET Debug.Assert(IslandSet.Count == 0); #else foreach (Body b in BodyList) { b._island = false; } #endif #if USE_ACTIVE_CONTACT_SET foreach (var c in ContactManager.ActiveContacts) { c.Flags &= ~ContactFlags.Island; } #else foreach (Contact c in ContactManager.ContactList) { c.IslandFlag = false; } #endif 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)]; #if USE_AWAKE_BODY_SET // If AwakeBodyList is empty, the Island code will not have a chance // to update the diagnostics timer so reset the timer here. Island.JointUpdateTime = 0; Debug.Assert(AwakeBodyList.Count == 0); AwakeBodyList.AddRange(AwakeBodySet); foreach (var seed in AwakeBodyList) { #else for (int index = BodyList.Count - 1; index >= 0; index--) { Body seed = BodyList[index]; #endif if (seed._island) { 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; #if USE_ISLAND_SET if (!IslandSet.Contains(body)) IslandSet.Add(body); #endif seed._island = true; // 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.IslandFlag) { continue; } // Is this contact solid and touching? if (ce.Contact.Enabled == false || ce.Contact.IsTouching == false) { continue; } // Skip sensors. bool sensorA = contact.FixtureA.IsSensor; bool sensorB = contact.FixtureB.IsSensor; if (sensorA || sensorB) { continue; } Island.Add(contact); contact.IslandFlag = true; Body other = ce.Other; // Was the other body already added to this island? if (other._island) { continue; } Debug.Assert(stackCount < stackSize); _stack[stackCount++] = other; #if USE_ISLAND_SET if (!IslandSet.Contains(body)) IslandSet.Add(body); #endif other._island = true; } // 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._island) { continue; } Debug.Assert(stackCount < stackSize); _stack[stackCount++] = other; #if USE_ISLAND_SET if (!IslandSet.Contains(body)) IslandSet.Add(body); #endif other._island = true; } 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._island = false; } } } // Synchronize fixtures, check for out of range bodies. #if USE_ISLAND_SET foreach (var b in IslandSet) #else foreach (Body b in BodyList) #endif { // If a body was not in an island then it did not move. if (!b._island) { continue; } #if USE_ISLAND_SET Debug.Assert(b.BodyType != BodyType.Static); #else if (b.BodyType == BodyType.Static) { continue; } #endif // Update fixtures (for broad-phase). b.SynchronizeFixtures(); } #if OPTIMIZE_TOI foreach (var b in IslandSet) { if (!TOISet.Contains(b)) { TOISet.Add(b); } } #endif #if USE_ISLAND_SET IslandSet.Clear(); #endif // Look for new contacts. ContactManager.FindNewContacts(); #if USE_AWAKE_BODY_SET AwakeBodyList.Clear(); #endif }
private void SolveTOI(ref TimeStep step) { Island.Reset(2 * Settings.MaxTOIContacts, Settings.MaxTOIContacts, 0, ContactManager); #if OPTIMIZE_TOI bool wasStepComplete = _stepComplete; #endif if (_stepComplete) { #if OPTIMIZE_TOI foreach (var b in TOISet) { b.Flags &= ~BodyFlags.Island; b.Sweep.Alpha0 = 0.0f; } #else for (int i = 0; i < BodyList.Count; i++) { BodyList[i]._island = false; BodyList[i]._sweep.Alpha0 = 0.0f; } #endif #if USE_ACTIVE_CONTACT_SET foreach (var c in ContactManager.ActiveContacts) { #else for (int i = 0; i < ContactManager.ContactList.Count; i++) { Contact c = ContactManager.ContactList[i]; #endif // Invalidate TOI c.IslandFlag = false; c.TOIFlag = false; 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; #if USE_ACTIVE_CONTACT_SET foreach (var c in ContactManager.ActiveContacts) { #else for (int i = 0; i < ContactManager.ContactList.Count; i++) { Contact c = ContactManager.ContactList[i]; #endif // Is this contact disabled? if (c.Enabled == false) { continue; } // Prevent excessive sub-stepping. if (c._toiCount > Settings.MaxSubSteps) { continue; } float alpha; if (c.TOIFlag) { // 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 activeA = bA.Awake && typeA != BodyType.Static; bool activeB = bB.Awake && typeB != BodyType.Static; // Is at least one body active (awake and dynamic or kinematic)? if (activeA == false && activeB == false) { continue; } bool collideA = (bA.IsBullet || typeA != BodyType.Dynamic) && ((fA.IgnoreCCDWith & fB.CollisionCategories) == 0); bool collideB = (bB.IsBullet || typeB != BodyType.Dynamic) && ((fB.IgnoreCCDWith & fA.CollisionCategories) == 0); // Are these two non-bullet dynamic bodies? if (collideA == false && collideB == false) { continue; } #if OPTIMIZE_TOI if (_stepComplete) { if (!TOISet.Contains(bA)) { TOISet.Add(bA); bA.Flags &= ~BodyFlags.Island; bA.Sweep.Alpha0 = 0.0f; } if (!TOISet.Contains(bB)) { TOISet.Add(bB); bB.Flags &= ~BodyFlags.Island; bB.Sweep.Alpha0 = 0.0f; } } #endif // 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.TOIFlag = true; } 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 bA0 = fA1.Body; Body bB0 = fB1.Body; Sweep backup1 = bA0._sweep; Sweep backup2 = bB0._sweep; bA0.Advance(minAlpha); bB0.Advance(minAlpha); // The TOI contact likely has some new contact points. minContact.Update(ContactManager); minContact.TOIFlag = false; ++minContact._toiCount; // Is the contact solid? if (minContact.Enabled == false || minContact.IsTouching == false) { // Restore the sweeps. minContact.Enabled = false; bA0._sweep = backup1; bB0._sweep = backup2; bA0.SynchronizeTransform(); bB0.SynchronizeTransform(); continue; } bA0.Awake = true; bB0.Awake = true; // Build the island Island.Clear(); Island.Add(bA0); Island.Add(bB0); Island.Add(minContact); bA0._island = true; bB0._island = true; minContact.IslandFlag = true; // Get contacts on bodyA and bodyB. Body[] bodies = { bA0, bB0 }; for (int i = 0; i < 2; ++i) { Body body = bodies[i]; if (body.BodyType == BodyType.Dynamic) { for (ContactEdge ce = body.ContactList; ce != null; ce = ce.Next) { Contact contact = ce.Contact; if (Island.BodyCount == Island.BodyCapacity) { break; } if (Island.ContactCount == Island.ContactCapacity) { break; } // Has this contact already been added to the island? if (contact.IslandFlag) { 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._island) { 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.IslandFlag = true; Island.Add(contact); // Has the other body already been added to the island? if (other._island) { continue; } // Add the other body to the island. other._island = true; if (other.BodyType != BodyType.Static) { other.Awake = true; } #if OPTIMIZE_TOI if (_stepComplete) { if (!TOISet.Contains(other)) { TOISet.Add(other); other.Sweep.Alpha0 = 0.0f; } } #endif Island.Add(other); } } } TimeStep subStep; subStep.dt = (1.0f - minAlpha) * step.dt; subStep.inv_dt = 1.0f / subStep.dt; subStep.dtRatio = 1.0f; Island.SolveTOI(ref subStep, bA0.IslandIndex, bB0.IslandIndex, false); // Reset island flags and synchronize broad-phase proxies. for (int i = 0; i < Island.BodyCount; ++i) { Body body = Island.Bodies[i]; body._island = false; 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.TOIFlag = false; ce.Contact.IslandFlag = false; } } // Commit fixture proxy movements to the broad-phase so that new contacts are created. // Also, some contacts can be destroyed. ContactManager.FindNewContacts(); if (Settings.EnableSubStepping) { _stepComplete = false; break; } } #if OPTIMIZE_TOI if (wasStepComplete) { TOISet.Clear(); } #endif }
void solve( ref TimeStep timeStep ) { // Size the island for the worst case. island.reset( bodyList.Count, contactManager.contactList.Count, jointList.Count, contactManager ); // Clear all the island flags. #if USE_ISLAND_SET Debug.Assert(IslandSet.Count == 0); #else foreach( Body b in bodyList ) { b._island = false; } #endif #if USE_ACTIVE_CONTACT_SET foreach (var c in ContactManager.ActiveContacts) { c.Flags &= ~ContactFlags.Island; } #else foreach( Contact c in contactManager.contactList ) { c.islandFlag = false; } #endif foreach( Joint j in jointList ) { j.islandFlag = false; } // Build and simulate all awake islands. var stackSize = bodyList.Count; if( stackSize > _stack.Length ) _stack = new Body[Math.Max( _stack.Length * 2, stackSize )]; #if USE_AWAKE_BODY_SET // If AwakeBodyList is empty, the Island code will not have a chance // to update the diagnostics timer so reset the timer here. Island.JointUpdateTime = 0; Debug.Assert(AwakeBodyList.Count == 0); AwakeBodyList.AddRange(AwakeBodySet); foreach (var seed in AwakeBodyList) { #else for( int index = bodyList.Count - 1; index >= 0; index-- ) { Body seed = bodyList[index]; #endif if( seed._island ) continue; if( seed.isAwake == 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; #if USE_ISLAND_SET if (!IslandSet.Contains(body)) IslandSet.Add(body); #endif seed._island = true; // 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. var b = _stack[--stackCount]; Debug.Assert( b.enabled ); island.add( b ); // Make sure the body is awake. b.isAwake = 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.islandFlag ) continue; // Is this contact solid and touching? if( ce.contact.enabled == false || ce.contact.isTouching == false ) continue; // Skip sensors. var sensorA = contact.fixtureA.isSensor; var sensorB = contact.fixtureB.isSensor; if( sensorA || sensorB ) continue; island.add( contact ); contact.islandFlag = true; Body other = ce.other; // Was the other body already added to this island? if( other._island ) continue; Debug.Assert( stackCount < stackSize ); _stack[stackCount++] = other; #if USE_ISLAND_SET if (!IslandSet.Contains(body)) IslandSet.Add(body); #endif other._island = true; } // Search all joints connect to this body. for( JointEdge je = b.jointList; je != null; je = je.next ) { if( je.joint.islandFlag ) continue; var 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._island ) continue; Debug.Assert( stackCount < stackSize ); _stack[stackCount++] = other; #if USE_ISLAND_SET if (!IslandSet.Contains(body)) IslandSet.Add(body); #endif other._island = true; } else { island.add( je.joint ); je.joint.islandFlag = true; } } } island.solve( ref timeStep, ref gravity ); // Post solve cleanup. for( int i = 0; i < island.BodyCount; ++i ) { // Allow static bodies to participate in other islands. var b = island.Bodies[i]; if( b.bodyType == BodyType.Static ) b._island = false; } } // Synchronize fixtures, check for out of range bodies. #if USE_ISLAND_SET foreach (var b in IslandSet) #else foreach( Body b in bodyList ) #endif { // If a body was not in an island then it did not move. if( !b._island ) continue; #if USE_ISLAND_SET Debug.Assert(b.BodyType != BodyType.Static); #else if( b.bodyType == BodyType.Static ) continue; #endif // Update fixtures (for broad-phase). b.synchronizeFixtures(); } #if OPTIMIZE_TOI foreach (var b in IslandSet) { if (!TOISet.Contains(b)) { TOISet.Add(b); } } #endif #if USE_ISLAND_SET IslandSet.Clear(); #endif // Look for new contacts. contactManager.findNewContacts(); #if USE_AWAKE_BODY_SET AwakeBodyList.Clear(); #endif } void solveTOI( ref TimeStep timeStep ) { island.reset( 2 * Settings.maxTOIContacts, Settings.maxTOIContacts, 0, contactManager ); #if OPTIMIZE_TOI bool wasStepComplete = _stepComplete; #endif if( _stepComplete ) { #if OPTIMIZE_TOI foreach (var b in TOISet) { b.Flags &= ~BodyFlags.Island; b.Sweep.Alpha0 = 0.0f; } #else for( int i = 0; i < bodyList.Count; i++ ) { bodyList[i]._island = false; bodyList[i]._sweep.Alpha0 = 0.0f; } #endif #if USE_ACTIVE_CONTACT_SET foreach (var c in ContactManager.ActiveContacts) { #else for( int i = 0; i < contactManager.contactList.Count; i++ ) { Contact c = contactManager.contactList[i]; #endif // Invalidate TOI c.islandFlag = false; c.toiFlag = false; 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; #if USE_ACTIVE_CONTACT_SET foreach (var c in ContactManager.ActiveContacts) { #else for( int i = 0; i < contactManager.contactList.Count; i++ ) { Contact c = contactManager.contactList[i]; #endif // Is this contact disabled? if( c.enabled == false ) continue; // Prevent excessive sub-stepping. if( c._toiCount > Settings.maxSubSteps ) continue; float alpha; if( c.toiFlag ) { // 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 activeA = bA.isAwake && typeA != BodyType.Static; bool activeB = bB.isAwake && typeB != BodyType.Static; // Is at least one body active (awake and dynamic or kinematic)? if( activeA == false && activeB == false ) continue; bool collideA = ( bA.isBullet || typeA != BodyType.Dynamic ) && ( ( fA.ignoreCCDWith & fB.collisionCategories ) == 0 ) && !bA.ignoreCCD; bool collideB = ( bB.isBullet || typeB != BodyType.Dynamic ) && ( ( fB.ignoreCCDWith & fA.collisionCategories ) == 0 ) && !bB.ignoreCCD; // Are these two non-bullet dynamic bodies? if( collideA == false && collideB == false ) continue; #if OPTIMIZE_TOI if (_stepComplete) { if (!TOISet.Contains(bA)) { TOISet.Add(bA); bA.Flags &= ~BodyFlags.Island; bA.Sweep.Alpha0 = 0.0f; } if (!TOISet.Contains(bB)) { TOISet.Add(bB); bB.Flags &= ~BodyFlags.Island; bB.Sweep.Alpha0 = 0.0f; } } #endif // 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.toiFlag = true; } 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 bA0 = fA1.body; Body bB0 = fB1.body; Sweep backup1 = bA0._sweep; Sweep backup2 = bB0._sweep; bA0.advance( minAlpha ); bB0.advance( minAlpha ); // The TOI contact likely has some new contact points. minContact.update( contactManager ); minContact.toiFlag = false; ++minContact._toiCount; // Is the contact solid? if( minContact.enabled == false || minContact.isTouching == false ) { // Restore the sweeps. minContact.enabled = false; bA0._sweep = backup1; bB0._sweep = backup2; bA0.synchronizeTransform(); bB0.synchronizeTransform(); continue; } bA0.isAwake = true; bB0.isAwake = true; // Build the island island.clear(); island.add( bA0 ); island.add( bB0 ); island.add( minContact ); bA0._island = true; bB0._island = true; minContact.islandFlag = true; // Get contacts on bodyA and bodyB. Body[] bodies = { bA0, bB0 }; for( int i = 0; i < 2; ++i ) { var body = bodies[i]; if( body.bodyType == BodyType.Dynamic ) { for( ContactEdge ce = body.contactList; ce != null; ce = ce.next ) { Contact contact = ce.contact; if( island.BodyCount == island.BodyCapacity ) break; if( island.ContactCount == island.ContactCapacity ) break; // Has this contact already been added to the island? if( contact.islandFlag ) 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._island ) 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.islandFlag = true; island.add( contact ); // Has the other body already been added to the island? if( other._island ) continue; // Add the other body to the island. other._island = true; if( other.bodyType != BodyType.Static ) other.isAwake = true; #if OPTIMIZE_TOI if (_stepComplete) { if (!TOISet.Contains(other)) { TOISet.Add(other); other.Sweep.Alpha0 = 0.0f; } } #endif island.add( other ); } } } TimeStep subStep; subStep.dt = ( 1.0f - minAlpha ) * timeStep.dt; subStep.inv_dt = 1.0f / subStep.dt; subStep.dtRatio = 1.0f; island.solveTOI( ref subStep, bA0.islandIndex, bB0.islandIndex ); // Reset island flags and synchronize broad-phase proxies. for( int i = 0; i < island.BodyCount; ++i ) { Body body = island.Bodies[i]; body._island = false; 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.toiFlag = false; ce.contact.islandFlag = false; } } // Commit fixture proxy movements to the broad-phase so that new contacts are created. // Also, some contacts can be destroyed. contactManager.findNewContacts(); #pragma warning disable CS0162 if( Settings.enableSubStepping ) { _stepComplete = false; break; } } #if OPTIMIZE_TOI if (wasStepComplete) { TOISet.Clear(); } #endif }
public void Solve(ref TimeStep step, Vector2 gravity) { // Integrate velocities and apply damping. for (int i = 0; i < BodyCount; ++i) { Body b = Bodies[i]; if (b.BodyType != BodyType.Dynamic) { continue; } // Integrate velocities. Only apply gravity if the body wants it. if (b.IgnoreGravity) { b.LinearVelocityInternal += step.dt * (b.InvMass * b.Force); b.AngularVelocityInternal += step.dt * b.InvI * b.Torque; } else { b.LinearVelocityInternal += step.dt * (gravity + b.InvMass * b.Force); 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; Body bodyA = fixtureA.Body; Body bodyB = fixtureB.Body; bool nonStatic = bodyA.BodyType != BodyType.Static && bodyB.BodyType != 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 < Settings.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.BodyType == BodyType.Static) { continue; } // Check for large velocities. Vector2 translation = step.dt * b.LinearVelocityInternal; if (Vector2.Dot(translation, translation) > Settings.MaxTranslationSquared) { float ratio = Settings.MaxTranslation / translation.Length(); b.LinearVelocityInternal *= 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 = b.Sweep.c; b.Sweep.a0 = b.Sweep.a; // Integrate b.Sweep.c += step.dt * b.LinearVelocityInternal; 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; for (int j = 0; j < JointCount; ++j) { bool jointOkay = _joints[j].SolvePositionConstraints(); jointsOkay = jointsOkay && jointOkay; } if (contactsOkay && jointsOkay) { // Exit early if the position errors are small. break; } } if (_contactManager.PostSolve != null) Report(_contactSolver.Constraints); if (Settings.AllowSleep) { float minSleepTime = Settings.MaxFloat; const float linTolSqr = Settings.LinearSleepTolerance * Settings.LinearSleepTolerance; const float angTolSqr = Settings.AngularSleepTolerance * Settings.AngularSleepTolerance; for (int i = 0; i < BodyCount; ++i) { Body 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) { Body b = Bodies[i]; b.Awake = false; } } } }
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(); }
public void solve( ref TimeStep step, ref Vector2 gravity ) { float h = step.dt; // Integrate velocities and apply damping. Initialize the body state. for( int i = 0; i < BodyCount; ++i ) { var b = Bodies[i]; var c = b._sweep.C; float a = b._sweep.A; var v = b._linearVelocity; float w = b._angularVelocity; // Store positions for continuous collision. b._sweep.C0 = b._sweep.C; b._sweep.A0 = b._sweep.A; if( b.bodyType == BodyType.Dynamic ) { // Integrate velocities. // FPE: Only apply gravity if the body wants it. if( b.ignoreGravity ) v += h * ( b._invMass * b._force ); else v += h * ( b.gravityScale * gravity + b._invMass * b._force ); w += h * 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 v *= MathUtils.clamp( 1.0f - h * b.linearDamping, 0.0f, 1.0f ); w *= MathUtils.clamp( 1.0f - h * b.angularDamping, 0.0f, 1.0f ); } _positions[i].c = c; _positions[i].a = a; _velocities[i].v = v; _velocities[i].w = w; } // Solver data SolverData solverData = new SolverData(); solverData.step = step; solverData.positions = _positions; solverData.velocities = _velocities; _contactSolver.reset( step, ContactCount, _contacts, _positions, _velocities ); _contactSolver.initializeVelocityConstraints(); if( Settings.enableWarmstarting ) { _contactSolver.warmStart(); } if( Settings.enableDiagnostics ) _watch.Start(); for( int i = 0; i < JointCount; ++i ) { if( _joints[i].enabled ) _joints[i].initVelocityConstraints( ref solverData ); } if( Settings.enableDiagnostics ) _watch.Stop(); // Solve velocity constraints. for( int i = 0; i < Settings.velocityIterations; ++i ) { for( int j = 0; j < JointCount; ++j ) { Joint joint = _joints[j]; if( !joint.enabled ) continue; if( Settings.enableDiagnostics ) _watch.Start(); joint.solveVelocityConstraints( ref solverData ); joint.validate( step.inv_dt ); if( Settings.enableDiagnostics ) _watch.Stop(); } _contactSolver.solveVelocityConstraints(); } // Store impulses for warm starting. _contactSolver.storeImpulses(); // Integrate positions for( int i = 0; i < BodyCount; ++i ) { Vector2 c = _positions[i].c; float a = _positions[i].a; Vector2 v = _velocities[i].v; float w = _velocities[i].w; // Check for large velocities Vector2 translation = h * v; if( Vector2.Dot( translation, translation ) > Settings.maxTranslationSquared ) { float ratio = Settings.maxTranslation / translation.Length(); v *= ratio; } float rotation = h * w; if( rotation * rotation > Settings.maxRotationSquared ) { float ratio = Settings.maxRotation / Math.Abs( rotation ); w *= ratio; } // Integrate c += h * v; a += h * w; _positions[i].c = c; _positions[i].a = a; _velocities[i].v = v; _velocities[i].w = w; } // Solve position constraints bool positionSolved = false; for( int i = 0; i < Settings.positionIterations; ++i ) { bool contactsOkay = _contactSolver.solvePositionConstraints(); bool jointsOkay = true; for( int j = 0; j < JointCount; ++j ) { Joint joint = _joints[j]; if( !joint.enabled ) continue; if( Settings.enableDiagnostics ) _watch.Start(); bool jointOkay = joint.solvePositionConstraints( ref solverData ); if( Settings.enableDiagnostics ) _watch.Stop(); jointsOkay = jointsOkay && jointOkay; } if( contactsOkay && jointsOkay ) { // Exit early if the position errors are small. positionSolved = true; break; } } if( Settings.enableDiagnostics ) { JointUpdateTime = _watch.ElapsedTicks; _watch.Reset(); } // Copy state buffers back to the bodies for( int i = 0; i < BodyCount; ++i ) { Body body = Bodies[i]; body._sweep.C = _positions[i].c; body._sweep.A = _positions[i].a; body._linearVelocity = _velocities[i].v; body._angularVelocity = _velocities[i].w; body.synchronizeTransform(); } report( _contactSolver._velocityConstraints ); if( Settings.allowSleep ) { float minSleepTime = Settings.maxFloat; for( int i = 0; i < BodyCount; ++i ) { Body b = Bodies[i]; if( b.bodyType == BodyType.Static ) continue; if( !b.isSleepingAllowed || b._angularVelocity * b._angularVelocity > AngTolSqr || Vector2.Dot( b._linearVelocity, b._linearVelocity ) > LinTolSqr ) { b._sleepTime = 0.0f; minSleepTime = 0.0f; } else { b._sleepTime += h; minSleepTime = Math.Min( minSleepTime, b._sleepTime ); } } if( minSleepTime >= Settings.timeToSleep && positionSolved ) { for( int i = 0; i < BodyCount; ++i ) { Body b = Bodies[i]; b.isAwake = false; } } } }
internal void SolveTOI(ref TimeStep subStep, Body bodyA, Body bodyB) { _contactSolver.Reset(_contacts, ContactCount, subStep.dtRatio); // Solve position constraints. const float kTOIBaumgarte = 0.75f; for (int i = 0; i < Settings.TOIPositionIterations; ++i) { bool contactsOkay = _contactSolver.SolvePositionConstraintsTOI(kTOIBaumgarte, bodyA, bodyB); if (contactsOkay) { break; } if (i == Settings.TOIPositionIterations - 1) { i += 0; } } /* #if 0 * // Is the new position really safe? * for (int32 i = 0; i < m_contactCount; ++i) * { * b2Contact* c = m_contacts[i]; * b2Fixture* fA = c->GetFixtureA(); * b2Fixture* fB = c->GetFixtureB(); * * b2Body* bA = fA->GetBody(); * b2Body* bB = fB->GetBody(); * * int32 indexA = c->GetChildIndexA(); * int32 indexB = c->GetChildIndexB(); * * b2DistanceInput input; * input.proxyA.Set(fA->GetShape(), indexA); * input.proxyB.Set(fB->GetShape(), indexB); * input.transformA = bA->GetTransform(); * input.transformB = bB->GetTransform(); * input.useRadii = false; * * b2DistanceOutput output; * b2SimplexCache cache; * cache.count = 0; * b2Distance(&output, &cache, &input); * * if (output.distance == 0 || cache.count == 3) * { * cache.count += 0; * } * } #endif */ // Leap of faith to new safe state. for (int i = 0; i < BodyCount; ++i) { Bodies[i].Sweep.A0 = Bodies[i].Sweep.A; Bodies[i].Sweep.C0 = Bodies[i].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) { Body b = Bodies[i]; if (b.BodyType == BodyType.Static) { continue; } // Check for large velocities. Vector2 translation = subStep.dt * b.LinearVelocity; if (Vector2.Dot(translation, translation) > Settings.MaxTranslationSquared) { translation.Normalize(); b.LinearVelocity = (Settings.MaxTranslation * subStep.inv_dt) * translation; } float rotation = subStep.dt * b.AngularVelocity; if (rotation * rotation > Settings.MaxRotationSquared) { if (rotation < 0.0) { b.AngularVelocity = -subStep.inv_dt * Settings.MaxRotation; } else { b.AngularVelocity = subStep.inv_dt * Settings.MaxRotation; } } // Integrate b.Sweep.C += subStep.dt * b.LinearVelocity; b.Sweep.A += subStep.dt * b.AngularVelocity; // Compute new transform b.SynchronizeTransform(); // Note: shapes are synchronized later. } if (_contactManager.PostSolve != null) { Report(_contactSolver.Constraints); } }
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) { Body 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) { Body 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); }
internal void SolveTOI(ref TimeStep subStep) { this._contactSolver.Reset(this._contacts, this.ContactCount, subStep.dtRatio, false); // Solve position constraints. const float kTOIBaumgarte = 0.75f; for (int i = 0; i < Settings.TOIPositionIterations; ++i) { bool contactsOkay = this._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 < this.BodyCount; ++i) { Body body = this.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. this._contactSolver.InitializeVelocityConstraints(); // Solve velocity constraints. for (int i = 0; i < Settings.TOIVelocityIterations; ++i) { this._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 < this.BodyCount; ++i) { Body b = this.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(this._contactSolver.Constraints); }
public void Solve(ref TimeStep step, ref Vector2 gravity) { // Integrate velocities and apply damping. for (int i = 0; i < this.BodyCount; ++i) { Body b = this.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 < this.ContactCount; ++i2) { Fixture fixtureA = this._contacts[i2].FixtureA; Fixture fixtureB = this._contacts[i2].FixtureB; Body bodyA = fixtureA.Body; Body 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 = this._contacts[i1]; this._contacts[i1] = this._contacts[i2]; this._contacts[i2] = tmp; } } // Initialize velocity constraints. this._contactSolver.Reset(this._contacts, this.ContactCount, step.dtRatio, Settings.EnableWarmstarting); this._contactSolver.InitializeVelocityConstraints(); #pragma warning disable 0162 // Unreachable code detected if (Settings.EnableWarmstarting) { this._contactSolver.WarmStart(); } #pragma warning restore 0162 // Unreachable code detected if (Settings.EnableDiagnostics) { this._watch.Start(); this._tmpTime = 0; } for (int i = 0; i < this.JointCount; ++i) { if (this._joints[i].Enabled) { this._joints[i].InitVelocityConstraints(ref step); } } if (Settings.EnableDiagnostics) { this._tmpTime += this._watch.ElapsedTicks; } // Solve velocity constraints. for (int i = 0; i < Settings.VelocityIterations; ++i) { if (Settings.EnableDiagnostics) { this._watch.Start(); } for (int j = 0; j < this.JointCount; ++j) { Joint joint = this._joints[j]; if (!joint.Enabled) { continue; } joint.SolveVelocityConstraints(ref step); joint.Validate(step.inv_dt); } if (Settings.EnableDiagnostics) { this._watch.Stop(); this._tmpTime += this._watch.ElapsedTicks; this._watch.Reset(); } this._contactSolver.SolveVelocityConstraints(); } // Post-solve (store impulses for warm starting). this._contactSolver.StoreImpulses(); // Integrate positions. for (int i = 0; i < this.BodyCount; ++i) { Body b = this.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 = this._contactSolver.SolvePositionConstraints(Settings.ContactBaumgarte); bool jointsOkay = true; if (Settings.EnableDiagnostics) { this._watch.Start(); } for (int j = 0; j < this.JointCount; ++j) { Joint joint = this._joints[j]; if (!joint.Enabled) { continue; } bool jointOkay = joint.SolvePositionConstraints(); jointsOkay = jointsOkay && jointOkay; } if (Settings.EnableDiagnostics) { this._watch.Stop(); this._tmpTime += this._watch.ElapsedTicks; this._watch.Reset(); } if (contactsOkay && jointsOkay) { // Exit early if the position errors are small. break; } } if (Settings.EnableDiagnostics) { this.JointUpdateTime = this._tmpTime; } Report(this._contactSolver.Constraints); if (Settings.AllowSleep) { float minSleepTime = Settings.MaxFloat; for (int i = 0; i < this.BodyCount; ++i) { Body b = this.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 < this.BodyCount; ++i) { Body b = this.Bodies[i]; b.Awake = false; } } } }
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; } } }
internal override void InitVelocityConstraints(TimeStep step) { Body b1 = _body1; Body b2 = _body2; Vector2 r1 = CommonMath.Mul(b1.GetXForm().R, _localAnchor1 - b1.GetLocalCenter()); Vector2 r2 = CommonMath.Mul(b2.GetXForm().R, _localAnchor2 - b2.GetLocalCenter()); Vector2 p1 = b1._sweep.C + r1; Vector2 p2 = b2._sweep.C + r2; Vector2 s1 = _ground.GetXForm().Position + _groundAnchor1; Vector2 s2 = _ground.GetXForm().Position + _groundAnchor2; // Get the pulley axes. _u1 = p1 - s1; _u2 = p2 - s2; float length1 = _u1.Length(); float length2 = _u2.Length(); if (length1 > Settings.LinearSlop) { _u1 *= 1.0f / length1; } else { _u1 = Vector2.Zero; } if (length2 > Settings.LinearSlop) { _u2 *= 1.0f / length2; } else { _u2 = Vector2.Zero; } float C = _constant - length1 - _ratio * length2; if (C > 0.0f) { _state = LimitState.InactiveLimit; _impulse = 0.0f; } else { _state = LimitState.AtUpperLimit; } if (length1 < _maxLength1) { _limitState1 = LimitState.InactiveLimit; _limitImpulse1 = 0.0f; } else { _limitState1 = LimitState.AtUpperLimit; } if (length2 < _maxLength2) { _limitState2 = LimitState.InactiveLimit; _limitImpulse2 = 0.0f; } else { _limitState2 = LimitState.AtUpperLimit; } // Compute effective mass. float cr1u1 = CommonMath.Cross(ref r1, ref _u1); float cr2u2 = CommonMath.Cross(ref r2, ref _u2); _limitMass1 = b1._invMass + b1._invI * cr1u1 * cr1u1; _limitMass2 = b2._invMass + b2._invI * cr2u2 * cr2u2; _pulleyMass = _limitMass1 + _ratio * _ratio * _limitMass2; //Box2DXDebug.Assert(_limitMass1 > Settings.FLT_EPSILON); //Box2DXDebug.Assert(_limitMass2 > Settings.FLT_EPSILON); //Box2DXDebug.Assert(_pulleyMass > Settings.FLT_EPSILON); _limitMass1 = 1.0f / _limitMass1; _limitMass2 = 1.0f / _limitMass2; _pulleyMass = 1.0f / _pulleyMass; if (step.WarmStarting) { // Scale impulses to support variable time steps. _impulse *= step.DtRatio; _limitImpulse1 *= step.DtRatio; _limitImpulse2 *= step.DtRatio; // Warm starting. Vector2 P1 = -(_impulse + _limitImpulse1) * _u1; Vector2 P2 = (-_ratio * _impulse - _limitImpulse2) * _u2; b1._linearVelocity += b1._invMass * P1; b1._angularVelocity += b1._invI * CommonMath.Cross(ref r1, ref P1); b2._linearVelocity += b2._invMass * P2; b2._angularVelocity += b2._invI * CommonMath.Cross(ref r2, ref P2); } else { _impulse = 0.0f; _limitImpulse1 = 0.0f; _limitImpulse2 = 0.0f; } }
public void Solve(ref TimeStep step, ref Vector2 gravity) { float h = step.dt; // Integrate velocities and apply damping. Initialize the body state. for (int i = 0; i < BodyCount; ++i) { Body b = Bodies[i]; Vector2 c = b.Sweep.C; float a = b.Sweep.A; Vector2 v = b.LinearVelocityInternal; float w = b.AngularVelocityInternal; // Store positions for continuous collision. b.Sweep.C0 = b.Sweep.C; b.Sweep.A0 = b.Sweep.A; if (b.BodyType == BodyType.Dynamic) { // Integrate velocities. v += h * (b.GravityScale * gravity + b.InvMass * b.Force); w += h * 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 v *= MathUtils.Clamp(1.0f - h * b.LinearDamping, 0.0f, 1.0f); w *= MathUtils.Clamp(1.0f - h * b.AngularDamping, 0.0f, 1.0f); } _positions[i].c = c; _positions[i].a = a; _velocities[i].v = v; _velocities[i].w = w; } // Solver data SolverData solverData = new SolverData(); solverData.step = step; solverData.positions = _positions; solverData.velocities = _velocities; _contactSolver.Reset(step, ContactCount, _contacts, _positions, _velocities); _contactSolver.InitializeVelocityConstraints(); if (Settings.EnableWarmstarting) { _contactSolver.WarmStart(); } #if (!SILVERLIGHT && !WINDOWS_PHONE) if (Settings.EnableDiagnostics) { _watch.Start(); _tmpTime = 0; } #endif for (int i = 0; i < JointCount; ++i) { //if (_joints[i].Enabled) //TODO: Activate again _joints[i].InitVelocityConstraints(ref solverData); } #if (!SILVERLIGHT && !WINDOWS_PHONE) if (Settings.EnableDiagnostics) { _tmpTime += _watch.ElapsedTicks; } #endif #if (!SILVERLIGHT && !WINDOWS_PHONE) if (Settings.EnableDiagnostics) _watch.Start(); #endif // Solve velocity constraints. for (int i = 0; i < Settings.VelocityIterations; ++i) { for (int j = 0; j < JointCount; ++j) { Joint joint = _joints[j]; //if (!joint.Enabled) //TODO: Activate again // continue; joint.SolveVelocityConstraints(ref solverData); //TODO: Move up before solve? //joint.Validate(step.inv_dt); //TODO: Activate again } _contactSolver.SolveVelocityConstraints(); } #if (!SILVERLIGHT && !WINDOWS_PHONE) if (Settings.EnableDiagnostics) { _watch.Stop(); _tmpTime += _watch.ElapsedTicks; _watch.Reset(); } #endif // Store impulses for warm starting. _contactSolver.StoreImpulses(); // Integrate positions for (int i = 0; i < BodyCount; ++i) { Vector2 c = _positions[i].c; float a = _positions[i].a; Vector2 v = _velocities[i].v; float w = _velocities[i].w; // Check for large velocities Vector2 translation = h * v; if (Vector2.Dot(translation, translation) > Settings.MaxTranslationSquared) { float ratio = Settings.MaxTranslation / translation.Length(); v *= ratio; } float rotation = h * w; if (rotation * rotation > Settings.MaxRotationSquared) { float ratio = Settings.MaxRotation / Math.Abs(rotation); w *= ratio; } // Integrate c += h * v; a += h * w; _positions[i].c = c; _positions[i].a = a; _velocities[i].v = v; _velocities[i].w = w; } #if (!SILVERLIGHT && !WINDOWS_PHONE) if (Settings.EnableDiagnostics) _watch.Start(); #endif // Solve position constraints bool positionSolved = false; for (int i = 0; i < Settings.PositionIterations; ++i) { bool contactsOkay = _contactSolver.SolvePositionConstraints(); bool jointsOkay = true; for (int j = 0; j < JointCount; ++j) { Joint joint = _joints[j]; //if (!joint.Enabled) //TODO: Enable again // continue; bool jointOkay = joint.SolvePositionConstraints(ref solverData); jointsOkay = jointsOkay && jointOkay; } if (contactsOkay && jointsOkay) { // Exit early if the position errors are small. positionSolved = true; break; } } #if (!SILVERLIGHT && !WINDOWS_PHONE) if (Settings.EnableDiagnostics) { _watch.Stop(); _tmpTime += _watch.ElapsedTicks; _watch.Reset(); } #endif #if (!SILVERLIGHT && !WINDOWS_PHONE) if (Settings.EnableDiagnostics) { JointUpdateTime = _tmpTime; } #endif // Copy state buffers back to the bodies for (int i = 0; i < BodyCount; ++i) { Body body = Bodies[i]; body.Sweep.C = _positions[i].c; body.Sweep.A = _positions[i].a; body.LinearVelocityInternal = _velocities[i].v; body.AngularVelocityInternal = _velocities[i].w; body.SynchronizeTransform(); } Report(_contactSolver._velocityConstraints); if (Settings.AllowSleep) { float minSleepTime = Settings.MaxFloat; for (int i = 0; i < BodyCount; ++i) { Body b = Bodies[i]; if (b.BodyType == BodyType.Static) continue; 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 += h; minSleepTime = Math.Min(minSleepTime, b.SleepTime); } } if (minSleepTime >= Settings.TimeToSleep && positionSolved) { for (int i = 0; i < BodyCount; ++i) { Body b = Bodies[i]; b.Awake = false; } } } }
public void Solve(ref TimeStep step, ref Vector2 gravity) { // Integrate velocities and apply damping. for (int i = 0; i < BodyCount; ++i) { Body 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 += step.dt * (b.InvMass * b.Force); b.AngularVelocityInternal += step.dt * b.InvI * b.Torque; } else { b.LinearVelocityInternal += step.dt * (gravity + b.InvMass * b.Force); 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; Body bodyA = fixtureA.Body; Body 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 //if (i1 != i2) MathUtils.Swap(ref _contacts[i1], ref _contacts[i2]); //Contact temp = _contacts[i1]; //_contacts[i1] = _contacts[i2]; //_contacts[i2] = temp; } } // Initialize velocity constraints. _contactSolver.Reset(_contacts, ContactCount, step.dtRatio); _contactSolver.InitializeVelocityConstraints(); if (Settings.EnableWarmstarting) { _contactSolver.WarmStart(); } #if (!SILVERLIGHT && !WINDOWS_PHONE) if (Settings.EnableDiagnostics) { _watch.Start(); _tmpTime = 0; } #endif for (int i = 0; i < JointCount; ++i) { _joints[i].InitVelocityConstraints(ref step); } #if (!SILVERLIGHT && !WINDOWS_PHONE) if (Settings.EnableDiagnostics) { _tmpTime += _watch.ElapsedTicks; } #endif // Solve velocity constraints. for (int i = 0; i < Settings.VelocityIterations; ++i) { #if (!SILVERLIGHT && !WINDOWS_PHONE) if (Settings.EnableDiagnostics) { _watch.Start(); } #endif for (int j = 0; j < JointCount; ++j) { _joints[j].SolveVelocityConstraints(ref step); } #if (!SILVERLIGHT && !WINDOWS_PHONE) 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) { Body b = Bodies[i]; if (b.BodyType == BodyType.Static) { continue; } // Check for large velocities. Vector2 translation = step.dt * b.LinearVelocityInternal; float result; Vector2.Dot(ref translation, ref translation, out result); if (result > Settings.MaxTranslationSquared) { float ratio = Settings.MaxTranslation / translation.Length(); b.LinearVelocityInternal *= 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 = b.Sweep.C; b.Sweep.A0 = b.Sweep.A; // Integrate b.Sweep.C += step.dt * b.LinearVelocityInternal; 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 && !WINDOWS_PHONE) if (Settings.EnableDiagnostics) { _watch.Start(); } #endif for (int j = 0; j < JointCount; ++j) { bool jointOkay = _joints[j].SolvePositionConstraints(); jointsOkay = jointsOkay && jointOkay; } #if (!SILVERLIGHT && !WINDOWS_PHONE) 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 && !WINDOWS_PHONE) if (Settings.EnableDiagnostics) { JointUpdateTime = _tmpTime; } #endif if (_contactManager.PostSolve != null) { Report(_contactSolver.Constraints); } if (Settings.AllowSleep) { float minSleepTime = Settings.MaxFloat; const float linTolSqr = Settings.LinearSleepTolerance * Settings.LinearSleepTolerance; const float angTolSqr = Settings.AngularSleepTolerance * Settings.AngularSleepTolerance; for (int i = 0; i < BodyCount; ++i) { Body 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) { Body b = Bodies[i]; b.Awake = false; } } } }
internal void SolveTOI(ref TimeStep subStep, int toiIndexA, int toiIndexB, bool warmstarting) { Debug.Assert(toiIndexA < BodyCount); Debug.Assert(toiIndexB < BodyCount); // Initialize the body state. for (int i = 0; i < BodyCount; ++i) { Body b = Bodies[i]; _positions[i].c = b.Sweep.C; _positions[i].a = b.Sweep.A; _velocities[i].v = b.LinearVelocityInternal; _velocities[i].w = b.AngularVelocityInternal; } _contactSolver.Reset(subStep, ContactCount, _contacts, _positions, _velocities, warmstarting); // Solve position constraints. for (int i = 0; i < Settings.TOIPositionIterations; ++i) { bool contactsOkay = _contactSolver.SolveTOIPositionConstraints(toiIndexA, toiIndexB); if (contactsOkay) { break; } } // Leap of faith to new safe state. Bodies[toiIndexA].Sweep.C0 = _positions[toiIndexA].c; Bodies[toiIndexA].Sweep.A0 = _positions[toiIndexA].a; Bodies[toiIndexB].Sweep.C0 = _positions[toiIndexB].c; Bodies[toiIndexB].Sweep.A0 = _positions[toiIndexB].a; // 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. float h = subStep.dt; // Integrate positions. for (int i = 0; i < BodyCount; ++i) { Vector2 c = _positions[i].c; float a = _positions[i].a; Vector2 v = _velocities[i].v; float w = _velocities[i].w; // Check for large velocities Vector2 translation = h * v; if (Vector2.Dot(translation, translation) > Settings.MaxTranslationSquared) { float ratio = Settings.MaxTranslation / translation.Length(); v *= ratio; } float rotation = h * w; if (rotation * rotation > Settings.MaxRotationSquared) { float ratio = Settings.MaxRotation / Math.Abs(rotation); w *= ratio; } // Integrate c += h * v; a += h * w; _positions[i].c = c; _positions[i].a = a; _velocities[i].v = v; _velocities[i].w = w; // Sync bodies Body body = Bodies[i]; body.Sweep.C = c; body.Sweep.A = a; body.LinearVelocityInternal = v; body.AngularVelocityInternal = w; body.SynchronizeTransform(); } Report(_contactSolver._velocityConstraints); }
internal void SolveTOI(ref TimeStep subStep, int toiIndexA, int toiIndexB, bool warmstarting) { Debug.Assert(toiIndexA < BodyCount); Debug.Assert(toiIndexB < BodyCount); // Initialize the body state. for (int i = 0; i < BodyCount; ++i) { Body b = Bodies[i]; _positions[i].c = b._sweep.C; _positions[i].a = b._sweep.A; _velocities[i].v = b._linearVelocity; _velocities[i].w = b._angularVelocity; } _contactSolver.Reset(subStep, ContactCount, _contacts, _positions, _velocities, warmstarting); // Solve position constraints. for (int i = 0; i < Settings.TOIPositionIterations; ++i) { bool contactsOkay = _contactSolver.SolveTOIPositionConstraints(toiIndexA, toiIndexB); if (contactsOkay) { break; } } // Leap of faith to new safe state. Bodies[toiIndexA]._sweep.C0 = _positions[toiIndexA].c; Bodies[toiIndexA]._sweep.A0 = _positions[toiIndexA].a; Bodies[toiIndexB]._sweep.C0 = _positions[toiIndexB].c; Bodies[toiIndexB]._sweep.A0 = _positions[toiIndexB].a; // 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. float h = subStep.dt; // Integrate positions. for (int i = 0; i < BodyCount; ++i) { Vector2 c = _positions[i].c; float a = _positions[i].a; Vector2 v = _velocities[i].v; float w = _velocities[i].w; // Check for large velocities Vector2 translation = h * v; if (Vector2.Dot(translation, translation) > Settings.MaxTranslationSquared) { float ratio = Settings.MaxTranslation / translation.Length(); v *= ratio; } float rotation = h * w; if (rotation * rotation > Settings.MaxRotationSquared) { float ratio = Settings.MaxRotation / Math.Abs(rotation); w *= ratio; } // Integrate c += h * v; a += h * w; _positions[i].c = c; _positions[i].a = a; _velocities[i].v = v; _velocities[i].w = w; // Sync bodies Body body = Bodies[i]; body._sweep.C = c; body._sweep.A = a; body._linearVelocity = v; body._angularVelocity = w; body.SynchronizeTransform(); } Report(_contactSolver._velocityConstraints); }
public void Solve(ref TimeStep step, ref Vector2 gravity) { // Integrate velocities and apply damping. for (int i = 0; i < BodyCount; ++i) { Body 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; Body bodyA = fixtureA.Body; Body 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) { Joint 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) { Body 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) { Joint 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) { Body 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) { Body b = Bodies[i]; b.Awake = false; } } } }
public void Solve(ref TimeStep step, ref Vector2 gravity) { float h = step.dt; // Integrate velocities and apply damping. Initialize the body state. for (int i = 0; i < BodyCount; ++i) { Body b = Bodies[i]; Vector2 c = b._sweep.C; float a = b._sweep.A; Vector2 v = b._linearVelocity; float w = b._angularVelocity; // Store positions for continuous collision. b._sweep.C0 = b._sweep.C; b._sweep.A0 = b._sweep.A; if (b.BodyType == BodyType.Dynamic) { // Integrate velocities. // FPE: Only apply gravity if the body wants it. if (b.IgnoreGravity) { v += h * (b._invMass * b._force); } else { v += h * (b.GravityScale * gravity + b._invMass * b._force); } w += h * 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 v *= MathUtils.Clamp(1.0f - h * b.LinearDamping, 0.0f, 1.0f); w *= MathUtils.Clamp(1.0f - h * b.AngularDamping, 0.0f, 1.0f); } _positions[i].c = c; _positions[i].a = a; _velocities[i].v = v; _velocities[i].w = w; } // Solver data SolverData solverData = new SolverData(); solverData.step = step; solverData.positions = _positions; solverData.velocities = _velocities; _contactSolver.Reset(step, ContactCount, _contacts, _positions, _velocities); _contactSolver.InitializeVelocityConstraints(); if (Settings.EnableWarmstarting) { _contactSolver.WarmStart(); } if (Settings.EnableDiagnostics) { _watch.Start(); } for (int i = 0; i < JointCount; ++i) { if (_joints[i].Enabled) { _joints[i].InitVelocityConstraints(ref solverData); } } if (Settings.EnableDiagnostics) { _watch.Stop(); } // Solve velocity constraints. for (int i = 0; i < Settings.VelocityIterations; ++i) { for (int j = 0; j < JointCount; ++j) { Joint joint = _joints[j]; if (!joint.Enabled) { continue; } if (Settings.EnableDiagnostics) { _watch.Start(); } joint.SolveVelocityConstraints(ref solverData); joint.Validate(step.inv_dt); if (Settings.EnableDiagnostics) { _watch.Stop(); } } _contactSolver.SolveVelocityConstraints(); } // Store impulses for warm starting. _contactSolver.StoreImpulses(); // Integrate positions for (int i = 0; i < BodyCount; ++i) { Vector2 c = _positions[i].c; float a = _positions[i].a; Vector2 v = _velocities[i].v; float w = _velocities[i].w; // Check for large velocities Vector2 translation = h * v; if (Vector2.Dot(translation, translation) > Settings.MaxTranslationSquared) { float ratio = Settings.MaxTranslation / translation.Length(); v *= ratio; } float rotation = h * w; if (rotation * rotation > Settings.MaxRotationSquared) { float ratio = Settings.MaxRotation / Math.Abs(rotation); w *= ratio; } // Integrate c += h * v; a += h * w; _positions[i].c = c; _positions[i].a = a; _velocities[i].v = v; _velocities[i].w = w; } // Solve position constraints bool positionSolved = false; for (int i = 0; i < Settings.PositionIterations; ++i) { bool contactsOkay = _contactSolver.SolvePositionConstraints(); bool jointsOkay = true; for (int j = 0; j < JointCount; ++j) { Joint joint = _joints[j]; if (!joint.Enabled) { continue; } if (Settings.EnableDiagnostics) { _watch.Start(); } bool jointOkay = joint.SolvePositionConstraints(ref solverData); if (Settings.EnableDiagnostics) { _watch.Stop(); } jointsOkay = jointsOkay && jointOkay; } if (contactsOkay && jointsOkay) { // Exit early if the position errors are small. positionSolved = true; break; } } if (Settings.EnableDiagnostics) { JointUpdateTime = _watch.ElapsedTicks; _watch.Reset(); } // Copy state buffers back to the bodies for (int i = 0; i < BodyCount; ++i) { Body body = Bodies[i]; body._sweep.C = _positions[i].c; body._sweep.A = _positions[i].a; body._linearVelocity = _velocities[i].v; body._angularVelocity = _velocities[i].w; body.SynchronizeTransform(); } Report(_contactSolver._velocityConstraints); if (Settings.AllowSleep) { float minSleepTime = Settings.MaxFloat; for (int i = 0; i < BodyCount; ++i) { Body b = Bodies[i]; if (b.BodyType == BodyType.Static) { continue; } if (!b.SleepingAllowed || b._angularVelocity * b._angularVelocity > AngTolSqr || Vector2.Dot(b._linearVelocity, b._linearVelocity) > LinTolSqr) { b._sleepTime = 0.0f; minSleepTime = 0.0f; } else { b._sleepTime += h; minSleepTime = Math.Min(minSleepTime, b._sleepTime); } } if (minSleepTime >= Settings.TimeToSleep && positionSolved) { for (int i = 0; i < BodyCount; ++i) { Body b = Bodies[i]; b.Awake = false; } } } }
/// <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; } } }
public void Solve(ref TimeStep step, Vector2 gravity) { // Integrate velocities and apply damping. for (int i = 0; i < BodyCount; ++i) { Body b = Bodies[i]; if (b.BodyType != BodyType.Dynamic) { continue; } // Integrate velocities. Only apply gravity if the body wants it. if (b.IgnoreGravity) { b.LinearVelocityInternal += step.dt * (b.InvMass * b.Force); b.AngularVelocityInternal += step.dt * b.InvI * b.Torque; } else { b.LinearVelocityInternal += step.dt * (gravity + b.InvMass * b.Force); 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; Body bodyA = fixtureA.Body; Body bodyB = fixtureB.Body; bool nonStatic = bodyA.BodyType != BodyType.Static && bodyB.BodyType != 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 < Settings.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.BodyType == BodyType.Static) { continue; } // Check for large velocities. Vector2 translation = step.dt * b.LinearVelocityInternal; if (Vector2.Dot(translation, translation) > Settings.MaxTranslationSquared) { float ratio = Settings.MaxTranslation / translation.Length(); b.LinearVelocityInternal *= 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 = b.Sweep.c; b.Sweep.a0 = b.Sweep.a; // Integrate b.Sweep.c += step.dt * b.LinearVelocityInternal; 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; for (int j = 0; j < JointCount; ++j) { bool jointOkay = _joints[j].SolvePositionConstraints(); jointsOkay = jointsOkay && jointOkay; } if (contactsOkay && jointsOkay) { // Exit early if the position errors are small. break; } } if (_contactManager.PostSolve != null) { Report(_contactSolver.Constraints); } if (Settings.AllowSleep) { float minSleepTime = Settings.MaxFloat; const float linTolSqr = Settings.LinearSleepTolerance * Settings.LinearSleepTolerance; const float angTolSqr = Settings.AngularSleepTolerance * Settings.AngularSleepTolerance; for (int i = 0; i < BodyCount; ++i) { Body 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) { Body b = Bodies[i]; b.Awake = false; } } } }