public void SolveTOI(ref 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];
b.InternalPosition.c = b.Sweep.c;
b.InternalPosition.a = b.Sweep.a;
b.InternalVelocity.v = b.LinearVelocity;
b.InternalVelocity.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 = b2ContactSolver.Create(ref 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
var bodyA = m_bodies[toiIndexA];
var bodyB = m_bodies[toiIndexB];
// Leap of faith to new safe state.
bodyA.Sweep.c0 = bodyA.InternalPosition.c;
bodyA.Sweep.a0 = bodyA.InternalPosition.a;
bodyB.Sweep.c0 = bodyB.InternalPosition.c;
bodyB.Sweep.a0 = bodyB.InternalPosition.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, count = m_bodyCount; i < count; ++i)
{
var body = m_bodies[i];
b2Vec2 c = body.InternalPosition.c;
float a = body.InternalPosition.a;
b2Vec2 v = body.InternalVelocity.v;
float w = body.InternalVelocity.w;
// Check for large velocities
b2Vec2 translation = h * v;
if (b2Math.b2Dot(ref translation, ref 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;
body.InternalPosition.c = c;
body.InternalPosition.a = a;
body.InternalVelocity.v = v;
body.InternalVelocity.w = w;
// Sync bodies
body.Sweep.c = c;
body.Sweep.a = a;
body.LinearVelocity = v;
body.AngularVelocity = w;
body.SynchronizeTransform();
}
Report(contactSolver.m_constraints);
contactSolver.Free();
}
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, count = m_bodyCount; i < count; ++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);
}
b.InternalPosition.c = c;
b.InternalPosition.a = a;
b.InternalVelocity.v = v;
b.InternalVelocity.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 = b2ContactSolver.Create(ref 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)
{
var b = m_bodies[i];
b2Vec2 c = b.InternalPosition.c;
float a = b.InternalPosition.a;
b2Vec2 v = b.InternalVelocity.v;
float w = b.InternalVelocity.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;
b.InternalPosition.c = c;
b.InternalPosition.a = a;
b.InternalVelocity.v = v;
b.InternalVelocity.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 = body.InternalPosition.c;
body.Sweep.a = body.InternalPosition.a;
body.LinearVelocity = body.InternalVelocity.v;
body.AngularVelocity = body.InternalVelocity.w;
body.SynchronizeTransform();
}
#if PROFILING
profile.solvePosition = timer.GetMilliseconds();
#endif
Report(contactSolver.m_constraints);
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 & b2BodyFlags.e_autoSleepFlag) == 0 ||
b.AngularVelocity * b.AngularVelocity > angTolSqr ||
b2Math.b2Dot(ref b.m_linearVelocity, ref b.m_linearVelocity) > linTolSqr)
{
b.SleepTime = 0.0f;
minSleepTime = 0.0f;
}
else
{
b.SleepTime = 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);
}
}
}
contactSolver.Free();
}
