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
}
