// Find TOI contacts and solve them.
public void SolveTOI(b2TimeStep step)
{
b2Island island = new b2Island(2 * b2Settings.b2_maxTOIContacts, b2Settings.b2_maxTOIContacts, 0, m_contactManager.ContactListener);
if (m_stepComplete)
{
for (b2Body b = m_bodyList; b; b = b.Next)
{
b.BodyFlags &= ~b2Body.e_islandFlag;
b.m_sweep.alpha0 = 0.0f;
}
for (b2Contact c = m_contactManager.ContactList; c; c = c.Next)
{
// Invalidate TOI
c.ContactFlags &= ~(b2ContactType.e_toiFlag | b2ContactType.e_islandFlag);
c.m_toiCount = 0;
c.m_toi = 1.0f;
}
}
// Find TOI events and solve them.
for (; ;)
{
// Find the first TOI.
b2Contact minContact = null;
float minAlpha = 1.0f;
for (b2Contact c = m_contactManager.ContactList; c != null; c = c.Next)
{
// Is this contact disabled?
if (c.IsEnabled() == false)
{
continue;
}
// Prevent excessive sub-stepping.
if (c.m_toiCount > b2Settings.b2_maxSubSteps)
{
continue;
}
float alpha = 1.0f;
if (c.ContactFlags.HasFlag(b2ContactFlags.e_toiFlag))
{
// This contact has a valid cached TOI.
alpha = c.m_toi;
}
else
{
b2Fixture fA = c.GetFixtureA();
b2Fixture fB = c.GetFixtureB();
// Is there a sensor?
if (fA.IsSensor || fB.IsSensor)
{
continue;
}
b2Body bA = fA.Body;
b2Body bB = fB.Body;
b2BodyType typeA = bA.BodyType;
b2BodyType typeB = bB.BodyType;
bool activeA = bA.IsAwake() && typeA != b2BodyType.b2_staticBody;
bool activeB = bB.IsAwake() && typeB != b2BodyType.b2_staticBody;
// Is at least one body active (awake and dynamic or kinematic)?
if (activeA == false && activeB == false)
{
continue;
}
bool collideA = bA.IsBullet() || typeA != b2BodyType.b2_dynamicBody;
bool collideB = bB.IsBullet() || typeB != b2BodyType.b2_dynamicBody;
// 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);
}
int indexA = c.GetChildIndexA();
int indexB = c.GetChildIndexB();
// Compute the time of impact in interval [0, minTOI]
b2TOIInput input = new b2TOIInput();
input.proxyA.Set(fA.Shape, indexA);
input.proxyB.Set(fB.Shape, indexB);
input.sweepA = bA.Sweep;
input.sweepB = bB.Sweep;
input.tMax = 1.0f;
b2TOIOutput output = b2TimeOfImpact(input);
// Beta is the fraction of the remaining portion of the .
float beta = output.t;
if (output.state == b2TOIOutputType.e_touching)
{
alpha = b2Math.b2Min(alpha0 + (1.0f - alpha0) * beta, 1.0f);
}
else
{
alpha = 1.0f;
}
c.m_toi = alpha;
c.ContactFlags |= b2ContactFlags.e_toiFlag;
}
if (alpha < minAlpha)
{
// This is the minimum TOI found so far.
minContact = c;
minAlpha = alpha;
}
}
if (minContact == null || 1.0f - 10.0f * b2Settings.b2_epsilon < minAlpha)
{
// No more TOI events. Done!
m_stepComplete = true;
break;
}
{
// Advance the bodies to the TOI.
b2Fixture fA = minContact.GetFixtureA();
b2Fixture fB = minContact.GetFixtureB();
b2Body bA = fA.Body;
b2Body bB = fB.Body;
b2Sweep backup1 = bA.Sweep;
b2Sweep backup2 = bB.Sweep;
bA.Advance(minAlpha);
bB.Advance(minAlpha);
// The TOI contact likely has some new contact points.
minContact.Update(m_contactManager.ContactListener);
minContact.ContactFlags &= ~b2ContactFlags.e_toiFlag;
++minContact.m_toiCount;
// Is the contact solid?
if (minContact.IsEnabled() == false || minContact.IsTouching() == false)
{
// Restore the sweeps.
minContact.SetEnabled(false);
bA.Sweep = backup1;
bB.Sweep = backup2;
bA.SynchronizeTransform();
bB.SynchronizeTransform();
continue;
}
bA.SetAwake(true);
bB.SetAwake(true);
// Build the island
island.Clear();
island.Add(bA);
island.Add(bB);
island.Add(minContact);
bA.BodyFlags |= b2BodyFlags.e_islandFlag;
bB.BodyFlags |= b2BodyFlags.e_islandFlag;
minContact.ContentType |= b2ContactFlags.e_islandFlag;
// Get contacts on bodyA and bodyB.
b2Body[] bodies = new b2Body[] { bA, bB };
for (int i = 0; i < 2; ++i)
{
b2Body body = bodies[i];
if (body.BodyType == b2BodyType.b2_dynamicBody)
{
for (b2ContactEdge ce = body.ContactList; ce != null; ce = ce.next)
{
if (island.BodyCount == island.BodyCapacity)
{
break;
}
if (island.ContactCount == island.ContactCapacity)
{
break;
}
b2Contact contact = ce.contact;
// Has this contact already been added to the island?
if (contact.ContactType & b2ContactType.e_islandFlag)
{
continue;
}
// Only add static, kinematic, or bullet bodies.
b2Body other = ce.other;
if (other.BodyType == b2BodyType.b2_dynamicBody &&
body.IsBullet() == false && other.IsBullet() == false)
{
continue;
}
// Skip sensors.
bool sensorA = contact.m_fixtureA.m_isSensor;
bool sensorB = contact.m_fixtureB.m_isSensor;
if (sensorA || sensorB)
{
continue;
}
// Tentatively advance the body to the TOI.
b2Sweep backup = other.Sweep;
if (other.BodyFlags.HasFlag(b2BodyFlags.e_islandFlag))
{
other.Advance(minAlpha);
}
// Update the contact points
contact.Update(m_contactManager.ContactListener);
// Was the contact disabled by the user?
if (contact.IsEnabled() == 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.ContactFlags |= b2ContactFlags.e_islandFlag;
island.Add(contact);
// Has the other body already been added to the island?
if (other.BodyFlags.HasFlag(b2BodyFlags.e_islandFlag))
{
continue;
}
// Add the other body to the island.
other.BodyFlags |= b2BodyFlags.e_islandFlag;
if (other.BodyType != b2BodyType.b2_staticBody)
{
other.SetAwake(true);
}
island.Add(other);
}
}
}
b2TimeStep 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(subStep, bA.m_islandIndex, bB.m_islandIndex);
// Reset island flags and synchronize broad-phase proxies.
for (int i = 0; i < island.m_bodyCount; ++i)
{
b2Body body = island.m_bodies[i];
body.BodyFlags &= ~b2BodyFlags.e_islandFlag;
if (body.BodyType != b2BodyType.b2_dynamicBody)
{
continue;
}
body.SynchronizeFixtures();
// Invalidate all contact TOIs on this displaced body.
for (b2ContactEdge ce = body.ContactList; ce != null; ce = ce.next)
{
ce.Contact.ContactFlags &= ~(b2ContactFlags.e_toiFlag | b2ContactFlags.e_islandFlag);
}
}
// Commit fixture proxy movements to the broad-phase so that new contacts are created.
// Also, some contacts can be destroyed.
m_contactManager.FindNewContacts();
if (m_subStepping)
{
m_stepComplete = false;
break;
}
}
}
}
public void SolveTOI(b2TimeStep subStep, int toiIndexA, int toiIndexB)
{
Debug.Assert(toiIndexA < m_bodyCount);
Debug.Assert(toiIndexB < m_bodyCount);
// Initialize the body state.
for (int i = 0; i < m_bodyCount; ++i)
{
b2Body b = m_bodies[i];
m_positions[i].c = b.Sweep.c;
m_positions[i].a = b.Sweep.a;
m_velocities[i].v = b.LinearVelocity;
m_velocities[i].w = b.AngularVelocity;
}
b2ContactSolverDef contactSolverDef;
contactSolverDef.contacts = m_contacts;
contactSolverDef.count = m_contactCount;
contactSolverDef.step = subStep;
contactSolverDef.positions = m_positions;
contactSolverDef.velocities = m_velocities;
b2ContactSolver contactSolver = new b2ContactSolver(contactSolverDef);
// Solve position constraints.
for (int i = 0; i < subStep.positionIterations; ++i)
{
bool contactsOkay = contactSolver.SolveTOIPositionConstraints(toiIndexA, toiIndexB);
if (contactsOkay)
{
break;
}
}
#if false
// Is the new position really safe?
for (int i = 0; i < m_contactCount; ++i)
{
b2Contact c = m_contacts[i];
b2Fixture fA = c.GetFixtureA();
b2Fixture fB = c.GetFixtureB();
b2Body bA = fA.Body;
b2Body bB = fB.Body;
int indexA = c.GetChildIndexA();
int indexB = c.GetChildIndexB();
b2DistanceInput input = new b2DistanceInput();
input.proxyA.Set(fA.Shape, indexA);
input.proxyB.Set(fB.Shape, indexB);
input.transformA = bA.Transform;
input.transformB = bB.Transform;
input.useRadii = false;
b2DistanceOutput output;
b2SimplexCache cache = new b2SimplexCache();
cache.count = 0;
output = b2Distance(cache, input);
if (output.distance == 0 || cache.count == 3)
{
cache.count += 0;
}
}
#endif
// Leap of faith to new safe state.
m_bodies[toiIndexA].Sweep.c0 = m_positions[toiIndexA].c;
m_bodies[toiIndexA].Sweep.a0 = m_positions[toiIndexA].a;
m_bodies[toiIndexB].Sweep.c0 = m_positions[toiIndexB].c;
m_bodies[toiIndexB].Sweep.a0 = m_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 < subStep.velocityIterations; ++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 < m_bodyCount; ++i)
{
b2Vec2 c = m_positions[i].c;
float a = m_positions[i].a;
b2Vec2 v = m_velocities[i].v;
float w = m_velocities[i].w;
// Check for large velocities
b2Vec2 translation = h * v;
if (b2Math.b2Dot(translation, translation) > b2Settings.b2_maxTranslationSquared)
{
float ratio = b2Settings.b2_maxTranslation / translation.Length;
v *= ratio;
}
float rotation = h * w;
if (rotation * rotation > b2Settings.b2_maxRotationSquared)
{
float ratio = b2Settings.b2_maxRotation / Math.Abs(rotation);
w *= ratio;
}
// Integrate
c += h * v;
a += h * w;
m_positions[i].c = c;
m_positions[i].a = a;
m_velocities[i].v = v;
m_velocities[i].w = w;
// Sync bodies
b2Body body = m_bodies[i];
body.Sweep.c = c;
body.Sweep.a = a;
body.LinearVelocity = v;
body.AngularVelocity = w;
body.SynchronizeTransform();
}
Report(contactSolver.m_velocityConstraints);
}
public void SolveTOI(b2TimeStep subStep)
{
int i;
int j;
m_contactSolver.Initialize(subStep, m_contacts, m_contactCount, m_allocator);
b2ContactSolver contactSolver = m_contactSolver;
// No warm starting is needed for TOI events because warm
// starting impulses were applied in the discrete solver.
// Warm starting for joints is off for now, but we need to
// call this function to compute Jacobians.
for (i = 0; i < m_jointCount; ++i)
{
m_joints[i].InitVelocityConstraints(subStep);
}
// Solve velocity constraints.
for (i = 0; i < subStep.velocityIterations; ++i)
{
contactSolver.SolveVelocityConstraints();
for (j = 0; j < m_jointCount; ++j)
{
m_joints[j].SolveVelocityConstraints(subStep);
}
}
// Don't store the TOI contact forces for warm starting
// because they can be quite large.
// Integrate positions.
for (i = 0; i < m_bodyCount; ++i)
{
b2Body b = m_bodies[i];
if (b.GetType() == b2Body.b2_staticBody)
{
continue;
}
// Check for large velocities.
// b2Vec2 translation = subStep.dt * b.m_linearVelocity;
float translationX = subStep.dt * b.m_linearVelocity.x;
float translationY = subStep.dt * b.m_linearVelocity.y;
//if (b2Dot(translation, translation) > b2_maxTranslationSquared)
if ((translationX * translationX + translationY * translationY) > b2Settings.b2_maxTranslationSquared)
{
b.m_linearVelocity.Normalize();
b.m_linearVelocity.x *= b2Settings.b2_maxTranslation * subStep.inv_dt;
b.m_linearVelocity.y *= b2Settings.b2_maxTranslation * subStep.inv_dt;
}
float rotation = subStep.dt * b.m_angularVelocity;
if (rotation * rotation > b2Settings.b2_maxRotationSquared)
{
if (b.m_angularVelocity < 0.0f)
{
b.m_angularVelocity = -b2Settings.b2_maxRotation * subStep.inv_dt;
}
else
{
b.m_angularVelocity = b2Settings.b2_maxRotation * subStep.inv_dt;
}
}
// Store positions for continuous collision.
b.m_sweep.c0.SetV(b.m_sweep.c);
b.m_sweep.a0 = b.m_sweep.a;
// Integrate
b.m_sweep.c.x += subStep.dt * b.m_linearVelocity.x;
b.m_sweep.c.y += subStep.dt * b.m_linearVelocity.y;
b.m_sweep.a += subStep.dt * b.m_angularVelocity;
// Compute new transform
b.SynchronizeTransform();
// Note: shapes are synchronized later.
}
// Solve position constraints.
float k_toiBaumgarte = 0.75f;
for (i = 0; i < subStep.positionIterations; ++i)
{
bool contactsOkay = contactSolver.SolvePositionConstraints(k_toiBaumgarte);
bool jointsOkay = true;
for (j = 0; j < m_jointCount; ++j)
{
bool jointOkay = m_joints[j].SolvePositionConstraints(b2Settings.b2_contactBaumgarte);
jointsOkay = jointsOkay && jointOkay;
}
if (contactsOkay && jointsOkay)
{
break;
}
}
Report(contactSolver.m_constraints);
}
public void Solve(b2TimeStep step, b2Vec2 gravity, bool allowSleep)
#endif
{
#if PROFILING
b2Timer timer = new b2Timer();
#endif
float h = step.dt;
// Integrate velocities and apply damping. Initialize the body state.
for (int i = 0; i < m_bodyCount; ++i)
{
b2Body b = m_bodies[i];
b2Vec2 c = b.Sweep.c;
float a = b.Sweep.a;
b2Vec2 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 == b2BodyType.b2_dynamicBody)
{
// Integrate velocities.
v += h * (b.GravityScale * gravity + b.InvertedMass * b.Force);
w += h * b.InvertedI * 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 *= b2Math.b2Clamp(1.0f - h * b.LinearDamping, 0.0f, 1.0f);
w *= b2Math.b2Clamp(1.0f - h * b.AngularDamping, 0.0f, 1.0f);
}
m_positions[i].c = c;
m_positions[i].a = a;
m_velocities[i].v = v;
m_velocities[i].w = w;
}
#if PROFILING
timer.Reset();
#endif
// Solver data
b2SolverData solverData = new b2SolverData();
solverData.step = step;
solverData.positions = m_positions;
solverData.velocities = m_velocities;
// Initialize velocity constraints.
b2ContactSolverDef contactSolverDef;
contactSolverDef.step = step;
contactSolverDef.contacts = m_contacts;
contactSolverDef.count = m_contactCount;
contactSolverDef.positions = m_positions;
contactSolverDef.velocities = m_velocities;
b2ContactSolver contactSolver = new b2ContactSolver(contactSolverDef);
contactSolver.InitializeVelocityConstraints();
if (step.warmStarting)
{
contactSolver.WarmStart();
}
for (int i = 0; i < m_jointCount; ++i)
{
m_joints[i].InitVelocityConstraints(solverData);
}
#if PROFILING
profile.solveInit = timer.GetMilliseconds();
#endif
// Solve velocity constraints
#if PROFILING
timer.Reset();
#endif
for (int i = 0; i < step.velocityIterations; ++i)
{
for (int j = 0; j < m_jointCount; ++j)
{
m_joints[j].SolveVelocityConstraints(solverData);
}
contactSolver.SolveVelocityConstraints();
}
// Store impulses for warm starting
contactSolver.StoreImpulses();
#if PROFILING
profile.solveVelocity = timer.GetMilliseconds();
#endif
// Integrate positions
for (int i = 0; i < m_bodyCount; ++i)
{
b2Vec2 c = m_positions[i].c;
float a = m_positions[i].a;
b2Vec2 v = m_velocities[i].v;
float w = m_velocities[i].w;
// Check for large velocities
b2Vec2 translation = h * v;
if (translation.LengthSquared /* b2Math.b2Dot(translation, translation)*/ > b2Settings.b2_maxTranslationSquared)
{
float ratio = b2Settings.b2_maxTranslation / translation.Length;
v *= ratio;
}
float rotation = h * w;
if (rotation * rotation > b2Settings.b2_maxRotationSquared)
{
float ratio = b2Settings.b2_maxRotation / Math.Abs(rotation);
w *= ratio;
}
// Integrate
c += h * v;
a += h * w;
m_positions[i].c = c;
m_positions[i].a = a;
m_velocities[i].v = v;
m_velocities[i].w = w;
}
// Solve position constraints
#if PROFILING
timer.Reset();
#endif
bool positionSolved = false;
for (int i = 0; i < step.positionIterations; ++i)
{
bool contactsOkay = contactSolver.SolvePositionConstraints();
bool jointsOkay = true;
for (int i2 = 0; i2 < m_jointCount; ++i2)
{
bool jointOkay = m_joints[i2].SolvePositionConstraints(solverData);
jointsOkay = jointsOkay && jointOkay;
}
if (contactsOkay && jointsOkay)
{
// Exit early if the position errors are small.
positionSolved = true;
break;
}
}
// Copy state buffers back to the bodies
for (int i = 0; i < m_bodyCount; ++i)
{
b2Body body = m_bodies[i];
body.Sweep.c = m_positions[i].c;
body.Sweep.a = m_positions[i].a;
body.LinearVelocity = m_velocities[i].v;
body.AngularVelocity = m_velocities[i].w;
body.SynchronizeTransform();
}
#if PROFILING
profile.solvePosition = timer.GetMilliseconds();
#endif
Report(contactSolver.m_velocityConstraints);
if (allowSleep)
{
float minSleepTime = b2Settings.b2_maxFloat;
float linTolSqr = b2Settings.b2_linearSleepTolerance * b2Settings.b2_linearSleepTolerance;
float angTolSqr = b2Settings.b2_angularSleepTolerance * b2Settings.b2_angularSleepTolerance;
for (int i = 0; i < m_bodyCount; ++i)
{
b2Body b = m_bodies[i];
if (b.BodyType == b2BodyType.b2_staticBody)
{
continue;
}
if (!(b.BodyFlags.HasFlag(b2BodyFlags.e_autoSleepFlag)) ||
b.AngularVelocity * b.AngularVelocity > angTolSqr ||
b2Math.b2Dot(b.LinearVelocity, b.LinearVelocity) > linTolSqr)
{
b.SleepTime = 0.0f;
minSleepTime = 0.0f;
}
else
{
b.SleepTime += h;
minSleepTime = Math.Min(minSleepTime, b.SleepTime);
}
}
if (minSleepTime >= b2Settings.b2_timeToSleep && positionSolved)
{
for (int i = 0; i < m_bodyCount; ++i)
{
b2Body b = m_bodies[i];
b.SetAwake(false);
}
}
}
}
