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;
}
}
}
}
