public bool SolvePositionConstraints(float baumgarte)
{
float minSeparation = 0.0f;
for (int i = 0; i < _constraintCount; ++i)
{
ContactConstraint c = _constraints[i];
Body b1 = c.Body1;
Body b2 = c.Body2;
float invMass1 = b1._mass * b1._invMass;
float invI1 = b1._mass * b1._invI;
float invMass2 = b2._mass * b2._invMass;
float invI2 = b2._mass * b2._invI;
Vec2 normal = c.Normal;
// Solver normal constraints
for (int j = 0; j < c.PointCount; ++j)
{
ContactConstraintPoint ccp = c.Points[j];
Vec2 r1 = Common.Math.Mul(b1.GetXForm().R, ccp.LocalAnchor1 - b1.GetLocalCenter());
Vec2 r2 = Common.Math.Mul(b2.GetXForm().R, ccp.LocalAnchor2 - b2.GetLocalCenter());
Vec2 p1 = b1._sweep.C + r1;
Vec2 p2 = b2._sweep.C + r2;
Vec2 dp = p2 - p1;
// Approximate the current separation.
float separation = Vec2.Dot(dp, normal) + ccp.Separation;
// Track max constraint error.
minSeparation = Common.Math.Min(minSeparation, separation);
// Prevent large corrections and allow slop.
float C = baumgarte * Common.Math.Clamp(separation + Settings.LinearSlop, -Settings.MaxLinearCorrection, 0.0f);
// Compute normal impulse
float impulse = -ccp.EqualizedMass * C;
Vec2 P = impulse * normal;
b1._sweep.C -= invMass1 * P;
b1._sweep.A -= invI1 * Vec2.Cross(r1, P);
b1.SynchronizeTransform();
b2._sweep.C += invMass2 * P;
b2._sweep.A += invI2 * Vec2.Cross(r2, P);
b2.SynchronizeTransform();
}
}
// We can't expect minSpeparation >= -Settings.LinearSlop because we don't
// push the separation above -Settings.LinearSlop.
return(minSeparation >= -1.5f * Settings.LinearSlop);
}
// Sequential solver.
public bool SolvePositionConstraints(float baumgarte)
{
float minSeparation = 0.0f;
for (int i = 0; i < ConstraintCount; ++i)
{
ContactConstraint c = Constraints[i];
Body bodyA = c.BodyA;
Body bodyB = c.BodyB;
float invMassA = bodyA._mass * bodyA._invMass;
float invIA = bodyA._mass * bodyA._invI;
float invMassB = bodyB._mass * bodyB._invMass;
float invIB = bodyB._mass * bodyB._invI;
PositionSolverManifold psm = new PositionSolverManifold();
psm.Initialize(c);
Vec2 normal = psm.Normal;
// Solve normal constraints
for (int j = 0; j < c.PointCount; ++j)
{
ContactConstraintPoint ccp = c.Points[j];
Vec2 point = psm.Points[j];
float separation = psm.Separations[j];
Vec2 rA = point - bodyA._sweep.C;
Vec2 rB = point - bodyB._sweep.C;
// Track max constraint error.
minSeparation = Common.Math.Min(minSeparation, separation);
// Prevent large corrections and allow slop.
float C = Common.Math.Clamp(baumgarte * (separation + Settings.LinearSlop), -Settings.MaxLinearCorrection, 0.0f);
// Compute normal impulse
float impulse = -ccp.EqualizedMass * C;
Vec2 P = impulse * normal;
bodyA._sweep.C -= invMassA * P;
bodyA._sweep.A -= invIA * Vec2.Cross(rA, P);
bodyA.SynchronizeTransform();
bodyB._sweep.C += invMassB * P;
bodyB._sweep.A += invIB * Vec2.Cross(rB, P);
bodyB.SynchronizeTransform();
}
}
// We can't expect minSpeparation >= -b2_linearSlop because we don't
// push the separation above -b2_linearSlop.
return(minSeparation >= -1.5f * Settings.LinearSlop);
}
internal override bool SolvePositionConstraints(float baumgarte)
{
//B2_NOT_USED(baumgarte);
float linearError = 0.0f;
Body b1 = _bodyA;
Body b2 = _bodyB;
float coordinate1, coordinate2;
if (_revolute1 != null)
{
coordinate1 = _revolute1.JointAngle;
}
else
{
coordinate1 = _prismatic1.JointTranslation;
}
if (_revolute2 != null)
{
coordinate2 = _revolute2.JointAngle;
}
else
{
coordinate2 = _prismatic2.JointTranslation;
}
float C = _constant - (coordinate1 + _ratio * coordinate2);
float impulse = _mass * (-C);
b1._sweep.C += b1._invMass * impulse * _J.Linear1;
b1._sweep.A += b1._invI * impulse * _J.Angular1;
b2._sweep.C += b2._invMass * impulse * _J.Linear2;
b2._sweep.A += b2._invI * impulse * _J.Angular2;
b1.SynchronizeTransform();
b2.SynchronizeTransform();
//TODO_ERIN not implemented
return(linearError < Settings.LinearSlop);
}
internal override bool SolvePositionConstraints(float baumgarte)
{
float num = 0f;
Body body = this._body1;
Body body2 = this._body2;
float num2;
if (this._revolute1 != null)
{
num2 = this._revolute1.JointAngle;
}
else
{
num2 = this._prismatic1.JointTranslation;
}
float num3;
if (this._revolute2 != null)
{
num3 = this._revolute2.JointAngle;
}
else
{
num3 = this._prismatic2.JointTranslation;
}
float num4 = this._constant - (num2 + this._ratio * num3);
float num5 = this._mass * -num4;
Body expr_97_cp_0 = body;
expr_97_cp_0._sweep.C = expr_97_cp_0._sweep.C + body._invMass * num5 * this._J.Linear1;
Body expr_C6_cp_0 = body;
expr_C6_cp_0._sweep.A = expr_C6_cp_0._sweep.A + body._invI * num5 * this._J.Angular1;
Body expr_ED_cp_0 = body2;
expr_ED_cp_0._sweep.C = expr_ED_cp_0._sweep.C + body2._invMass * num5 * this._J.Linear2;
Body expr_11C_cp_0 = body2;
expr_11C_cp_0._sweep.A = expr_11C_cp_0._sweep.A + body2._invI * num5 * this._J.Angular2;
body.SynchronizeTransform();
body2.SynchronizeTransform();
return(num < Settings.LinearSlop);
}
public bool SolvePositionConstraints(float baumgarte)
{
float num = 0f;
for (int i = 0; i < this._constraintCount; i++)
{
ContactConstraint contactConstraint = this._constraints[i];
Body body = contactConstraint.Body1;
Body body2 = contactConstraint.Body2;
float a = body._mass * body._invMass;
float num2 = body._mass * body._invI;
float a2 = body2._mass * body2._invMass;
float num3 = body2._mass * body2._invI;
Vec2 normal = contactConstraint.Normal;
for (int j = 0; j < contactConstraint.PointCount; j++)
{
ContactConstraintPoint contactConstraintPoint = contactConstraint.Points[j];
Vec2 vec = Box2DX.Common.Math.Mul(body.GetXForm().R, contactConstraintPoint.LocalAnchor1 - body.GetLocalCenter());
Vec2 vec2 = Box2DX.Common.Math.Mul(body2.GetXForm().R, contactConstraintPoint.LocalAnchor2 - body2.GetLocalCenter());
Vec2 v = body._sweep.C + vec;
Vec2 v2 = body2._sweep.C + vec2;
Vec2 a3 = v2 - v;
float num4 = Vec2.Dot(a3, normal) + contactConstraintPoint.Separation;
num = Box2DX.Common.Math.Min(num, num4);
float num5 = baumgarte * Box2DX.Common.Math.Clamp(num4 + Settings.LinearSlop, -Settings.MaxLinearCorrection, 0f);
float a4 = -contactConstraintPoint.EqualizedMass * num5;
Vec2 vec3 = a4 * normal;
Body expr_157_cp_0 = body;
expr_157_cp_0._sweep.C = expr_157_cp_0._sweep.C - a * vec3;
Body expr_176_cp_0 = body;
expr_176_cp_0._sweep.A = expr_176_cp_0._sweep.A - num2 * Vec2.Cross(vec, vec3);
body.SynchronizeTransform();
Body expr_19C_cp_0 = body2;
expr_19C_cp_0._sweep.C = expr_19C_cp_0._sweep.C + a2 * vec3;
Body expr_1BC_cp_0 = body2;
expr_1BC_cp_0._sweep.A = expr_1BC_cp_0._sweep.A + num3 * Vec2.Cross(vec2, vec3);
body2.SynchronizeTransform();
}
}
return(num >= -1.5f * Settings.LinearSlop);
}
internal override bool SolvePositionConstraints()
{
float linearError = 0.0f;
Body b1 = _body1;
Body b2 = _body2;
float coordinate1, coordinate2;
if (_revolute1 != null)
{
coordinate1 = _revolute1.JointAngle;
}
else
{
coordinate1 = _prismatic1.JointTranslation;
}
if (_revolute2 != null)
{
coordinate2 = _revolute2.JointAngle;
}
else
{
coordinate2 = _prismatic2.JointTranslation;
}
float C = _constant - (coordinate1 + _ratio * coordinate2);
float impulse = -_mass * C;
b1._sweep.C += b1._invMass * impulse * _J.Linear1;
b1._sweep.A += b1._invI * impulse * _J.Angular1;
b2._sweep.C += b2._invMass * impulse * _J.Linear2;
b2._sweep.A += b2._invI * impulse * _J.Angular2;
b1.SynchronizeTransform();
b2.SynchronizeTransform();
return(linearError < Settings.LinearSlop);
}
internal override bool SolvePositionConstraints(float baumgarte)
{
if (_frequencyHz > 0.0f)
{
//There is no possition correction for soft distace constraint.
return(true);
}
Body b1 = _body1;
Body b2 = _body2;
Vector2 r1 = b1.GetTransform().TransformDirection(_localAnchor1 - b1.GetLocalCenter());
Vector2 r2 = b2.GetTransform().TransformDirection(_localAnchor2 - b2.GetLocalCenter());
Vector2 d = b2._sweep.C + r2 - b1._sweep.C - r1;
float length = d.magnitude;
d.Normalize();
float C = length - _length;
C = Mathf.Clamp(C, -Settings.MaxLinearCorrection, Settings.MaxLinearCorrection);
float impulse = -_mass * C;
_u = d;
Vector2 P = impulse * _u;
b1._sweep.C -= b1._invMass * P;
b1._sweep.A -= b1._invI * r1.Cross(P);
b2._sweep.C += b2._invMass * P;
b2._sweep.A += b2._invI * r2.Cross(P);
b1.SynchronizeTransform();
b2.SynchronizeTransform();
return(System.Math.Abs(C) < Settings.LinearSlop);
}
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);
}
}
internal override bool SolvePositionConstraints(float baumgarte)
{
Body b1 = _body1;
Body b2 = _body2;
Vector2 c1 = b1._sweep.C;
float a1 = b1._sweep.A;
Vector2 c2 = b2._sweep.C;
float a2 = b2._sweep.A;
// Solve linear limit constraint.
float linearError = 0.0f, angularError = 0.0f;
bool active = false;
float C2 = 0.0f;
Mat22 R1 = new Mat22(a1), R2 = new Mat22(a2);
Vector2 r1 = R1.Multiply(_localAnchor1 - _localCenter1);
Vector2 r2 = R2.Multiply(_localAnchor2 - _localCenter2);
Vector2 d = c2 + r2 - c1 - r1;
if (_enableLimit)
{
_axis = R1.Multiply(_localXAxis1);
_a1 = (d + r1).Cross(_axis);
_a2 = r2.Cross(_axis);
float translation = Vector2.Dot(_axis, d);
if (System.Math.Abs(_upperTranslation - _lowerTranslation) < 2.0f * Settings.LinearSlop)
{
// Prevent large angular corrections
C2 = Box2DX.Common.Math.Clamp(translation, -Settings.MaxLinearCorrection, Settings.MaxLinearCorrection);
linearError = Box2DX.Common.Math.Abs(translation);
active = true;
}
else if (translation <= _lowerTranslation)
{
// Prevent large linear corrections and allow some slop.
C2 = Box2DX.Common.Math.Clamp(translation - _lowerTranslation + Settings.LinearSlop, -Settings.MaxLinearCorrection, 0.0f);
linearError = _lowerTranslation - translation;
active = true;
}
else if (translation >= _upperTranslation)
{
// Prevent large linear corrections and allow some slop.
C2 = Box2DX.Common.Math.Clamp(translation - _upperTranslation - Settings.LinearSlop, 0.0f, Settings.MaxLinearCorrection);
linearError = translation - _upperTranslation;
active = true;
}
}
_perp = R1.Multiply(_localYAxis1);
_s1 = (d + r1).Cross(_perp);
_s2 = r2.Cross(_perp);
Vector2 impulse;
float C1;
C1 = Vector2.Dot(_perp, d);
linearError = Box2DX.Common.Math.Max(linearError, Box2DX.Common.Math.Abs(C1));
angularError = 0.0f;
if (active)
{
float m1 = _invMass1, m2 = _invMass2;
float i1 = _invI1, i2 = _invI2;
float k11 = m1 + m2 + i1 * _s1 * _s1 + i2 * _s2 * _s2;
float k12 = i1 * _s1 * _a1 + i2 * _s2 * _a2;
float k22 = m1 + m2 + i1 * _a1 * _a1 + i2 * _a2 * _a2;
_K.Col1 = new Vector2(k11, k12);
_K.Col2 = new Vector2(k12, k22);
Vector2 C = new Vector2();
C.X = C1;
C.Y = C2;
impulse = _K.Solve(-C);
}
else
{
float m1 = _invMass1, m2 = _invMass2;
float i1 = _invI1, i2 = _invI2;
float k11 = m1 + m2 + i1 * _s1 * _s1 + i2 * _s2 * _s2;
float impulse1 = (-C1) / k11;
impulse.X = impulse1;
impulse.Y = 0.0f;
}
Vector2 P = impulse.X * _perp + impulse.Y * _axis;
float L1 = impulse.X * _s1 + impulse.Y * _a1;
float L2 = impulse.X * _s2 + impulse.Y * _a2;
c1 -= _invMass1 * P;
a1 -= _invI1 * L1;
c2 += _invMass2 * P;
a2 += _invI2 * L2;
// TODO_ERIN remove need for this.
b1._sweep.C = c1;
b1._sweep.A = a1;
b2._sweep.C = c2;
b2._sweep.A = a2;
b1.SynchronizeTransform();
b2.SynchronizeTransform();
return(linearError <= Settings.LinearSlop && angularError <= Settings.AngularSlop);
}
internal override bool SolvePositionConstraints()
{
Body b1 = _body1;
Body b2 = _body2;
float positionError = 0.0f;
// Solve point-to-point position error.
Vec2 r1 = Box2DXMath.Mul(b1.GetXForm().R, _localAnchor1 - b1.GetLocalCenter());
Vec2 r2 = Box2DXMath.Mul(b2.GetXForm().R, _localAnchor2 - b2.GetLocalCenter());
Vec2 p1 = b1._sweep.C + r1;
Vec2 p2 = b2._sweep.C + r2;
Vec2 ptpC = p2 - p1;
positionError = ptpC.Length();
// Prevent overly large corrections.
//b2Vec2 dpMax(b2_maxLinearCorrection, b2_maxLinearCorrection);
//ptpC = b2Clamp(ptpC, -dpMax, dpMax);
float invMass1 = b1._invMass, invMass2 = b2._invMass;
float invI1 = b1._invI, invI2 = b2._invI;
Mat22 K1 = new Mat22();
K1.Col1.X = invMass1 + invMass2; K1.Col2.X = 0.0f;
K1.Col1.Y = 0.0f; K1.Col2.Y = invMass1 + invMass2;
Mat22 K2 = new Mat22();
K2.Col1.X = invI1 * r1.Y * r1.Y; K2.Col2.X = -invI1 * r1.X * r1.Y;
K2.Col1.Y = -invI1 * r1.X * r1.Y; K2.Col2.Y = invI1 * r1.X * r1.X;
Mat22 K3 = new Mat22();
K3.Col1.X = invI2 * r2.Y * r2.Y; K3.Col2.X = -invI2 * r2.X * r2.Y;
K3.Col1.Y = -invI2 * r2.X * r2.Y; K3.Col2.Y = invI2 * r2.X * r2.X;
Mat22 K = K1 + K2 + K3;
Vec2 impulse = K.Solve(-ptpC);
b1._sweep.C -= b1._invMass * impulse;
b1._sweep.A -= b1._invI * Vec2.Cross(r1, impulse);
b2._sweep.C += b2._invMass * impulse;
b2._sweep.A += b2._invI * Vec2.Cross(r2, impulse);
b1.SynchronizeTransform();
b2.SynchronizeTransform();
// Handle limits.
float angularError = 0.0f;
if (_enableLimit && _limitState != LimitState.InactiveLimit)
{
float angle = b2._sweep.A - b1._sweep.A - _referenceAngle;
float limitImpulse = 0.0f;
if (_limitState == LimitState.EqualLimits)
{
// Prevent large angular corrections
float limitC = Box2DXMath.Clamp(angle, -Settings.MaxAngularCorrection, Settings.MaxAngularCorrection);
limitImpulse = -_motorMass * limitC;
angularError = Box2DXMath.Abs(limitC);
}
else if (_limitState == LimitState.AtLowerLimit)
{
float limitC = angle - _lowerAngle;
angularError = Box2DXMath.Max(0.0f, -limitC);
// Prevent large angular corrections and allow some slop.
limitC = Box2DXMath.Clamp(limitC + Settings.AngularSlop, -Settings.MaxAngularCorrection, 0.0f);
limitImpulse = -_motorMass * limitC;
float oldLimitImpulse = _limitPositionImpulse;
_limitPositionImpulse = Box2DXMath.Max(_limitPositionImpulse + limitImpulse, 0.0f);
limitImpulse = _limitPositionImpulse - oldLimitImpulse;
}
else if (_limitState == LimitState.AtUpperLimit)
{
float limitC = angle - _upperAngle;
angularError = Box2DXMath.Max(0.0f, limitC);
// Prevent large angular corrections and allow some slop.
limitC = Box2DXMath.Clamp(limitC - Settings.AngularSlop, 0.0f, Settings.MaxAngularCorrection);
limitImpulse = -_motorMass * limitC;
float oldLimitImpulse = _limitPositionImpulse;
_limitPositionImpulse = Box2DXMath.Min(_limitPositionImpulse + limitImpulse, 0.0f);
limitImpulse = _limitPositionImpulse - oldLimitImpulse;
}
b1._sweep.A -= b1._invI * limitImpulse;
b2._sweep.A += b2._invI * limitImpulse;
b1.SynchronizeTransform();
b2.SynchronizeTransform();
}
return(positionError <= Settings.LinearSlop && angularError <= Settings.AngularSlop);
}
internal override bool SolvePositionConstraints(float baumgarte)
{
Body b1 = _body1;
Body b2 = _body2;
Vec2 s1 = _ground.GetXForm().Position + _groundAnchor1;
Vec2 s2 = _ground.GetXForm().Position + _groundAnchor2;
float linearError = 0.0f;
if (_state == LimitState.AtUpperLimit)
{
Vec2 r1 = Box2DXMath.Mul(b1.GetXForm().R, _localAnchor1 - b1.GetLocalCenter());
Vec2 r2 = Box2DXMath.Mul(b2.GetXForm().R, _localAnchor2 - b2.GetLocalCenter());
Vec2 p1 = b1._sweep.C + r1;
Vec2 p2 = b2._sweep.C + r2;
// Get the pulley axes.
_u1 = p1 - s1;
_u2 = p2 - s2;
float length1 = _u1.Length();
float length2 = _u2.Length();
if (length1 > Settings.LinearSlop)
{
_u1 *= 1.0f / length1;
}
else
{
_u1.SetZero();
}
if (length2 > Settings.LinearSlop)
{
_u2 *= 1.0f / length2;
}
else
{
_u2.SetZero();
}
float C = _constant - length1 - _ratio * length2;
linearError = Box2DXMath.Max(linearError, -C);
C = Box2DXMath.Clamp(C + Settings.LinearSlop, -Settings.MaxLinearCorrection, 0.0f);
float impulse = -_pulleyMass * C;
Vec2 P1 = -impulse * _u1;
Vec2 P2 = -_ratio * impulse * _u2;
b1._sweep.C += b1._invMass * P1;
b1._sweep.A += b1._invI * Vec2.Cross(r1, P1);
b2._sweep.C += b2._invMass * P2;
b2._sweep.A += b2._invI * Vec2.Cross(r2, P2);
b1.SynchronizeTransform();
b2.SynchronizeTransform();
}
if (_limitState1 == LimitState.AtUpperLimit)
{
Vec2 r1 = Box2DXMath.Mul(b1.GetXForm().R, _localAnchor1 - b1.GetLocalCenter());
Vec2 p1 = b1._sweep.C + r1;
_u1 = p1 - s1;
float length1 = _u1.Length();
if (length1 > Settings.LinearSlop)
{
_u1 *= 1.0f / length1;
}
else
{
_u1.SetZero();
}
float C = _maxLength1 - length1;
linearError = Box2DXMath.Max(linearError, -C);
C = Box2DXMath.Clamp(C + Settings.LinearSlop, -Settings.MaxLinearCorrection, 0.0f);
float impulse = -_limitMass1 * C;
Vec2 P1 = -impulse * _u1;
b1._sweep.C += b1._invMass * P1;
b1._sweep.A += b1._invI * Vec2.Cross(r1, P1);
b1.SynchronizeTransform();
}
if (_limitState2 == LimitState.AtUpperLimit)
{
Vec2 r2 = Box2DXMath.Mul(b2.GetXForm().R, _localAnchor2 - b2.GetLocalCenter());
Vec2 p2 = b2._sweep.C + r2;
_u2 = p2 - s2;
float length2 = _u2.Length();
if (length2 > Settings.LinearSlop)
{
_u2 *= 1.0f / length2;
}
else
{
_u2.SetZero();
}
float C = _maxLength2 - length2;
linearError = Box2DXMath.Max(linearError, -C);
C = Box2DXMath.Clamp(C + Settings.LinearSlop, -Settings.MaxLinearCorrection, 0.0f);
float impulse = -_limitMass2 * C;
Vec2 P2 = -impulse * _u2;
b2._sweep.C += b2._invMass * P2;
b2._sweep.A += b2._invI * Vec2.Cross(r2, P2);
b2.SynchronizeTransform();
}
return(linearError < Settings.LinearSlop);
}
// Find TOI contacts and solve them.
private void SolveTOI(TimeStep step)
{
// Reserve an island and a queue for TOI island solution.
Island island = new Island(_bodyCount,
Settings.MaxTOIContactsPerIsland,
Settings.MaxTOIJointsPerIsland,
_contactManager._contactListener);
//Simple one pass queue
//Relies on the fact that we're only making one pass
//through and each body can only be pushed/popped once.
//To push:
// queue[queueStart+queueSize++] = newElement;
//To pop:
// poppedElement = queue[queueStart++];
// --queueSize;
int queueCapacity = _bodyCount;
Body[] queue = new Body[queueCapacity];
for (Body b = _bodyList; b != null; b = b._next)
{
b._flags &= ~Body.BodyFlags.Island;
b._sweep.T0 = 0.0f;
}
for (Contact c = _contactManager._contactList; c != null; c = c.Next)
{
// Invalidate TOI
c.Flags &= ~(ContactFlag.ToiFlag | ContactFlag.IslandFlag);
}
for (Joint j = _jointList; j != null; j = j._next)
{
j._islandFlag = false;
}
// Find TOI events and solve them.
for (; ;)
{
// Find the first TOI.
Contact minContact = null;
float minTOI = 1.0f;
for (Contact c = _contactManager._contactList; c != null; c = c.Next)
{
// Can this contact generate a solid TOI contact?
if (c.IsSolid() == false || c.IsContinuous() == false)
{
continue;
}
// TODO_ERIN keep a counter on the contact, only respond to M TOIs per contact.
float toi = 1.0f;
if ((c.Flags & ContactFlag.ToiFlag) != 0)
{
// This contact has a valid cached TOI.
toi = c.Toi;
}
else
{
// Compute the TOI for this contact.
Fixture s1 = c.GetFixtureA();
Fixture s2 = c.GetFixtureB();
Body b1 = s1.GetBody();
Body b2 = s2.GetBody();
if ((b1.IsStatic() || b1.IsSleeping()) && (b2.IsStatic() || b2.IsSleeping()))
{
continue;
}
// Put the sweeps onto the same time interval.
float t0 = b1._sweep.T0;
if (b1._sweep.T0 < b2._sweep.T0)
{
t0 = b2._sweep.T0;
b1._sweep.Advance(t0);
}
else if (b2._sweep.T0 < b1._sweep.T0)
{
t0 = b1._sweep.T0;
b2._sweep.Advance(t0);
}
Box2DXDebug.Assert(t0 < 1.0f);
// Compute the time of impact.
toi = c.ComputeTOI(b1._sweep, b2._sweep);
Box2DXDebug.Assert(0.0f <= toi && toi <= 1.0f);
// If the TOI is in range ...
if (0.0f < toi && toi < 1.0f)
{
// Interpolate on the actual range.
toi = Math.Min((1.0f - toi) * t0 + toi, 1.0f);
}
c.Toi = toi;
c.Flags |= ContactFlag.ToiFlag;
}
if (Settings.FLT_EPSILON < toi && toi < minTOI)
{
// This is the minimum TOI found so far.
minContact = c;
minTOI = toi;
}
}
if (minContact == null || 1.0f - 100.0f * Settings.FLT_EPSILON < minTOI)
{
// No more TOI events. Done!
break;
}
// Advance the bodies to the TOI.
Fixture f1 = minContact.GetFixtureA();
Fixture f2 = minContact.GetFixtureB();
Body b3 = f1.GetBody();
Body b4 = f2.GetBody();
Sweep backup1 = b3._sweep;
Sweep backup2 = b4._sweep;
b3.Advance(minTOI);
b4.Advance(minTOI);
// The TOI contact likely has some new contact points.
minContact.Update(_contactManager._contactListener);
minContact.Flags &= ~ContactFlag.ToiFlag;
// Is the contact solid?
if (minContact.IsSolid() == false)
{
// Restore the sweeps.
b3._sweep = backup1;
b4._sweep = backup2;
b3.SynchronizeTransform();
b4.SynchronizeTransform();
continue;
}
// Did numerical issues prevent a contact point from being generated?
if (minContact.IsTouching() == false)
{
// Give up on this TOI.
continue;
}
// Build the TOI island. We need a dynamic seed.
Body seed = b3;
if (seed.IsStatic())
{
seed = b4;
}
// Reset island and queue.
island.Clear();
int queueStart = 0; // starting index for queue
int queueSize = 0; // elements in queue
queue[queueStart + queueSize++] = seed;
seed._flags |= Body.BodyFlags.Island;
// Perform a breadth first search (BFS) on the contact/joint graph.
while (queueSize > 0)
{
// Grab the next body off the stack and add it to the island.
Body b = queue[queueStart++];
--queueSize;
island.Add(ref b);
// Make sure the body is awake.
b._flags &= ~Body.BodyFlags.Sleep;
// To keep islands as small as possible, we don't
// propagate islands across static bodies.
if (b.IsStatic())
{
continue;
}
// Search all contacts connected to this body.
for (ContactEdge cEdge = b._contactList; cEdge != null; cEdge = cEdge.Next)
{
// Does the TOI island still have space for contacts?
if (island.ContactCount == island.ContactCapacity)
{
break;
}
// Has this contact already been added to an island? Skip slow or non-solid contacts.
if ((cEdge.Contact.Flags & ContactFlag.IslandFlag) != 0)
{
continue;
}
// Is this contact touching? For performance we are not updating this contact.
if (cEdge.Contact.IsSolid() == false || cEdge.Contact.IsTouching() == false)
{
continue;
}
island.Add(ref cEdge.Contact);
cEdge.Contact.Flags |= ContactFlag.IslandFlag;
// Update other body.
Body other = cEdge.Other;
// Was the other body already added to this island?
if ((other._flags & Body.BodyFlags.Island) != 0)
{
continue;
}
// March forward, this can do no harm since this is the min TOI.
if (other.IsStatic() == false)
{
other.Advance(minTOI);
other.WakeUp();
}
Box2DXDebug.Assert(queueStart + queueSize < queueCapacity);
queue[queueStart + queueSize] = other;
++queueSize;
other._flags |= Body.BodyFlags.Island;
}
for (JointEdge jEdge = b._jointList; jEdge != null; jEdge = jEdge.Next)
{
if (island.JointCount == island.JointCapacity)
{
continue;
}
if (jEdge.Joint._islandFlag == true)
{
continue;
}
island.Add(jEdge.Joint);
jEdge.Joint._islandFlag = true;
Body other = jEdge.Other;
if ((other._flags & Body.BodyFlags.Island) != 0)
{
continue;
}
if (!other.IsStatic())
{
other.Advance(minTOI);
other.WakeUp();
}
Box2DXDebug.Assert(queueStart + queueSize < queueCapacity);
queue[queueStart + queueSize] = other;
++queueSize;
other._flags |= Body.BodyFlags.Island;
}
}
TimeStep subStep;
subStep.WarmStarting = false;
subStep.Dt = (1.0f - minTOI) * step.Dt;
subStep.Inv_Dt = 1.0f / subStep.Dt;
subStep.DtRatio = 0.0f;
subStep.VelocityIterations = step.VelocityIterations;
subStep.PositionIterations = step.PositionIterations;
island.SolveTOI(ref subStep);
// Post solve cleanup.
for (int i = 0; i < island.BodyCount; ++i)
{
// Allow bodies to participate in future TOI islands.
Body b = island.Bodies[i];
b._flags &= ~Body.BodyFlags.Island;
if ((b._flags & Body.BodyFlags.Sleep) != 0)
{
continue;
}
if (b.IsStatic())
{
continue;
}
b.SynchronizeFixtures();
// Invalidate all contact TOIs associated with this body. Some of these
// may not be in the island because they were not touching.
for (ContactEdge ce = b._contactList; ce != null; ce = ce.Next)
{
ce.Contact.Flags &= ~ContactFlag.ToiFlag;
}
}
for (int i = 0; i < island.ContactCount; ++i)
{
// Allow contacts to participate in future TOI islands.
Contact c = island.Contacts[i];
c.Flags &= ~(ContactFlag.ToiFlag | ContactFlag.IslandFlag);
}
for (int i = 0; i < island.JointCount; ++i)
{
// Allow joints to participate in future TOI islands.
Joint j = island.Joints[i];
j._islandFlag = false;
}
// Commit fixture proxy movements to the broad-phase so that new contacts are created.
// Also, some contacts can be destroyed.
_contactManager.FindNewContacts();
}
queue = null;
}
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);
}
}
internal override bool SolvePositionConstraints(float baumgarte)
{
Body body = this._body1;
Body body2 = this._body2;
float num = 0f;
if (this._enableLimit && this._limitState != LimitState.InactiveLimit)
{
float num2 = body2._sweep.A - body._sweep.A - this._referenceAngle;
float num3 = 0f;
if (this._limitState == LimitState.EqualLimits)
{
float num4 = Box2DX.Common.Math.Clamp(num2, -Settings.MaxAngularCorrection, Settings.MaxAngularCorrection);
num3 = -this._motorMass * num4;
num = Box2DX.Common.Math.Abs(num4);
}
else
{
if (this._limitState == LimitState.AtLowerLimit)
{
float num4 = num2 - this._lowerAngle;
num = -num4;
num4 = Box2DX.Common.Math.Clamp(num4 + Settings.AngularSlop, -Settings.MaxAngularCorrection, 0f);
num3 = -this._motorMass * num4;
}
else
{
if (this._limitState == LimitState.AtUpperLimit)
{
float num4 = num2 - this._upperAngle;
num = num4;
num4 = Box2DX.Common.Math.Clamp(num4 - Settings.AngularSlop, 0f, Settings.MaxAngularCorrection);
num3 = -this._motorMass * num4;
}
}
}
Body expr_139_cp_0 = body;
expr_139_cp_0._sweep.A = expr_139_cp_0._sweep.A - body._invI * num3;
Body expr_154_cp_0 = body2;
expr_154_cp_0._sweep.A = expr_154_cp_0._sweep.A + body2._invI * num3;
body.SynchronizeTransform();
body2.SynchronizeTransform();
}
Vec2 vec = Box2DX.Common.Math.Mul(body.GetXForm().R, this._localAnchor1 - body.GetLocalCenter());
Vec2 vec2 = Box2DX.Common.Math.Mul(body2.GetXForm().R, this._localAnchor2 - body2.GetLocalCenter());
Vec2 vec3 = body2._sweep.C + vec2 - body._sweep.C - vec;
float num5 = vec3.Length();
float invMass = body._invMass;
float invMass2 = body2._invMass;
float invI = body._invI;
float invI2 = body2._invI;
float num6 = 10f * Settings.LinearSlop;
if (vec3.LengthSquared() > num6 * num6)
{
Vec2 vec4 = vec3;
vec4.Normalize();
float num7 = invMass + invMass2;
Box2DXDebug.Assert(num7 > Settings.FLT_EPSILON);
float a = 1f / num7;
Vec2 v = a * -vec3;
float num8 = 0.5f;
Body expr_283_cp_0 = body;
expr_283_cp_0._sweep.C = expr_283_cp_0._sweep.C - num8 * invMass * v;
Body expr_2A5_cp_0 = body2;
expr_2A5_cp_0._sweep.C = expr_2A5_cp_0._sweep.C + num8 * invMass2 * v;
vec3 = body2._sweep.C + vec2 - body._sweep.C - vec;
}
Mat22 a2 = default(Mat22);
a2.Col1.X = invMass + invMass2;
a2.Col2.X = 0f;
a2.Col1.Y = 0f;
a2.Col2.Y = invMass + invMass2;
Mat22 b = default(Mat22);
b.Col1.X = invI * vec.Y * vec.Y;
b.Col2.X = -invI * vec.X * vec.Y;
b.Col1.Y = -invI * vec.X * vec.Y;
b.Col2.Y = invI * vec.X * vec.X;
Mat22 b2 = default(Mat22);
b2.Col1.X = invI2 * vec2.Y * vec2.Y;
b2.Col2.X = -invI2 * vec2.X * vec2.Y;
b2.Col1.Y = -invI2 * vec2.X * vec2.Y;
b2.Col2.Y = invI2 * vec2.X * vec2.X;
Vec2 vec5 = (a2 + b + b2).Solve(-vec3);
Body expr_465_cp_0 = body;
expr_465_cp_0._sweep.C = expr_465_cp_0._sweep.C - body._invMass * vec5;
Body expr_488_cp_0 = body;
expr_488_cp_0._sweep.A = expr_488_cp_0._sweep.A - body._invI * Vec2.Cross(vec, vec5);
Body expr_4AA_cp_0 = body2;
expr_4AA_cp_0._sweep.C = expr_4AA_cp_0._sweep.C + body2._invMass * vec5;
Body expr_4CD_cp_0 = body2;
expr_4CD_cp_0._sweep.A = expr_4CD_cp_0._sweep.A + body2._invI * Vec2.Cross(vec2, vec5);
body.SynchronizeTransform();
body2.SynchronizeTransform();
return(num5 <= Settings.LinearSlop && num <= Settings.AngularSlop);
}
internal override bool SolvePositionConstraints(float baumgarte)
{
Body body = this._body1;
Body body2 = this._body2;
Vec2 v = this._ground.GetXForm().Position + this._groundAnchor1;
Vec2 v2 = this._ground.GetXForm().Position + this._groundAnchor2;
float num = 0f;
if (this._state == LimitState.AtUpperLimit)
{
Vec2 vec = Box2DX.Common.Math.Mul(body.GetXForm().R, this._localAnchor1 - body.GetLocalCenter());
Vec2 vec2 = Box2DX.Common.Math.Mul(body2.GetXForm().R, this._localAnchor2 - body2.GetLocalCenter());
Vec2 v3 = body._sweep.C + vec;
Vec2 v4 = body2._sweep.C + vec2;
this._u1 = v3 - v;
this._u2 = v4 - v2;
float num2 = this._u1.Length();
float num3 = this._u2.Length();
if (num2 > Settings.LinearSlop)
{
this._u1 *= 1f / num2;
}
else
{
this._u1.SetZero();
}
if (num3 > Settings.LinearSlop)
{
this._u2 *= 1f / num3;
}
else
{
this._u2.SetZero();
}
float num4 = this._constant - num2 - this._ratio * num3;
num = Box2DX.Common.Math.Max(num, -num4);
num4 = Box2DX.Common.Math.Clamp(num4 + Settings.LinearSlop, -Settings.MaxLinearCorrection, 0f);
float num5 = -this._pulleyMass * num4;
Vec2 vec3 = -num5 * this._u1;
Vec2 vec4 = -this._ratio * num5 * this._u2;
Body expr_1F6_cp_0 = body;
expr_1F6_cp_0._sweep.C = expr_1F6_cp_0._sweep.C + body._invMass * vec3;
Body expr_219_cp_0 = body;
expr_219_cp_0._sweep.A = expr_219_cp_0._sweep.A + body._invI * Vec2.Cross(vec, vec3);
Body expr_23B_cp_0 = body2;
expr_23B_cp_0._sweep.C = expr_23B_cp_0._sweep.C + body2._invMass * vec4;
Body expr_25E_cp_0 = body2;
expr_25E_cp_0._sweep.A = expr_25E_cp_0._sweep.A + body2._invI * Vec2.Cross(vec2, vec4);
body.SynchronizeTransform();
body2.SynchronizeTransform();
}
if (this._limitState1 == LimitState.AtUpperLimit)
{
Vec2 vec = Box2DX.Common.Math.Mul(body.GetXForm().R, this._localAnchor1 - body.GetLocalCenter());
Vec2 v3 = body._sweep.C + vec;
this._u1 = v3 - v;
float num2 = this._u1.Length();
if (num2 > Settings.LinearSlop)
{
this._u1 *= 1f / num2;
}
else
{
this._u1.SetZero();
}
float num4 = this._maxLength1 - num2;
num = Box2DX.Common.Math.Max(num, -num4);
num4 = Box2DX.Common.Math.Clamp(num4 + Settings.LinearSlop, -Settings.MaxLinearCorrection, 0f);
float num5 = -this._limitMass1 * num4;
Vec2 vec3 = -num5 * this._u1;
Body expr_381_cp_0 = body;
expr_381_cp_0._sweep.C = expr_381_cp_0._sweep.C + body._invMass * vec3;
Body expr_3A4_cp_0 = body;
expr_3A4_cp_0._sweep.A = expr_3A4_cp_0._sweep.A + body._invI * Vec2.Cross(vec, vec3);
body.SynchronizeTransform();
}
if (this._limitState2 == LimitState.AtUpperLimit)
{
Vec2 vec2 = Box2DX.Common.Math.Mul(body2.GetXForm().R, this._localAnchor2 - body2.GetLocalCenter());
Vec2 v4 = body2._sweep.C + vec2;
this._u2 = v4 - v2;
float num3 = this._u2.Length();
if (num3 > Settings.LinearSlop)
{
this._u2 *= 1f / num3;
}
else
{
this._u2.SetZero();
}
float num4 = this._maxLength2 - num3;
num = Box2DX.Common.Math.Max(num, -num4);
num4 = Box2DX.Common.Math.Clamp(num4 + Settings.LinearSlop, -Settings.MaxLinearCorrection, 0f);
float num5 = -this._limitMass2 * num4;
Vec2 vec4 = -num5 * this._u2;
Body expr_4C0_cp_0 = body2;
expr_4C0_cp_0._sweep.C = expr_4C0_cp_0._sweep.C + body2._invMass * vec4;
Body expr_4E3_cp_0 = body2;
expr_4E3_cp_0._sweep.A = expr_4E3_cp_0._sweep.A + body2._invI * Vec2.Cross(vec2, vec4);
body2.SynchronizeTransform();
}
return(num < Settings.LinearSlop);
}
internal override bool SolvePositionConstraints()
{
Body b1 = _body1;
Body b2 = _body2;
float invMass1 = b1._invMass, invMass2 = b2._invMass;
float invI1 = b1._invI, invI2 = b2._invI;
Vec2 r1 = Box2DXMath.Mul(b1.GetXForm().R, _localAnchor1 - b1.GetLocalCenter());
Vec2 r2 = Box2DXMath.Mul(b2.GetXForm().R, _localAnchor2 - b2.GetLocalCenter());
Vec2 p1 = b1._sweep.C + r1;
Vec2 p2 = b2._sweep.C + r2;
Vec2 d = p2 - p1;
Vec2 ay1 = Box2DXMath.Mul(b1.GetXForm().R, _localYAxis1);
// Solve linear (point-to-line) constraint.
float linearC = Vec2.Dot(ay1, d);
// Prevent overly large corrections.
linearC = Box2DXMath.Clamp(linearC, -Settings.MaxLinearCorrection, Settings.MaxLinearCorrection);
float linearImpulse = -_linearMass * linearC;
b1._sweep.C += (invMass1 * linearImpulse) * _linearJacobian.Linear1;
b1._sweep.A += invI1 * linearImpulse * _linearJacobian.Angular1;
//b1->SynchronizeTransform(); // updated by angular constraint
b2._sweep.C += (invMass2 * linearImpulse) * _linearJacobian.Linear2;
b2._sweep.A += invI2 * linearImpulse * _linearJacobian.Angular2;
//b2->SynchronizeTransform(); // updated by angular constraint
float positionError = Box2DXMath.Abs(linearC);
// Solve angular constraint.
float angularC = b2._sweep.A - b1._sweep.A - _refAngle;
// Prevent overly large corrections.
angularC = Box2DXMath.Clamp(angularC, -Settings.MaxAngularCorrection, Settings.MaxAngularCorrection);
float angularImpulse = -_angularMass * angularC;
b1._sweep.A -= b1._invI * angularImpulse;
b2._sweep.A += b2._invI * angularImpulse;
b1.SynchronizeTransform();
b2.SynchronizeTransform();
float angularError = Box2DXMath.Abs(angularC);
// Solve linear limit constraint.
if (_enableLimit && _limitState != LimitState.InactiveLimit)
{
Vec2 r1_ = Box2DXMath.Mul(b1.GetXForm().R, _localAnchor1 - b1.GetLocalCenter());
Vec2 r2_ = Box2DXMath.Mul(b2.GetXForm().R, _localAnchor2 - b2.GetLocalCenter());
Vec2 p1_ = b1._sweep.C + r1_;
Vec2 p2_ = b2._sweep.C + r2_;
Vec2 d_ = p2_ - p1_;
Vec2 ax1 = Box2DXMath.Mul(b1.GetXForm().R, _localXAxis1);
float translation = Vec2.Dot(ax1, d_);
float limitImpulse = 0.0f;
if (_limitState == LimitState.EqualLimits)
{
// Prevent large angular corrections
float limitC = Box2DXMath.Clamp(translation, -Settings.MaxLinearCorrection, Settings.MaxLinearCorrection);
limitImpulse = -_motorMass * limitC;
positionError = Box2DXMath.Max(positionError, Box2DXMath.Abs(angularC));
}
else if (_limitState == LimitState.AtLowerLimit)
{
float limitC = translation - _lowerTranslation;
positionError = Box2DXMath.Max(positionError, -limitC);
// Prevent large linear corrections and allow some slop.
limitC = Box2DXMath.Clamp(limitC + Settings.LinearSlop, -Settings.MaxLinearCorrection, 0.0f);
limitImpulse = -_motorMass * limitC;
float oldLimitImpulse = _limitPositionImpulse;
_limitPositionImpulse = Box2DXMath.Max(_limitPositionImpulse + limitImpulse, 0.0f);
limitImpulse = _limitPositionImpulse - oldLimitImpulse;
}
else if (_limitState == LimitState.AtUpperLimit)
{
float limitC = translation - _upperTranslation;
positionError = Box2DXMath.Max(positionError, limitC);
// Prevent large linear corrections and allow some slop.
limitC = Box2DXMath.Clamp(limitC - Settings.LinearSlop, 0.0f, Settings.MaxLinearCorrection);
limitImpulse = -_motorMass * limitC;
float oldLimitImpulse = _limitPositionImpulse;
_limitPositionImpulse = Box2DXMath.Min(_limitPositionImpulse + limitImpulse, 0.0f);
limitImpulse = _limitPositionImpulse - oldLimitImpulse;
}
b1._sweep.C += (invMass1 * limitImpulse) * _motorJacobian.Linear1;
b1._sweep.A += invI1 * limitImpulse * _motorJacobian.Angular1;
b2._sweep.C += (invMass2 * limitImpulse) * _motorJacobian.Linear2;
b2._sweep.A += invI2 * limitImpulse * _motorJacobian.Angular2;
b1.SynchronizeTransform();
b2.SynchronizeTransform();
}
return(positionError <= Settings.LinearSlop && angularError <= Settings.AngularSlop);
}
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;
}
}
}
}
internal override bool SolvePositionConstraints(float baumgarte)
{
Body b1 = _bodyA;
Body b2 = _bodyB;
Vec2 c1 = b1._sweep.C;
float a1 = b1._sweep.A;
Vec2 c2 = b2._sweep.C;
float a2 = b2._sweep.A;
// Solve linear limit constraint.
float linearError = 0.0f, angularError = 0.0f;
bool active = false;
float C2 = 0.0f;
Mat22 R1 = new Mat22(a1), R2 = new Mat22(a2);
Vec2 r1 = Box2DX.Common.Math.Mul(R1, _localAnchor1 - _localCenter1);
Vec2 r2 = Box2DX.Common.Math.Mul(R2, _localAnchor2 - _localCenter2);
Vec2 d = c2 + r2 - c1 - r1;
if (_enableLimit)
{
_axis = Box2DX.Common.Math.Mul(R1, _localXAxis1);
_a1 = Vec2.Cross(d + r1, _axis);
_a2 = Vec2.Cross(r2, _axis);
float translation = Vec2.Dot(_axis, d);
if (Box2DX.Common.Math.Abs(_upperTranslation - _lowerTranslation) < 2.0f * Settings.LinearSlop)
{
// Prevent large angular corrections
C2 = Box2DX.Common.Math.Clamp(translation, -Settings.MaxLinearCorrection, Settings.MaxLinearCorrection);
linearError = Box2DX.Common.Math.Abs(translation);
active = true;
}
else if (translation <= _lowerTranslation)
{
// Prevent large linear corrections and allow some slop.
C2 = Box2DX.Common.Math.Clamp(translation - _lowerTranslation + Settings.LinearSlop, -Settings.MaxLinearCorrection, 0.0f);
linearError = _lowerTranslation - translation;
active = true;
}
else if (translation >= _upperTranslation)
{
// Prevent large linear corrections and allow some slop.
C2 = Box2DX.Common.Math.Clamp(translation - _upperTranslation - Settings.LinearSlop, 0.0f, Settings.MaxLinearCorrection);
linearError = translation - _upperTranslation;
active = true;
}
}
_perp = Box2DX.Common.Math.Mul(R1, _localYAxis1);
_s1 = Vec2.Cross(d + r1, _perp);
_s2 = Vec2.Cross(r2, _perp);
Vec3 impulse;
Vec2 C1 = new Vec2();
C1.X = Vec2.Dot(_perp, d);
C1.Y = a2 - a1 - _refAngle;
linearError = Box2DX.Common.Math.Max(linearError, Box2DX.Common.Math.Abs(C1.X));
angularError = Box2DX.Common.Math.Abs(C1.Y);
if (active)
{
float m1 = _invMass1, m2 = _invMass2;
float i1 = _invI1, i2 = _invI2;
float k11 = m1 + m2 + i1 * _s1 * _s1 + i2 * _s2 * _s2;
float k12 = i1 * _s1 + i2 * _s2;
float k13 = i1 * _s1 * _a1 + i2 * _s2 * _a2;
float k22 = i1 + i2;
float k23 = i1 * _a1 + i2 * _a2;
float k33 = m1 + m2 + i1 * _a1 * _a1 + i2 * _a2 * _a2;
_K.Col1.Set(k11, k12, k13);
_K.Col2.Set(k12, k22, k23);
_K.Col3.Set(k13, k23, k33);
Vec3 C = new Vec3();
C.X = C1.X;
C.Y = C1.Y;
C.Z = C2;
impulse = _K.Solve33(-C);
}
else
{
float m1 = _invMass1, m2 = _invMass2;
float i1 = _invI1, i2 = _invI2;
float k11 = m1 + m2 + i1 * _s1 * _s1 + i2 * _s2 * _s2;
float k12 = i1 * _s1 + i2 * _s2;
float k22 = i1 + i2;
_K.Col1.Set(k11, k12, 0.0f);
_K.Col2.Set(k12, k22, 0.0f);
Vec2 impulse1 = _K.Solve22(-C1);
impulse.X = impulse1.X;
impulse.Y = impulse1.Y;
impulse.Z = 0.0f;
}
Vec2 P = impulse.X * _perp + impulse.Z * _axis;
float L1 = impulse.X * _s1 + impulse.Y + impulse.Z * _a1;
float L2 = impulse.X * _s2 + impulse.Y + impulse.Z * _a2;
c1 -= _invMass1 * P;
a1 -= _invI1 * L1;
c2 += _invMass2 * P;
a2 += _invI2 * L2;
// TODO_ERIN remove need for this.
b1._sweep.C = c1;
b1._sweep.A = a1;
b2._sweep.C = c2;
b2._sweep.A = a2;
b1.SynchronizeTransform();
b2.SynchronizeTransform();
return(linearError <= Settings.LinearSlop && angularError <= Settings.AngularSlop);
}
internal override bool SolvePositionConstraints(float baumgarte)
{
// TODO_ERIN block solve with limit.
Body b1 = _body1;
Body b2 = _body2;
float angularError = 0.0f;
float positionError = 0.0f;
// Solve angular limit constraint.
if (_enableLimit && _limitState != LimitState.InactiveLimit)
{
float angle = b2._sweep.A - b1._sweep.A - _referenceAngle;
float limitImpulse = 0.0f;
if (_limitState == LimitState.EqualLimits)
{
// Prevent large angular corrections
float C = Box2DX.Common.Math.Clamp(angle, -Settings.MaxAngularCorrection, Settings.MaxAngularCorrection);
limitImpulse = -_motorMass * C;
angularError = Box2DXMath.Abs(C);
}
else if (_limitState == LimitState.AtLowerLimit)
{
float C = angle - _lowerAngle;
angularError = -C;
// Prevent large angular corrections and allow some slop.
C = Box2DX.Common.Math.Clamp(C + Settings.AngularSlop, -Settings.MaxAngularCorrection, 0.0f);
limitImpulse = -_motorMass * C;
}
else if (_limitState == LimitState.AtUpperLimit)
{
float C = angle - _upperAngle;
angularError = C;
// Prevent large angular corrections and allow some slop.
C = Box2DX.Common.Math.Clamp(C - Settings.AngularSlop, 0.0f, Settings.MaxAngularCorrection);
limitImpulse = -_motorMass * C;
}
b1._sweep.A -= b1._invI * limitImpulse;
b2._sweep.A += b2._invI * limitImpulse;
b1.SynchronizeTransform();
b2.SynchronizeTransform();
}
// Solve point-to-point constraint.
{
Vector2 r1 = b1.GetTransform().TransformDirection(_localAnchor1 - b1.GetLocalCenter());
Vector2 r2 = b2.GetTransform().TransformDirection(_localAnchor2 - b2.GetLocalCenter());
Vector2 C = b2._sweep.C + r2 - b1._sweep.C - r1;
positionError = C.Length;
float invMass1 = b1._invMass, invMass2 = b2._invMass;
float invI1 = b1._invI, invI2 = b2._invI;
// Handle large detachment.
float k_allowedStretch = 10.0f * Settings.LinearSlop;
if (C.LengthSquared > k_allowedStretch * k_allowedStretch)
{
// Use a particle solution (no rotation).
Vector2 u = C; u.Normalize();
float k = invMass1 + invMass2;
Box2DXDebug.Assert(k > Settings.FLT_EPSILON);
float m = 1.0f / k;
Vector2 impulse = m * (-C);
float k_beta = 0.5f;
b1._sweep.C -= k_beta * invMass1 * impulse;
b2._sweep.C += k_beta * invMass2 * impulse;
C = b2._sweep.C + r2 - b1._sweep.C - r1;
}
Mat22 K1 = new Mat22();
K1.Col1.X = invMass1 + invMass2; K1.Col2.X = 0.0f;
K1.Col1.Y = 0.0f; K1.Col2.Y = invMass1 + invMass2;
Mat22 K2 = new Mat22();
K2.Col1.X = invI1 * r1.Y * r1.Y; K2.Col2.X = -invI1 * r1.X * r1.Y;
K2.Col1.Y = -invI1 * r1.X * r1.Y; K2.Col2.Y = invI1 * r1.X * r1.X;
Mat22 K3 = new Mat22();
K3.Col1.X = invI2 * r2.Y * r2.Y; K3.Col2.X = -invI2 * r2.X * r2.Y;
K3.Col1.Y = -invI2 * r2.X * r2.Y; K3.Col2.Y = invI2 * r2.X * r2.X;
Mat22 K = K1 + K2 + K3;
Vector2 impulse_ = K.Solve(-C);
b1._sweep.C -= b1._invMass * impulse_;
b1._sweep.A -= b1._invI * r1.Cross(impulse_);
b2._sweep.C += b2._invMass * impulse_;
b2._sweep.A += b2._invI * r2.Cross(impulse_);
b1.SynchronizeTransform();
b2.SynchronizeTransform();
}
return(positionError <= Settings.LinearSlop && angularError <= Settings.AngularSlop);
}
internal override bool SolvePositionConstraints(float baumgarte)
{
Body body = this._body1;
Body body2 = this._body2;
Vec2 vec = body._sweep.C;
float num = body._sweep.A;
Vec2 vec2 = body2._sweep.C;
float num2 = body2._sweep.A;
float num3 = 0f;
bool flag = false;
float z = 0f;
Mat22 a = new Mat22(num);
Mat22 a2 = new Mat22(num2);
Vec2 v = Box2DX.Common.Math.Mul(a, this._localAnchor1 - this._localCenter1);
Vec2 vec3 = Box2DX.Common.Math.Mul(a2, this._localAnchor2 - this._localCenter2);
Vec2 vec4 = vec2 + vec3 - vec - v;
if (this._enableLimit)
{
this._axis = Box2DX.Common.Math.Mul(a, this._localXAxis1);
this._a1 = Vec2.Cross(vec4 + v, this._axis);
this._a2 = Vec2.Cross(vec3, this._axis);
float num4 = Vec2.Dot(this._axis, vec4);
if (Box2DX.Common.Math.Abs(this._upperTranslation - this._lowerTranslation) < 2f * Settings.LinearSlop)
{
z = Box2DX.Common.Math.Clamp(num4, -Settings.MaxLinearCorrection, Settings.MaxLinearCorrection);
num3 = Box2DX.Common.Math.Abs(num4);
flag = true;
}
else
{
if (num4 <= this._lowerTranslation)
{
z = Box2DX.Common.Math.Clamp(num4 - this._lowerTranslation + Settings.LinearSlop, -Settings.MaxLinearCorrection, 0f);
num3 = this._lowerTranslation - num4;
flag = true;
}
else
{
if (num4 >= this._upperTranslation)
{
z = Box2DX.Common.Math.Clamp(num4 - this._upperTranslation - Settings.LinearSlop, 0f, Settings.MaxLinearCorrection);
num3 = num4 - this._upperTranslation;
flag = true;
}
}
}
}
this._perp = Box2DX.Common.Math.Mul(a, this._localYAxis1);
this._s1 = Vec2.Cross(vec4 + v, this._perp);
this._s2 = Vec2.Cross(vec3, this._perp);
Vec2 v2 = default(Vec2);
v2.X = Vec2.Dot(this._perp, vec4);
v2.Y = num2 - num - this._refAngle;
num3 = Box2DX.Common.Math.Max(num3, Box2DX.Common.Math.Abs(v2.X));
float num5 = Box2DX.Common.Math.Abs(v2.Y);
Vec3 vec5;
if (flag)
{
float invMass = this._invMass1;
float invMass2 = this._invMass2;
float invI = this._invI1;
float invI2 = this._invI2;
float x = invMass + invMass2 + invI * this._s1 * this._s1 + invI2 * this._s2 * this._s2;
float num6 = invI * this._s1 + invI2 * this._s2;
float num7 = invI * this._s1 * this._a1 + invI2 * this._s2 * this._a2;
float y = invI + invI2;
float num8 = invI * this._a1 + invI2 * this._a2;
float z2 = invMass + invMass2 + invI * this._a1 * this._a1 + invI2 * this._a2 * this._a2;
this._K.Col1.Set(x, num6, num7);
this._K.Col2.Set(num6, y, num8);
this._K.Col3.Set(num7, num8, z2);
vec5 = this._K.Solve33(-new Vec3
{
X = v2.X,
Y = v2.Y,
Z = z
});
}
else
{
float invMass = this._invMass1;
float invMass2 = this._invMass2;
float invI = this._invI1;
float invI2 = this._invI2;
float x = invMass + invMass2 + invI * this._s1 * this._s1 + invI2 * this._s2 * this._s2;
float num6 = invI * this._s1 + invI2 * this._s2;
float y = invI + invI2;
this._K.Col1.Set(x, num6, 0f);
this._K.Col2.Set(num6, y, 0f);
Vec2 vec6 = this._K.Solve22(-v2);
vec5.X = vec6.X;
vec5.Y = vec6.Y;
vec5.Z = 0f;
}
Vec2 v3 = vec5.X * this._perp + vec5.Z * this._axis;
float num9 = vec5.X * this._s1 + vec5.Y + vec5.Z * this._a1;
float num10 = vec5.X * this._s2 + vec5.Y + vec5.Z * this._a2;
vec -= this._invMass1 * v3;
num -= this._invI1 * num9;
vec2 += this._invMass2 * v3;
num2 += this._invI2 * num10;
body._sweep.C = vec;
body._sweep.A = num;
body2._sweep.C = vec2;
body2._sweep.A = num2;
body.SynchronizeTransform();
body2.SynchronizeTransform();
return(num3 <= Settings.LinearSlop && num5 <= Settings.AngularSlop);
}
