| public FinalizeVelocityConstraints ( ) : void | ||
| return | void | |
public void Solve(TimeStep step, Vec2 gravity, bool allowSleep)
{
// Integrate velocities and apply damping.
for (int i = 0; i < _bodyCount; ++i)
{
Body b = _bodies[i];
if (b.IsStatic())
continue;
// Integrate velocities.
b._linearVelocity += step.Dt * (gravity + b._invMass * b._force);
b._angularVelocity += step.Dt * b._invI * b._torque;
// Reset forces.
b._force.Set(0.0f, 0.0f);
b._torque = 0.0f;
// 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._linearVelocity *= Common.Math.Clamp(1.0f - step.Dt * b._linearDamping, 0.0f, 1.0f);
b._angularVelocity *= Common.Math.Clamp(1.0f - step.Dt * b._angularDamping, 0.0f, 1.0f);
// Check for large velocities.
#if TARGET_FLOAT32_IS_FIXED
// Fixed point code written this way to prevent
// overflows, float code is optimized for speed
float vMagnitude = b._linearVelocity.Length();
if(vMagnitude > Settings.MaxLinearVelocity)
{
b._linearVelocity *= Settings.MaxLinearVelocity/vMagnitude;
}
b._angularVelocity = Vector2.Clamp(b._angularVelocity,
-Settings.MaxAngularVelocity, Settings.MaxAngularVelocity);
#else
if (Vec2.Dot(b._linearVelocity, b._linearVelocity) > Settings.MaxLinearVelocitySquared)
{
b._linearVelocity.Normalize();
b._linearVelocity *= Settings.MaxLinearVelocity;
}
if (b._angularVelocity * b._angularVelocity > Settings.MaxAngularVelocitySquared)
{
if (b._angularVelocity < 0.0f)
{
b._angularVelocity = -Settings.MaxAngularVelocity;
}
else
{
b._angularVelocity = Settings.MaxAngularVelocity;
}
}
#endif
}
ContactSolver contactSolver = new ContactSolver(step, _contacts, _contactCount);
// Initialize velocity constraints.
contactSolver.InitVelocityConstraints(step);
for (int i = 0; i < _jointCount; ++i)
{
_joints[i].InitVelocityConstraints(step);
}
// Solve velocity constraints.
for (int i = 0; i < step.VelocityIterations; ++i)
{
for (int j = 0; j < _jointCount; ++j)
{
_joints[j].SolveVelocityConstraints(step);
}
contactSolver.SolveVelocityConstraints();
}
// Post-solve (store impulses for warm starting).
contactSolver.FinalizeVelocityConstraints();
// Integrate positions.
for (int i = 0; i < _bodyCount; ++i)
{
Body b = _bodies[i];
if (b.IsStatic())
continue;
// Store positions for continuous collision.
b._sweep.C0 = b._sweep.C;
b._sweep.A0 = b._sweep.A;
// Integrate
b._sweep.C += step.Dt * b._linearVelocity;
b._sweep.A += step.Dt * b._angularVelocity;
// Compute new transform
b.SynchronizeTransform();
// Note: shapes are synchronized later.
}
// Iterate over constraints.
for (int ii = 0; ii < step.PositionIterations; ++ii)
{
bool contactsOkay = contactSolver.SolvePositionConstraints(Settings.ContactBaumgarte);
bool jointsOkay = true;
for (int i = 0; i < _jointCount; ++i)
{
bool jointOkay = _joints[i].SolvePositionConstraints(/*Settings.ContactBaumgarte*/);
jointsOkay = jointsOkay && jointOkay;
}
if (contactsOkay && jointsOkay)
{
// Exit early if the position errors are small.
break;
}
}
Report(contactSolver._constraints);
if (allowSleep)
{
float minSleepTime = Common.Settings.FLT_MAX;
#if !TARGET_FLOAT32_IS_FIXED
float linTolSqr = Settings.LinearSleepTolerance * Settings.LinearSleepTolerance;
float angTolSqr = Settings.AngularSleepTolerance * Settings.AngularSleepTolerance;
#endif
for (int i = 0; i < _bodyCount; ++i)
{
Body b = _bodies[i];
if (b._invMass == 0.0f)
{
continue;
}
if ((b._flags & Body.BodyFlags.AllowSleep) == 0)
{
b._sleepTime = 0.0f;
minSleepTime = 0.0f;
}
if ((b._flags & Body.BodyFlags.AllowSleep) == 0 ||
#if TARGET_FLOAT32_IS_FIXED
Common.Math.Abs(b._angularVelocity) > Settings.AngularSleepTolerance ||
Common.Math.Abs(b._linearVelocity.X) > Settings.LinearSleepTolerance ||
Common.Math.Abs(b._linearVelocity.Y) > Settings.LinearSleepTolerance)
#else
b._angularVelocity * b._angularVelocity > angTolSqr ||
Vec2.Dot(b._linearVelocity, b._linearVelocity) > linTolSqr)
#endif
{
b._sleepTime = 0.0f;
minSleepTime = 0.0f;
}
else
{
b._sleepTime += step.Dt;
minSleepTime = Common.Math.Min(minSleepTime, b._sleepTime);
}
}
public void Solve(TimeStep step, Vec2 gravity, bool allowSleep)
{
for (int i = 0; i < this._bodyCount; i++)
{
Body body = this._bodies[i];
if (!body.IsStatic())
{
Body expr_27 = body;
//expr_27._linearVelocity += step.Dt * (gravity + body._invMass * body._force);
expr_27._linearVelocity += step.Dt * (body._useGravity ? body._gravity : gravity + body._invMass * body._force); //Steve body gravity
body._angularVelocity += step.Dt * body._invI * body._torque;
body._force.Set(0f, 0f);
body._torque = 0f;
Body expr_9E = body;
expr_9E._linearVelocity *= Box2DX.Common.Math.Clamp(1f - step.Dt * body._linearDamping, 0f, 1f);
body._angularVelocity *= Box2DX.Common.Math.Clamp(1f - step.Dt * body._angularDamping, 0f, 1f);
if (Vec2.Dot(body._linearVelocity, body._linearVelocity) > Settings.MaxLinearVelocitySquared)
{
body._linearVelocity.Normalize();
Body expr_130 = body;
expr_130._linearVelocity *= Settings.MaxLinearVelocity;
}
if (body._angularVelocity * body._angularVelocity > Settings.MaxAngularVelocitySquared)
{
if (body._angularVelocity < 0f)
{
body._angularVelocity = -Settings.MaxAngularVelocity;
}
else
{
body._angularVelocity = Settings.MaxAngularVelocity;
}
}
}
}
ContactSolver contactSolver = new ContactSolver(step, this._contacts, this._contactCount);
contactSolver.InitVelocityConstraints(step);
for (int i = 0; i < this._jointCount; i++)
{
this._joints[i].InitVelocityConstraints(step);
}
for (int i = 0; i < step.VelocityIterations; i++)
{
for (int j = 0; j < this._jointCount; j++)
{
this._joints[j].SolveVelocityConstraints(step);
}
contactSolver.SolveVelocityConstraints();
}
contactSolver.FinalizeVelocityConstraints();
for (int i = 0; i < this._bodyCount; i++)
{
Body body = this._bodies[i];
if (!body.IsStatic())
{
body._sweep.C0 = body._sweep.C;
body._sweep.A0 = body._sweep.A;
Body expr_296_cp_0 = body;
expr_296_cp_0._sweep.C = expr_296_cp_0._sweep.C + step.Dt * body._linearVelocity;
Body expr_2BE_cp_0 = body;
expr_2BE_cp_0._sweep.A = expr_2BE_cp_0._sweep.A + step.Dt * body._angularVelocity;
body.SynchronizeTransform();
}
}
for (int k = 0; k < step.PositionIterations; k++)
{
bool flag = contactSolver.SolvePositionConstraints(Settings.ContactBaumgarte);
bool flag2 = true;
for (int i = 0; i < this._jointCount; i++)
{
bool flag3 = this._joints[i].SolvePositionConstraints(Settings.ContactBaumgarte);
flag2 = (flag2 && flag3);
}
if (flag && flag2)
{
break;
}
}
this.Report(contactSolver._constraints);
if (allowSleep)
{
float num = Settings.FLT_MAX;
float num2 = Settings.LinearSleepTolerance * Settings.LinearSleepTolerance;
float num3 = Settings.AngularSleepTolerance * Settings.AngularSleepTolerance;
for (int i = 0; i < this._bodyCount; i++)
{
Body body = this._bodies[i];
if (body._invMass != 0f)
{
if ((body._flags & Body.BodyFlags.AllowSleep) == (Body.BodyFlags) 0)
{
body._sleepTime = 0f;
num = 0f;
}
if ((body._flags & Body.BodyFlags.AllowSleep) == (Body.BodyFlags) 0 || body._angularVelocity * body._angularVelocity > num3 || Vec2.Dot(body._linearVelocity, body._linearVelocity) > num2)
{
body._sleepTime = 0f;
num = 0f;
}
else
{
body._sleepTime += step.Dt;
num = Box2DX.Common.Math.Min(num, body._sleepTime);
}
}
}
if (num >= Settings.TimeToSleep)
{
for (int i = 0; i < this._bodyCount; i++)
{
Body body = this._bodies[i];
body._flags |= Body.BodyFlags.Sleep;
body._linearVelocity = Vec2.Zero;
body._angularVelocity = 0f;
}
}
}
}
public void Solve(TimeStep step, Vec2 gravity, bool allowSleep)
{
// Integrate velocities and apply damping.
for (int i = 0; i < _bodyCount; ++i)
{
Body b = _bodies[i];
if (b.IsStatic())
{
continue;
}
// Integrate velocities.
b._linearVelocity += step.Dt * (gravity + b._invMass * b._force);
b._angularVelocity += step.Dt * b._invI * b._torque;
// Reset forces.
b._force.Set(0.0f, 0.0f);
b._torque = 0.0f;
// 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._linearVelocity *= Common.Math.Clamp(1.0f - step.Dt * b._linearDamping, 0.0f, 1.0f);
b._angularVelocity *= Common.Math.Clamp(1.0f - step.Dt * b._angularDamping, 0.0f, 1.0f);
// Check for large velocities.
#if TARGET_FLOAT32_IS_FIXED
// Fixed point code written this way to prevent
// overflows, float code is optimized for speed
float vMagnitude = b._linearVelocity.Length();
if (vMagnitude > Settings.MaxLinearVelocity)
{
b._linearVelocity *= Settings.MaxLinearVelocity / vMagnitude;
}
b._angularVelocity = Vector2.Clamp(b._angularVelocity,
-Settings.MaxAngularVelocity, Settings.MaxAngularVelocity);
#else
if (Vec2.Dot(b._linearVelocity, b._linearVelocity) > Settings.MaxLinearVelocitySquared)
{
b._linearVelocity.Normalize();
b._linearVelocity *= Settings.MaxLinearVelocity;
}
if (b._angularVelocity * b._angularVelocity > Settings.MaxAngularVelocitySquared)
{
if (b._angularVelocity < 0.0f)
{
b._angularVelocity = -Settings.MaxAngularVelocity;
}
else
{
b._angularVelocity = Settings.MaxAngularVelocity;
}
}
#endif
}
ContactSolver contactSolver = new ContactSolver(step, _contacts, _contactCount);
// Initialize velocity constraints.
contactSolver.InitVelocityConstraints(step);
for (int i = 0; i < _jointCount; ++i)
{
_joints[i].InitVelocityConstraints(step);
}
// Solve velocity constraints.
for (int i = 0; i < step.VelocityIterations; ++i)
{
for (int j = 0; j < _jointCount; ++j)
{
_joints[j].SolveVelocityConstraints(step);
}
contactSolver.SolveVelocityConstraints();
}
// Post-solve (store impulses for warm starting).
contactSolver.FinalizeVelocityConstraints();
// Integrate positions.
for (int i = 0; i < _bodyCount; ++i)
{
Body b = _bodies[i];
if (b.IsStatic())
{
continue;
}
// Store positions for continuous collision.
b._sweep.C0 = b._sweep.C;
b._sweep.A0 = b._sweep.A;
// Integrate
b._sweep.C += step.Dt * b._linearVelocity;
b._sweep.A += step.Dt * b._angularVelocity;
// Compute new transform
b.SynchronizeTransform();
// Note: shapes are synchronized later.
}
// Iterate over constraints.
for (int ii = 0; ii < step.PositionIterations; ++ii)
{
bool contactsOkay = contactSolver.SolvePositionConstraints(Settings.ContactBaumgarte);
bool jointsOkay = true;
for (int i = 0; i < _jointCount; ++i)
{
bool jointOkay = _joints[i].SolvePositionConstraints(/*Settings.ContactBaumgarte*/);
jointsOkay = jointsOkay && jointOkay;
}
if (contactsOkay && jointsOkay)
{
// Exit early if the position errors are small.
break;
}
}
Report(contactSolver._constraints);
if (allowSleep)
{
float minSleepTime = Common.Settings.FLT_MAX;
#if !TARGET_FLOAT32_IS_FIXED
float linTolSqr = Settings.LinearSleepTolerance * Settings.LinearSleepTolerance;
float angTolSqr = Settings.AngularSleepTolerance * Settings.AngularSleepTolerance;
#endif
for (int i = 0; i < _bodyCount; ++i)
{
Body b = _bodies[i];
if (b._invMass == 0.0f)
{
continue;
}
if ((b._flags & Body.BodyFlags.AllowSleep) == 0)
{
b._sleepTime = 0.0f;
minSleepTime = 0.0f;
}
if ((b._flags & Body.BodyFlags.AllowSleep) == 0 ||
#if TARGET_FLOAT32_IS_FIXED
Common.Math.Abs(b._angularVelocity) > Settings.AngularSleepTolerance ||
Common.Math.Abs(b._linearVelocity.X) > Settings.LinearSleepTolerance ||
Common.Math.Abs(b._linearVelocity.Y) > Settings.LinearSleepTolerance)
#else
b._angularVelocity *b._angularVelocity > angTolSqr ||
Vec2.Dot(b._linearVelocity, b._linearVelocity) > linTolSqr)
#endif
{
b._sleepTime = 0.0f;
minSleepTime = 0.0f;
}
else
{
b._sleepTime += step.Dt;
minSleepTime = Common.Math.Min(minSleepTime, b._sleepTime);
}
}
public void Solve(TimeStep step, Vec2 gravity, bool allowSleep)
{
// Integrate velocities and apply damping.
for (int i = 0; i < BodyCount; ++i)
{
Body b = Bodies[i];
if (b.IsStatic())
continue;
// Integrate velocities.
b._linearVelocity += step.Dt * (gravity + b._invMass * b._force);
b._angularVelocity += step.Dt * b._invI * b._torque;
// Reset forces.
b._force.Set(0.0f, 0.0f);
b._torque = 0.0f;
// 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._linearVelocity *= Common.Math.Clamp(1.0f - step.Dt * b._linearDamping, 0.0f, 1.0f);
b._angularVelocity *= Common.Math.Clamp(1.0f - step.Dt * b._angularDamping, 0.0f, 1.0f);
}
ContactSolver contactSolver = new ContactSolver(step, Contacts, ContactCount);
// Initialize velocity constraints.
contactSolver.InitVelocityConstraints(step);
for (int i = 0; i < JointCount; ++i)
{
Joints[i].InitVelocityConstraints(step);
}
// Solve velocity constraints.
for (int i = 0; i < step.VelocityIterations; ++i)
{
for (int j = 0; j < JointCount; ++j)
{
Joints[j].SolveVelocityConstraints(step);
}
contactSolver.SolveVelocityConstraints();
}
// Post-solve (store impulses for warm starting).
contactSolver.FinalizeVelocityConstraints();
// Integrate positions.
for (int i = 0; i < BodyCount; ++i)
{
Body b = Bodies[i];
if (b.IsStatic())
continue;
// Check for large velocities.
Vec2 translation = step.Dt * b._linearVelocity;
if (Vec2.Dot(translation, translation) > Settings.MaxTranslationSquared)
{
translation.Normalize();
b._linearVelocity = (Settings.MaxTranslation * step.Inv_Dt) * translation;
}
float rotation = step.Dt * Bodies[i]._angularVelocity;
if (rotation * rotation > Settings.MaxRotationSquared)
{
if (rotation < 0.0)
{
b._angularVelocity = -step.Inv_Dt * Settings.MaxRotation;
}
else
{
b._angularVelocity = step.Inv_Dt * Settings.MaxRotation;
}
}
// Store positions for continuous collision.
b._sweep.C0 = b._sweep.C;
b._sweep.A0 = b._sweep.A;
// Integrate
b._sweep.C += step.Dt * b._linearVelocity;
b._sweep.A += step.Dt * b._angularVelocity;
// Compute new transform
b.SynchronizeTransform();
// Note: shapes are synchronized later.
}
// Iterate over constraints.
for (int i = 0; i < step.PositionIterations; ++i)
{
bool contactsOkay = contactSolver.SolvePositionConstraints(Settings.ContactBaumgarte);
bool jointsOkay = true;
for (int j = 0; j < JointCount; ++j)
{
bool jointOkay = Joints[j].SolvePositionConstraints(Settings.ContactBaumgarte);
jointsOkay = jointsOkay && jointOkay;
}
if (contactsOkay && jointsOkay)
{
// Exit early if the position errors are small.
break;
}
}
Report(contactSolver.Constraints);
if (allowSleep)
{
float minSleepTime = Settings.FLT_MAX;
#if !TARGET_FLOAT32_IS_FIXED
float linTolSqr = Settings.LinearSleepTolerance * Settings.LinearSleepTolerance;
float angTolSqr = Settings.AngularSleepTolerance * Settings.AngularSleepTolerance;
#endif
for (int i = 0; i < BodyCount; ++i)
{
Body b = Bodies[i];
if (b._invMass == 0.0f)
{
continue;
}
if ((b._flags & Body.BodyFlags.AllowSleep) == 0)
{
b._sleepTime = 0.0f;
minSleepTime = 0.0f;
}
if ((b._flags & Body.BodyFlags.AllowSleep) == 0 ||
#if TARGET_FLOAT32_IS_FIXED
Common.Math.Abs(b._angularVelocity) > Settings.AngularSleepTolerance ||
Common.Math.Abs(b._linearVelocity.X) > Settings.LinearSleepTolerance ||
Common.Math.Abs(b._linearVelocity.Y) > Settings.LinearSleepTolerance)
#else
b._angularVelocity * b._angularVelocity > angTolSqr ||
Vec2.Dot(b._linearVelocity, b._linearVelocity) > linTolSqr)
#endif
{
b._sleepTime = 0.0f;
minSleepTime = 0.0f;
}
else
{
b._sleepTime += step.Dt;
minSleepTime = Common.Math.Min(minSleepTime, b._sleepTime);
}
}
public void Solve(TimeStep step, Vec2 gravity, bool allowSleep)
{
// Integrate velocities and apply damping.
for (int i = 0; i < BodyCount; ++i)
{
Body b = Bodies[i];
if (b.IsStatic())
{
continue;
}
// Integrate velocities.
b._linearVelocity += step.Dt * (gravity + b._invMass * b._force);
b._angularVelocity += step.Dt * b._invI * b._torque;
// Reset forces.
b._force.Set(0.0f, 0.0f);
b._torque = 0.0f;
// 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._linearVelocity *= Common.Math.Clamp(1.0f - step.Dt * b._linearDamping, 0.0f, 1.0f);
b._angularVelocity *= Common.Math.Clamp(1.0f - step.Dt * b._angularDamping, 0.0f, 1.0f);
}
ContactSolver contactSolver = new ContactSolver(step, Contacts, ContactCount);
// Initialize velocity constraints.
contactSolver.InitVelocityConstraints(step);
for (int i = 0; i < JointCount; ++i)
{
Joints[i].InitVelocityConstraints(step);
}
// Solve velocity constraints.
for (int i = 0; i < step.VelocityIterations; ++i)
{
for (int j = 0; j < JointCount; ++j)
{
Joints[j].SolveVelocityConstraints(step);
}
contactSolver.SolveVelocityConstraints();
}
// Post-solve (store impulses for warm starting).
contactSolver.FinalizeVelocityConstraints();
// Integrate positions.
for (int i = 0; i < BodyCount; ++i)
{
Body b = Bodies[i];
if (b.IsStatic())
{
continue;
}
// Check for large velocities.
Vec2 translation = step.Dt * b._linearVelocity;
if (Vec2.Dot(translation, translation) > Settings.MaxTranslationSquared)
{
translation.Normalize();
b._linearVelocity = (Settings.MaxTranslation * step.Inv_Dt) * translation;
}
float rotation = step.Dt * Bodies[i]._angularVelocity;
if (rotation * rotation > Settings.MaxRotationSquared)
{
if (rotation < 0.0)
{
b._angularVelocity = -step.Inv_Dt * Settings.MaxRotation;
}
else
{
b._angularVelocity = step.Inv_Dt * Settings.MaxRotation;
}
}
// Store positions for continuous collision.
b._sweep.C0 = b._sweep.C;
b._sweep.A0 = b._sweep.A;
// Integrate
b._sweep.C += step.Dt * b._linearVelocity;
b._sweep.A += step.Dt * b._angularVelocity;
// Compute new transform
b.SynchronizeTransform();
// Note: shapes are synchronized later.
}
// Iterate over constraints.
for (int i = 0; i < step.PositionIterations; ++i)
{
bool contactsOkay = contactSolver.SolvePositionConstraints(Settings.ContactBaumgarte);
bool jointsOkay = true;
for (int j = 0; j < JointCount; ++j)
{
bool jointOkay = Joints[j].SolvePositionConstraints(Settings.ContactBaumgarte);
jointsOkay = jointsOkay && jointOkay;
}
if (contactsOkay && jointsOkay)
{
// Exit early if the position errors are small.
break;
}
}
Report(contactSolver.Constraints);
if (allowSleep)
{
float minSleepTime = Settings.FLT_MAX;
#if !TARGET_FLOAT32_IS_FIXED
float linTolSqr = Settings.LinearSleepTolerance * Settings.LinearSleepTolerance;
float angTolSqr = Settings.AngularSleepTolerance * Settings.AngularSleepTolerance;
#endif
for (int i = 0; i < BodyCount; ++i)
{
Body b = Bodies[i];
if (b._invMass == 0.0f)
{
continue;
}
if ((b._flags & Body.BodyFlags.AllowSleep) == 0)
{
b._sleepTime = 0.0f;
minSleepTime = 0.0f;
}
if ((b._flags & Body.BodyFlags.AllowSleep) == 0 ||
#if TARGET_FLOAT32_IS_FIXED
Common.Math.Abs(b._angularVelocity) > Settings.AngularSleepTolerance ||
Common.Math.Abs(b._linearVelocity.X) > Settings.LinearSleepTolerance ||
Common.Math.Abs(b._linearVelocity.Y) > Settings.LinearSleepTolerance)
#else
b._angularVelocity *b._angularVelocity > angTolSqr ||
Vec2.Dot(b._linearVelocity, b._linearVelocity) > linTolSqr)
#endif
{
b._sleepTime = 0.0f;
minSleepTime = 0.0f;
}
else
{
b._sleepTime += step.Dt;
minSleepTime = Common.Math.Min(minSleepTime, b._sleepTime);
}
}
public void Solve(TimeStep step, Vec2 gravity, bool allowSleep)
{
for (int i = 0; i < this._bodyCount; i++)
{
Body body = this._bodies[i];
if (!body.IsStatic())
{
Body expr_27 = body;
//expr_27._linearVelocity += step.Dt * (gravity + body._invMass * body._force);
expr_27._linearVelocity += step.Dt * (body._useGravity ? body._gravity : gravity + body._invMass * body._force); //Steve body gravity
body._angularVelocity += step.Dt * body._invI * body._torque;
body._force.Set(0f, 0f);
body._torque = 0f;
Body expr_9E = body;
expr_9E._linearVelocity *= Box2DX.Common.Math.Clamp(1f - step.Dt * body._linearDamping, 0f, 1f);
body._angularVelocity *= Box2DX.Common.Math.Clamp(1f - step.Dt * body._angularDamping, 0f, 1f);
if (Vec2.Dot(body._linearVelocity, body._linearVelocity) > Settings.MaxLinearVelocitySquared)
{
body._linearVelocity.Normalize();
Body expr_130 = body;
expr_130._linearVelocity *= Settings.MaxLinearVelocity;
}
if (body._angularVelocity * body._angularVelocity > Settings.MaxAngularVelocitySquared)
{
if (body._angularVelocity < 0f)
{
body._angularVelocity = -Settings.MaxAngularVelocity;
}
else
{
body._angularVelocity = Settings.MaxAngularVelocity;
}
}
}
}
ContactSolver contactSolver = new ContactSolver(step, this._contacts, this._contactCount);
contactSolver.InitVelocityConstraints(step);
for (int i = 0; i < this._jointCount; i++)
{
this._joints[i].InitVelocityConstraints(step);
}
for (int i = 0; i < step.VelocityIterations; i++)
{
for (int j = 0; j < this._jointCount; j++)
{
this._joints[j].SolveVelocityConstraints(step);
}
contactSolver.SolveVelocityConstraints();
}
contactSolver.FinalizeVelocityConstraints();
for (int i = 0; i < this._bodyCount; i++)
{
Body body = this._bodies[i];
if (!body.IsStatic())
{
body._sweep.C0 = body._sweep.C;
body._sweep.A0 = body._sweep.A;
Body expr_296_cp_0 = body;
expr_296_cp_0._sweep.C = expr_296_cp_0._sweep.C + step.Dt * body._linearVelocity;
Body expr_2BE_cp_0 = body;
expr_2BE_cp_0._sweep.A = expr_2BE_cp_0._sweep.A + step.Dt * body._angularVelocity;
body.SynchronizeTransform();
}
}
for (int k = 0; k < step.PositionIterations; k++)
{
bool flag = contactSolver.SolvePositionConstraints(Settings.ContactBaumgarte);
bool flag2 = true;
for (int i = 0; i < this._jointCount; i++)
{
bool flag3 = this._joints[i].SolvePositionConstraints(Settings.ContactBaumgarte);
flag2 = (flag2 && flag3);
}
if (flag && flag2)
{
break;
}
}
this.Report(contactSolver._constraints);
if (allowSleep)
{
float num = Settings.FLT_MAX;
float num2 = Settings.LinearSleepTolerance * Settings.LinearSleepTolerance;
float num3 = Settings.AngularSleepTolerance * Settings.AngularSleepTolerance;
for (int i = 0; i < this._bodyCount; i++)
{
Body body = this._bodies[i];
if (body._invMass != 0f)
{
if ((body._flags & Body.BodyFlags.AllowSleep) == (Body.BodyFlags)0)
{
body._sleepTime = 0f;
num = 0f;
}
if ((body._flags & Body.BodyFlags.AllowSleep) == (Body.BodyFlags)0 || body._angularVelocity * body._angularVelocity > num3 || Vec2.Dot(body._linearVelocity, body._linearVelocity) > num2)
{
body._sleepTime = 0f;
num = 0f;
}
else
{
body._sleepTime += step.Dt;
num = Box2DX.Common.Math.Min(num, body._sleepTime);
}
}
}
if (num >= Settings.TimeToSleep)
{
for (int i = 0; i < this._bodyCount; i++)
{
Body body = this._bodies[i];
body._flags |= Body.BodyFlags.Sleep;
body._linearVelocity = Vec2.Zero;
body._angularVelocity = 0f;
}
}
}
}