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)
{
PhysicsBody 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)
{
PhysicsBody 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);
}
/// <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 (int i = 0; i < ContactManager.ContactList.Count; i++)
{
Contact c = ContactManager.ContactList[i];
// 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 (int i = 0; i < ContactManager.ContactList.Count; i++)
{
Contact c = ContactManager.ContactList[i];
// 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;
}
PhysicsBody bA = fA.Body;
PhysicsBody bB = fB.Body;
BodyType typeA = bA.BodyType;
BodyType typeB = bB.BodyType;
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) && !bA.IgnoreCCD;
bool collideB = (bB.IsBullet || typeB != BodyType.Dynamic) && !bB.IgnoreCCD;
// 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]
_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.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;
PhysicsBody bA1 = fA1.Body;
PhysicsBody 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.
PhysicsBody[] bodies = { bA1, bB1 };
for (int i = 0; i < 2; ++i)
{
PhysicsBody 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.
PhysicsBody 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)
{
PhysicsBody 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 (EnableSubStepping)
{
_stepComplete = false;
break;
}
}
}
public void Solve(ref TimeStep step, ref Vector2 gravity)
{
// Integrate velocities and apply damping.
for (int i = 0; i < BodyCount; ++i)
{
PhysicsBody 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;
PhysicsBody bodyA = fixtureA.Body;
PhysicsBody 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)
{
PhysicsJoint 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)
{
PhysicsBody 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)
{
PhysicsJoint 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)
{
PhysicsBody 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)
{
PhysicsBody b = Bodies[i];
b.Awake = false;
}
}
}
}
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.
foreach (PhysicsBody b in BodyList)
{
b.Flags &= ~BodyFlags.Island;
}
for (int i = 0; i < ContactManager.ContactList.Count; i++)
{
Contact c = ContactManager.ContactList[i];
c.Flags &= ~ContactFlags.Island;
}
foreach (PhysicsJoint j in JointList)
{
j.IslandFlag = false;
}
// Build and simulate all awake islands.
int stackSize = BodyList.Count;
if (stackSize > _stack.Length)
_stack = new PhysicsBody[Math.Max(_stack.Length * 2, stackSize)];
for (int index = BodyList.Count - 1; index >= 0; index--)
{
PhysicsBody 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.
PhysicsBody 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;
PhysicsBody 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;
}
PhysicsBody 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.
PhysicsBody b = Island.Bodies[i];
if (b.BodyType == BodyType.Static)
{
b.Flags &= ~BodyFlags.Island;
}
}
}
// Synchronize fixtures, check for out of range bodies.
foreach (PhysicsBody 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();
}