private void UndoTransformOrientation(SolverData data)
{
int indexB = BodyB.IslandIndex;
Position positionB = data.positions[indexB];
Velocity velocityB = data.velocities[indexB];
var t = Portal.Enter(PortalEnter, new Transform2(positionB.c, 1, positionB.a));
var v = Portal.EnterVelocity(PortalEnter, 0.5f, new Transform2(velocityB.v, 1, velocityB.w));
data.positions[indexB].c = (Vector2)t.Position;
data.positions[indexB].a = t.Rotation;
data.velocities[indexB].v = (Vector2)v.Position;
data.velocities[indexB].w = v.Rotation;
}
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 override void SolveVelocityConstraints(ref SolverData data)
{
TransformOrientation(data);
Vector2 vA = data.velocities[_indexA].v;
float wA = data.velocities[_indexA].w;
Vector2 vB = data.velocities[_indexB].v;
float wB = data.velocities[_indexB].w;
float mA = _invMassA, mB = _invMassB;
float iA = _invIA, iB = _invIB;
if (FrequencyHz > 0.0f)
{
float Cdot2 = wB - wA;
float impulse2 = -_mass.ez.Z * (Cdot2 + _bias + _gamma * _impulse.Z);
_impulse.Z += impulse2;
wA -= iA * impulse2;
wB += iB * impulse2;
Vector2 Cdot1 = vB + MathUtils.Cross(wB, _rB) - vA - MathUtils.Cross(wA, _rA);
Vector2 impulse1 = -MathUtils.Mul22(_mass, Cdot1);
_impulse.X += impulse1.X;
_impulse.Y += impulse1.Y;
Vector2 P = impulse1;
vA -= mA * P;
wA -= iA * MathUtils.Cross(_rA, P);
vB += mB * P;
wB += iB * MathUtils.Cross(_rB, P);
}
else
{
Vector2 Cdot1 = vB + MathUtils.Cross(wB, _rB) - vA - MathUtils.Cross(wA, _rA);
float Cdot2 = wB - wA;
Vector3 Cdot = new Vector3(Cdot1.X, Cdot1.Y, Cdot2);
Vector3 impulse = -MathUtils.Mul(_mass, Cdot);
_impulse += impulse;
Vector2 P = new Vector2(impulse.X, impulse.Y);
vA -= mA * P;
wA -= iA * (MathUtils.Cross(_rA, P) + impulse.Z);
vB += mB * P;
wB += iB * (MathUtils.Cross(_rB, P) + impulse.Z);
}
data.velocities[_indexA].v = vA;
data.velocities[_indexA].w = wA;
data.velocities[_indexB].v = vB;
data.velocities[_indexB].w = wB;
UndoTransformOrientation(data);
}
public override bool SolvePositionConstraints(ref SolverData data)
{
TransformOrientation(data);
Vector2 cA = data.positions[_indexA].c;
float aA = data.positions[_indexA].a;
Vector2 cB = data.positions[_indexB].c;
float aB = data.positions[_indexB].a;
Rot qA = new Rot(aA), qB = new Rot(aB);
float mA = _invMassA, mB = _invMassB;
float iA = _invIA, iB = _invIB;
Vector2 rA = MathUtils.Mul(qA, LocalAnchorA - _localCenterA);
Vector2 rB = MathUtils.Mul(qB, LocalAnchorB - _localCenterB);
float positionError, angularError;
Mat33 K = new Mat33();
K.ex.X = mA + mB + rA.Y * rA.Y * iA + rB.Y * rB.Y * iB;
K.ey.X = -rA.Y * rA.X * iA - rB.Y * rB.X * iB;
K.ez.X = -rA.Y * iA - rB.Y * iB;
K.ex.Y = K.ey.X;
K.ey.Y = mA + mB + rA.X * rA.X * iA + rB.X * rB.X * iB;
K.ez.Y = rA.X * iA + rB.X * iB;
K.ex.Z = K.ez.X;
K.ey.Z = K.ez.Y;
K.ez.Z = iA + iB;
if (FrequencyHz > 0.0f)
{
Vector2 C1 = cB + rB - cA - rA;
positionError = C1.Length();
angularError = 0.0f;
Vector2 P = -K.Solve22(C1);
cA -= mA * P;
aA -= iA * MathUtils.Cross(rA, P);
cB += mB * P;
aB += iB * MathUtils.Cross(rB, P);
}
else
{
Vector2 C1 = cB + rB - cA - rA;
float C2 = aB - aA;
positionError = C1.Length();
angularError = Math.Abs(C2);
Vector3 C = new Vector3(C1.X, C1.Y, C2);
Vector3 impulse = -K.Solve33(C);
Vector2 P = new Vector2(impulse.X, impulse.Y);
cA -= mA * P;
aA -= iA * (MathUtils.Cross(rA, P) + impulse.Z);
cB += mB * P;
aB += iB * (MathUtils.Cross(rB, P) + impulse.Z);
}
data.positions[_indexA].c = cA;
data.positions[_indexA].a = aA;
data.positions[_indexB].c = cB;
data.positions[_indexB].a = aB;
UndoTransformOrientation(data);
return positionError <= Settings.LinearSlop && angularError <= Settings.AngularSlop;
}
public override void InitVelocityConstraints(ref SolverData data)
{
TransformOrientation(data);
_indexA = BodyA.IslandIndex;
_indexB = BodyB.IslandIndex;
_localCenterA = BodyA._sweep.LocalCenter;
_localCenterB = BodyB._sweep.LocalCenter;
_invMassA = BodyA._invMass;
_invMassB = BodyB._invMass;
_invIA = BodyA._invI;
_invIB = BodyB._invI;
float aA = data.positions[_indexA].a;
Vector2 vA = data.velocities[_indexA].v;
float wA = data.velocities[_indexA].w;
float aB = data.positions[_indexB].a;
Vector2 vB = data.velocities[_indexB].v;
float wB = data.velocities[_indexB].w;
Rot qA = new Rot(aA), qB = new Rot(aB);
_rA = MathUtils.Mul(qA, LocalAnchorA - _localCenterA);
_rB = MathUtils.Mul(qB, LocalAnchorB - _localCenterB);
// J = [-I -r1_skew I r2_skew]
// [ 0 -1 0 1]
// r_skew = [-ry; rx]
// Matlab
// K = [ mA+r1y^2*iA+mB+r2y^2*iB, -r1y*iA*r1x-r2y*iB*r2x, -r1y*iA-r2y*iB]
// [ -r1y*iA*r1x-r2y*iB*r2x, mA+r1x^2*iA+mB+r2x^2*iB, r1x*iA+r2x*iB]
// [ -r1y*iA-r2y*iB, r1x*iA+r2x*iB, iA+iB]
float mA = _invMassA, mB = _invMassB;
float iA = _invIA, iB = _invIB;
Mat33 K = new Mat33();
K.ex.X = mA + mB + _rA.Y * _rA.Y * iA + _rB.Y * _rB.Y * iB;
K.ey.X = -_rA.Y * _rA.X * iA - _rB.Y * _rB.X * iB;
K.ez.X = -_rA.Y * iA - _rB.Y * iB;
K.ex.Y = K.ey.X;
K.ey.Y = mA + mB + _rA.X * _rA.X * iA + _rB.X * _rB.X * iB;
K.ez.Y = _rA.X * iA + _rB.X * iB;
K.ex.Z = K.ez.X;
K.ey.Z = K.ez.Y;
K.ez.Z = iA + iB;
if (FrequencyHz > 0.0f)
{
K.GetInverse22(ref _mass);
float invM = iA + iB;
float m = invM > 0.0f ? 1.0f / invM : 0.0f;
float C = aB - aA;
// Frequency
float omega = 2.0f * Settings.Pi * FrequencyHz;
// Damping coefficient
float d = 2.0f * m * DampingRatio * omega;
// Spring stiffness
float k = m * omega * omega;
// magic formulas
float h = data.step.dt;
_gamma = h * (d + h * k);
_gamma = _gamma != 0.0f ? 1.0f / _gamma : 0.0f;
_bias = C * h * k * _gamma;
invM += _gamma;
_mass.ez.Z = invM != 0.0f ? 1.0f / invM : 0.0f;
}
else
{
K.GetSymInverse33(ref _mass);
_gamma = 0.0f;
_bias = 0.0f;
}
if (Settings.EnableWarmstarting)
{
// Scale impulses to support a variable time step.
_impulse *= data.step.dtRatio;
Vector2 P = new Vector2(_impulse.X, _impulse.Y);
vA -= mA * P;
wA -= iA * (MathUtils.Cross(_rA, P) + _impulse.Z);
vB += mB * P;
wB += iB * (MathUtils.Cross(_rB, P) + _impulse.Z);
}
else
{
#pragma warning disable CS0162 // Unreachable code detected
_impulse = Vector3.Zero;
#pragma warning restore CS0162 // Unreachable code detected
}
data.velocities[_indexA].v = vA;
data.velocities[_indexA].w = wA;
data.velocities[_indexB].v = vB;
data.velocities[_indexB].w = wB;
UndoTransformOrientation(data);
}
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;
}
}
}
}
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;
}
}
}
}