public void Solve(ref TimeStep step, ref TSVector2 gravity)
{
FP dt = step.dt;
for (int i = 0; i < this.BodyCount; i++)
{
Body body = this.Bodies[i];
TSVector2 c = body._sweep.C;
FP a = body._sweep.A;
TSVector2 tSVector = body._linearVelocity;
FP fP = body._angularVelocity;
body._sweep.C0 = body._sweep.C;
body._sweep.A0 = body._sweep.A;
bool flag = body.BodyType == BodyType.Dynamic;
if (flag)
{
bool ignoreGravity = body.IgnoreGravity;
if (ignoreGravity)
{
tSVector += dt * (body._invMass * body._force);
}
else
{
tSVector += dt * (body.GravityScale * gravity + body._invMass * body._force);
}
fP += dt * body._invI * body._torque;
tSVector *= MathUtils.Clamp(1f - dt * body.LinearDamping, 0f, 1f);
fP *= MathUtils.Clamp(1f - dt * body.AngularDamping, 0f, 1f);
}
this._positions[i].c = c;
this._positions[i].a = a;
this._velocities[i].v = tSVector;
this._velocities[i].w = fP;
}
SolverData solverData = default(SolverData);
solverData.step = step;
solverData.positions = this._positions;
solverData.velocities = this._velocities;
this._contactSolver.Reset(step, this.ContactCount, this._contacts, this._positions, this._velocities, true);
this._contactSolver.InitializeVelocityConstraints();
this._contactSolver.WarmStart();
for (int j = 0; j < this.JointCount; j++)
{
bool enabled = this._joints[j].Enabled;
if (enabled)
{
this._joints[j].InitVelocityConstraints(ref solverData);
}
}
for (int k = 0; k < Settings.VelocityIterations; k++)
{
for (int l = 0; l < this.JointCount; l++)
{
Joint joint = this._joints[l];
bool flag2 = !joint.Enabled;
if (!flag2)
{
joint.SolveVelocityConstraints(ref solverData);
joint.Validate(step.inv_dt);
}
}
this._contactSolver.SolveVelocityConstraints();
}
this._contactSolver.StoreImpulses();
for (int m = 0; m < this.BodyCount; m++)
{
TSVector2 tSVector2 = this._positions[m].c;
FP fP2 = this._positions[m].a;
TSVector2 tSVector3 = this._velocities[m].v;
FP fP3 = this._velocities[m].w;
TSVector2 tSVector4 = dt * tSVector3;
bool flag3 = TSVector2.Dot(tSVector4, tSVector4) > Settings.MaxTranslationSquared;
if (flag3)
{
FP scaleFactor = Settings.MaxTranslation / tSVector4.magnitude;
tSVector3 *= scaleFactor;
}
FP fP4 = dt * fP3;
bool flag4 = fP4 * fP4 > Settings.MaxRotationSquared;
if (flag4)
{
FP y = Settings.MaxRotation / FP.Abs(fP4);
fP3 *= y;
}
tSVector2 += dt * tSVector3;
fP2 += dt * fP3;
this._positions[m].c = tSVector2;
this._positions[m].a = fP2;
this._velocities[m].v = tSVector3;
this._velocities[m].w = fP3;
}
bool flag5 = false;
for (int n = 0; n < Settings.PositionIterations; n++)
{
bool flag6 = this._contactSolver.SolvePositionConstraints();
bool flag7 = true;
for (int num = 0; num < this.JointCount; num++)
{
Joint joint2 = this._joints[num];
bool flag8 = !joint2.Enabled;
if (!flag8)
{
bool flag9 = joint2.SolvePositionConstraints(ref solverData);
flag7 &= flag9;
}
}
bool flag10 = flag6 & flag7;
if (flag10)
{
flag5 = true;
break;
}
}
for (int num2 = 0; num2 < this.BodyCount; num2++)
{
Body body2 = this.Bodies[num2];
body2._sweep.C = this._positions[num2].c;
body2._sweep.A = this._positions[num2].a;
body2._linearVelocity = this._velocities[num2].v;
body2._angularVelocity = this._velocities[num2].w;
body2.SynchronizeTransform();
}
this.Report(this._contactSolver._velocityConstraints);
bool allowSleep = Settings.AllowSleep;
if (allowSleep)
{
FP fP5 = Settings.MaxFP;
for (int num3 = 0; num3 < this.BodyCount; num3++)
{
Body body3 = this.Bodies[num3];
bool flag11 = body3.BodyType == BodyType.Static;
if (!flag11)
{
bool flag12 = !body3.SleepingAllowed || body3._angularVelocity * body3._angularVelocity > Island.AngTolSqr || TSVector2.Dot(body3._linearVelocity, body3._linearVelocity) > Island.LinTolSqr;
if (flag12)
{
body3._sleepTime = 0f;
fP5 = 0f;
}
else
{
body3._sleepTime += dt;
fP5 = TSMath.Min(fP5, body3._sleepTime);
}
}
}
bool flag13 = fP5 >= Settings.TimeToSleep & flag5;
if (flag13)
{
for (int num4 = 0; num4 < this.BodyCount; num4++)
{
Body body4 = this.Bodies[num4];
body4.Awake = false;
}
}
}
}
internal void SolveTOI(ref TimeStep subStep, int toiIndexA, int toiIndexB, bool warmstarting)
{
Debug.Assert(toiIndexA < this.BodyCount);
Debug.Assert(toiIndexB < this.BodyCount);
for (int i = 0; i < this.BodyCount; i++)
{
Body body = this.Bodies[i];
this._positions[i].c = body._sweep.C;
this._positions[i].a = body._sweep.A;
this._velocities[i].v = body._linearVelocity;
this._velocities[i].w = body._angularVelocity;
}
this._contactSolver.Reset(subStep, this.ContactCount, this._contacts, this._positions, this._velocities, warmstarting);
for (int j = 0; j < Settings.TOIPositionIterations; j++)
{
bool flag = this._contactSolver.SolveTOIPositionConstraints(toiIndexA, toiIndexB);
bool flag2 = flag;
if (flag2)
{
break;
}
}
this.Bodies[toiIndexA]._sweep.C0 = this._positions[toiIndexA].c;
this.Bodies[toiIndexA]._sweep.A0 = this._positions[toiIndexA].a;
this.Bodies[toiIndexB]._sweep.C0 = this._positions[toiIndexB].c;
this.Bodies[toiIndexB]._sweep.A0 = this._positions[toiIndexB].a;
this._contactSolver.InitializeVelocityConstraints();
for (int k = 0; k < Settings.TOIVelocityIterations; k++)
{
this._contactSolver.SolveVelocityConstraints();
}
FP dt = subStep.dt;
for (int l = 0; l < this.BodyCount; l++)
{
TSVector2 tSVector = this._positions[l].c;
FP fP = this._positions[l].a;
TSVector2 tSVector2 = this._velocities[l].v;
FP fP2 = this._velocities[l].w;
TSVector2 tSVector3 = dt * tSVector2;
bool flag3 = TSVector2.Dot(tSVector3, tSVector3) > Settings.MaxTranslationSquared;
if (flag3)
{
FP scaleFactor = Settings.MaxTranslation / tSVector3.magnitude;
tSVector2 *= scaleFactor;
}
FP fP3 = dt * fP2;
bool flag4 = fP3 * fP3 > Settings.MaxRotationSquared;
if (flag4)
{
FP y = Settings.MaxRotation / FP.Abs(fP3);
fP2 *= y;
}
tSVector += dt * tSVector2;
fP += dt * fP2;
this._positions[l].c = tSVector;
this._positions[l].a = fP;
this._velocities[l].v = tSVector2;
this._velocities[l].w = fP2;
Body body2 = this.Bodies[l];
body2._sweep.C = tSVector;
body2._sweep.A = fP;
body2._linearVelocity = tSVector2;
body2._angularVelocity = fP2;
body2.SynchronizeTransform();
}
this.Report(this._contactSolver._velocityConstraints);
}
internal void SolveTOI(ref TimeStep subStep, int toiIndexA, int toiIndexB, bool warmstarting)
{
Debug.Assert(toiIndexA < BodyCount);
Debug.Assert(toiIndexB < BodyCount);
// Initialize the body state.
for (int i = 0; i < BodyCount; ++i)
{
Body b = Bodies[i];
_positions[i].c = b._sweep.C;
_positions[i].a = b._sweep.A;
_velocities[i].v = b._linearVelocity;
_velocities[i].w = b._angularVelocity;
}
_contactSolver.Reset(subStep, ContactCount, _contacts, _positions, _velocities, warmstarting);
// Solve position constraints.
for (int i = 0; i < Settings.TOIPositionIterations; ++i)
{
bool contactsOkay = _contactSolver.SolveTOIPositionConstraints(toiIndexA, toiIndexB);
if (contactsOkay)
{
break;
}
}
// Leap of faith to new safe state.
Bodies[toiIndexA]._sweep.C0 = _positions[toiIndexA].c;
Bodies[toiIndexA]._sweep.A0 = _positions[toiIndexA].a;
Bodies[toiIndexB]._sweep.C0 = _positions[toiIndexB].c;
Bodies[toiIndexB]._sweep.A0 = _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 < Settings.TOIVelocityIterations; ++i)
{
_contactSolver.SolveVelocityConstraints();
}
// Don't store the TOI contact forces for warm starting
// because they can be quite large.
FP h = subStep.dt;
// Integrate positions.
for (int i = 0; i < BodyCount; ++i)
{
TSVector2 c = _positions[i].c;
FP a = _positions[i].a;
TSVector2 v = _velocities[i].v;
FP w = _velocities[i].w;
// Check for large velocities
TSVector2 translation = h * v;
if (TSVector2.Dot(translation, translation) > Settings.MaxTranslationSquared)
{
FP ratio = Settings.MaxTranslation / translation.magnitude;
v *= ratio;
}
FP rotation = h * w;
if (rotation * rotation > Settings.MaxRotationSquared)
{
FP ratio = Settings.MaxRotation / FP.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;
// Sync bodies
Body body = Bodies[i];
body._sweep.C = c;
body._sweep.A = a;
body._linearVelocity = v;
body._angularVelocity = w;
body.SynchronizeTransform();
}
Report(_contactSolver._velocityConstraints);
}
public void Solve(ref TimeStep step, ref TSVector2 gravity)
{
FP h = step.dt;
// Integrate velocities and apply damping. Initialize the body state.
for (int i = 0; i < BodyCount; ++i)
{
Body b = Bodies[i];
TSVector2 c = b._sweep.C;
FP a = b._sweep.A;
TSVector2 v = b._linearVelocity;
FP 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);
v *= 1 / (1 + h * b.LinearDamping);
w *= 1 / (1 + h * b.AngularDamping);
}
_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();
}
for (int i = 0; i < JointCount; ++i)
{
if (_joints[i].Enabled)
{
_joints[i].InitVelocityConstraints(ref solverData);
}
}
// Solve velocity constraints.
for (int i = 0; i < Settings.VelocityIterations; ++i)
{
for (int j = 0; j < JointCount; ++j)
{
Joint2D joint = _joints[j];
if (!joint.Enabled)
{
continue;
}
joint.SolveVelocityConstraints(ref solverData);
joint.Validate(step.inv_dt);
}
_contactSolver.SolveVelocityConstraints();
}
// Store impulses for warm starting.
_contactSolver.StoreImpulses();
// Integrate positions
for (int i = 0; i < BodyCount; ++i)
{
TSVector2 c = _positions[i].c;
FP a = _positions[i].a;
TSVector2 v = _velocities[i].v;
FP w = _velocities[i].w;
// Check for large velocities
TSVector2 translation = h * v;
if (TSVector2.Dot(translation, translation) > Settings.MaxTranslationSquared)
{
FP ratio = Settings.MaxTranslation / translation.magnitude;
v *= ratio;
}
FP rotation = h * w;
if (rotation * rotation > Settings.MaxRotationSquared)
{
FP ratio = Settings.MaxRotation / FP.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)
{
Joint2D joint = _joints[j];
if (!joint.Enabled)
{
continue;
}
bool jointOkay = joint.SolvePositionConstraints(ref 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 < 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)
{
FP minSleepTime = Settings.MaxFP;
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 || TSVector2.Dot(b._linearVelocity, b._linearVelocity) > LinTolSqr)
{
b._sleepTime = 0.0f;
minSleepTime = 0.0f;
}
else
{
b._sleepTime += h;
minSleepTime = TrueSync.TSMath.Min(minSleepTime, b._sleepTime);
}
}
if (minSleepTime >= Settings.TimeToSleep && positionSolved)
{
for (int i = 0; i < BodyCount; ++i)
{
Body b = Bodies[i];
b.Awake = false;
}
}
}
}
private void SolveTOI(ref TimeStep step)
{
this.Island.Reset(64, 32, 0, this.ContactManager);
bool stepComplete = this._stepComplete;
if (stepComplete)
{
for (int i = 0; i < this.BodyList.Count; i++)
{
this.BodyList[i]._island = false;
this.BodyList[i]._sweep.Alpha0 = 0f;
}
for (int j = 0; j < this.ContactManager.ContactList.Count; j++)
{
Contact contact = this.ContactManager.ContactList[j];
contact.IslandFlag = false;
contact.TOIFlag = false;
contact._toiCount = 0;
contact._toi = 1f;
}
}
while (true)
{
Contact contact2 = null;
FP fP = 1f;
for (int k = 0; k < this.ContactManager.ContactList.Count; k++)
{
Contact contact3 = this.ContactManager.ContactList[k];
bool flag = !contact3.Enabled;
if (!flag)
{
bool flag2 = contact3._toiCount > 8;
if (!flag2)
{
bool tOIFlag = contact3.TOIFlag;
FP fP2;
if (tOIFlag)
{
fP2 = contact3._toi;
}
else
{
Fixture fixtureA = contact3.FixtureA;
Fixture fixtureB = contact3.FixtureB;
bool flag3 = fixtureA.IsSensor || fixtureB.IsSensor;
if (flag3)
{
goto IL_424;
}
Body body = fixtureA.Body;
Body body2 = fixtureB.Body;
BodyType bodyType = body.BodyType;
BodyType bodyType2 = body2.BodyType;
Debug.Assert(bodyType == BodyType.Dynamic || bodyType2 == BodyType.Dynamic);
bool flag4 = body.Awake && bodyType > BodyType.Static;
bool flag5 = body2.Awake && bodyType2 > BodyType.Static;
bool flag6 = !flag4 && !flag5;
if (flag6)
{
goto IL_424;
}
bool flag7 = (body.IsBullet || bodyType != BodyType.Dynamic) && (fixtureA.IgnoreCCDWith & fixtureB.CollisionCategories) == Category.None && !body.IgnoreCCD;
bool flag8 = (body2.IsBullet || bodyType2 != BodyType.Dynamic) && (fixtureB.IgnoreCCDWith & fixtureA.CollisionCategories) == Category.None && !body2.IgnoreCCD;
bool flag9 = !flag7 && !flag8;
if (flag9)
{
goto IL_424;
}
FP alpha = body._sweep.Alpha0;
bool flag10 = body._sweep.Alpha0 < body2._sweep.Alpha0;
if (flag10)
{
alpha = body2._sweep.Alpha0;
body._sweep.Advance(alpha);
}
else
{
bool flag11 = body2._sweep.Alpha0 < body._sweep.Alpha0;
if (flag11)
{
alpha = body._sweep.Alpha0;
body2._sweep.Advance(alpha);
}
}
Debug.Assert(alpha < 1f);
this._input.ProxyA.Set(fixtureA.Shape, contact3.ChildIndexA);
this._input.ProxyB.Set(fixtureB.Shape, contact3.ChildIndexB);
this._input.SweepA = body._sweep;
this._input.SweepB = body2._sweep;
this._input.TMax = 1f;
TOIOutput tOIOutput;
TimeOfImpact.CalculateTimeOfImpact(out tOIOutput, this._input);
FP t = tOIOutput.T;
bool flag12 = tOIOutput.State == TOIOutputState.Touching;
if (flag12)
{
fP2 = TSMath.Min(alpha + (1f - alpha) * t, 1f);
}
else
{
fP2 = 1f;
}
contact3._toi = fP2;
contact3.TOIFlag = true;
}
bool flag13 = fP2 < fP;
if (flag13)
{
contact2 = contact3;
fP = fP2;
}
}
}
IL_424 :;
}
bool flag14 = contact2 == null || 1f - 10f * Settings.Epsilon < fP;
if (flag14)
{
break;
}
Fixture fixtureA2 = contact2.FixtureA;
Fixture fixtureB2 = contact2.FixtureB;
Body body3 = fixtureA2.Body;
Body body4 = fixtureB2.Body;
Sweep sweep = body3._sweep;
Sweep sweep2 = body4._sweep;
body3.Advance(fP);
body4.Advance(fP);
contact2.Update(this.ContactManager);
contact2.TOIFlag = false;
contact2._toiCount++;
bool flag15 = !contact2.Enabled || !contact2.IsTouching;
if (flag15)
{
contact2.Enabled = false;
body3._sweep = sweep;
body4._sweep = sweep2;
body3.SynchronizeTransform();
body4.SynchronizeTransform();
}
else
{
body3.Awake = true;
body4.Awake = true;
this.Island.Clear();
this.Island.Add(body3);
this.Island.Add(body4);
this.Island.Add(contact2);
body3._island = true;
body4._island = true;
contact2.IslandFlag = true;
Body[] array = new Body[]
{
body3,
body4
};
for (int l = 0; l < 2; l++)
{
Body body5 = array[l];
bool flag16 = body5.BodyType == BodyType.Dynamic;
if (flag16)
{
for (ContactEdge contactEdge = body5.ContactList; contactEdge != null; contactEdge = contactEdge.Next)
{
Contact contact4 = contactEdge.Contact;
bool flag17 = this.Island.BodyCount == this.Island.BodyCapacity;
if (flag17)
{
break;
}
bool flag18 = this.Island.ContactCount == this.Island.ContactCapacity;
if (flag18)
{
break;
}
bool islandFlag = contact4.IslandFlag;
if (!islandFlag)
{
Body other = contactEdge.Other;
bool flag19 = other.BodyType == BodyType.Dynamic && !body5.IsBullet && !other.IsBullet;
if (!flag19)
{
bool flag20 = contact4.FixtureA.IsSensor || contact4.FixtureB.IsSensor;
if (!flag20)
{
Sweep sweep3 = other._sweep;
bool flag21 = !other._island;
if (flag21)
{
other.Advance(fP);
}
contact4.Update(this.ContactManager);
bool flag22 = !contact4.Enabled;
if (flag22)
{
other._sweep = sweep3;
other.SynchronizeTransform();
}
else
{
bool flag23 = !contact4.IsTouching;
if (flag23)
{
other._sweep = sweep3;
other.SynchronizeTransform();
}
else
{
contact4.IslandFlag = true;
this.Island.Add(contact4);
bool island = other._island;
if (!island)
{
other._island = true;
bool flag24 = other.BodyType > BodyType.Static;
if (flag24)
{
other.Awake = true;
}
this.Island.Add(other);
}
}
}
}
}
}
}
}
}
TimeStep timeStep;
timeStep.dt = (1f - fP) * step.dt;
timeStep.inv_dt = 1f / timeStep.dt;
timeStep.dtRatio = 1f;
this.Island.SolveTOI(ref timeStep, body3.IslandIndex, body4.IslandIndex, false);
for (int m = 0; m < this.Island.BodyCount; m++)
{
Body body6 = this.Island.Bodies[m];
body6._island = false;
bool flag25 = body6.BodyType != BodyType.Dynamic;
if (!flag25)
{
body6.SynchronizeFixtures();
for (ContactEdge contactEdge2 = body6.ContactList; contactEdge2 != null; contactEdge2 = contactEdge2.Next)
{
contactEdge2.Contact.TOIFlag = false;
contactEdge2.Contact.IslandFlag = false;
}
}
}
this.ContactManager.FindNewContacts();
}
}
this._stepComplete = true;
}
