internal override bool SolvePositionConstraints(float baumgarte)
{
if (_frequencyHz > 0.0f)
{
// There is no position correction for soft distance raints.
return(true);
}
Body b1 = _bodyA;
Body b2 = _bodyB;
XForm xf1, xf2;
b1.GetXForm(out xf1);
b2.GetXForm(out xf2);
Vector2 r1 = MathUtils.Multiply(ref xf1.R, _localAnchor1 - b1.GetLocalCenter());
Vector2 r2 = MathUtils.Multiply(ref xf2.R, _localAnchor2 - b2.GetLocalCenter());
Vector2 d = b2._sweep.c + r2 - b1._sweep.c - r1;
float length = d.Length();
d /= length;
float C = length - _length;
C = MathUtils.Clamp(C, -Settings.b2_maxLinearCorrection, Settings.b2_maxLinearCorrection);
float impulse = -_mass * C;
_u = d;
Vector2 P = impulse * _u;
b1._sweep.c -= b1._invMass * P;
b1._sweep.a -= b1._invI * MathUtils.Cross(r1, P);
b2._sweep.c += b2._invMass * P;
b2._sweep.a += b2._invI * MathUtils.Cross(r2, P);
b1.SynchronizeTransform();
b2.SynchronizeTransform();
return(Math.Abs(C) < Settings.b2_linearSlop);
}
internal override bool SolvePositionConstraints(float baumgarte)
{
float linearError = 0.0f;
Body b1 = _bodyA;
Body b2 = _bodyB;
float coordinate1, coordinate2;
if (_revolute1 != null)
{
coordinate1 = _revolute1.GetJointAngle();
}
else
{
coordinate1 = _prismatic1.GetJointTranslation();
}
if (_revolute2 != null)
{
coordinate2 = _revolute2.GetJointAngle();
}
else
{
coordinate2 = _prismatic2.GetJointTranslation();
}
float C = _ant - (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.b2_linearSlop);
}
internal override bool SolvePositionConstraints(float baumgarte)
{
Body b1 = _bodyA;
Body b2 = _bodyB;
Vector2 c1 = b1._sweep.c;
float a1 = b1._sweep.a;
Vector2 c2 = b2._sweep.c;
float a2 = b2._sweep.a;
// Solve linear limit raint.
float linearError = 0.0f, angularError = 0.0f;
bool active = false;
float C2 = 0.0f;
Mat22 R1 = new Mat22(a1);
Mat22 R2 = new Mat22(a2);
Vector2 r1 = MathUtils.Multiply(ref R1, _localAnchor1 - _localCenter1);
Vector2 r2 = MathUtils.Multiply(ref R2, _localAnchor2 - _localCenter2);
Vector2 d = c2 + r2 - c1 - r1;
if (_enableLimit)
{
_axis = MathUtils.Multiply(ref R1, _localXAxis1);
_a1 = MathUtils.Cross(d + r1, _axis);
_a2 = MathUtils.Cross(r2, _axis);
float translation = Vector2.Dot(_axis, d);
if (Math.Abs(_upperTranslation - _lowerTranslation) < 2.0f * Settings.b2_linearSlop)
{
// Prevent large angular corrections
C2 = MathUtils.Clamp(translation, -Settings.b2_maxLinearCorrection, Settings.b2_maxLinearCorrection);
linearError = Math.Abs(translation);
active = true;
}
else if (translation <= _lowerTranslation)
{
// Prevent large linear corrections and allow some slop.
C2 = MathUtils.Clamp(translation - _lowerTranslation + Settings.b2_linearSlop, -Settings.b2_maxLinearCorrection, 0.0f);
linearError = _lowerTranslation - translation;
active = true;
}
else if (translation >= _upperTranslation)
{
// Prevent large linear corrections and allow some slop.
C2 = MathUtils.Clamp(translation - _upperTranslation - Settings.b2_linearSlop, 0.0f, Settings.b2_maxLinearCorrection);
linearError = translation - _upperTranslation;
active = true;
}
}
_perp = MathUtils.Multiply(ref R1, _localYAxis1);
_s1 = MathUtils.Cross(d + r1, _perp);
_s2 = MathUtils.Cross(r2, _perp);
Vector3 impulse;
Vector2 C1 = new Vector2(Vector2.Dot(_perp, d), a2 - a1 - _refAngle);
linearError = Math.Max(linearError, Math.Abs(C1.X));
angularError = 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 = new Vector3(k11, k12, k13);
_K.col2 = new Vector3(k12, k22, k23);
_K.col3 = new Vector3(k13, k23, k33);
Vector3 C = new Vector3(-C1.X, -C1.Y, -C2);
impulse = _K.Solve33(C); // negated above
}
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 = new Vector3(k11, k12, 0.0f);
_K.col2 = new Vector3(k12, k22, 0.0f);
Vector2 impulse1 = _K.Solve22(-C1);
impulse.X = impulse1.X;
impulse.Y = impulse1.Y;
impulse.Z = 0.0f;
}
Vector2 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.b2_linearSlop && angularError <= Settings.b2_angularSlop);
}
internal override bool SolvePositionConstraints(float baumgarte)
{
Body b1 = _bodyA;
Body b2 = _bodyB;
Vector2 s1 = _groundAnchor1;
Vector2 s2 = _groundAnchor2;
float linearError = 0.0f;
if (_state == LimitState.AtUpper)
{
XForm xf1, xf2;
b1.GetXForm(out xf1);
b2.GetXForm(out xf2);
Vector2 r1 = MathUtils.Multiply(ref xf1.R, _localAnchor1 - b1.GetLocalCenter());
Vector2 r2 = MathUtils.Multiply(ref xf2.R, _localAnchor2 - b2.GetLocalCenter());
Vector2 p1 = b1._sweep.c + r1;
Vector2 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.b2_linearSlop)
{
_u1 *= 1.0f / length1;
}
else
{
_u1 = Vector2.Zero;
}
if (length2 > Settings.b2_linearSlop)
{
_u2 *= 1.0f / length2;
}
else
{
_u2 = Vector2.Zero;
}
float C = _ant - length1 - _ratio * length2;
linearError = Math.Max(linearError, -C);
C = MathUtils.Clamp(C + Settings.b2_linearSlop, -Settings.b2_maxLinearCorrection, 0.0f);
float impulse = -_pulleyMass * C;
Vector2 P1 = -impulse * _u1;
Vector2 P2 = -_ratio * impulse * _u2;
b1._sweep.c += b1._invMass * P1;
b1._sweep.a += b1._invI * MathUtils.Cross(r1, P1);
b2._sweep.c += b2._invMass * P2;
b2._sweep.a += b2._invI * MathUtils.Cross(r2, P2);
b1.SynchronizeTransform();
b2.SynchronizeTransform();
}
if (_limitState1 == LimitState.AtUpper)
{
XForm xf1;
b1.GetXForm(out xf1);
Vector2 r1 = MathUtils.Multiply(ref xf1.R, _localAnchor1 - b1.GetLocalCenter());
Vector2 p1 = b1._sweep.c + r1;
_u1 = p1 - s1;
float length1 = _u1.Length();
if (length1 > Settings.b2_linearSlop)
{
_u1 *= 1.0f / length1;
}
else
{
_u1 = Vector2.Zero;
}
float C = _maxLength1 - length1;
linearError = Math.Max(linearError, -C);
C = MathUtils.Clamp(C + Settings.b2_linearSlop, -Settings.b2_maxLinearCorrection, 0.0f);
float impulse = -_limitMass1 * C;
Vector2 P1 = -impulse * _u1;
b1._sweep.c += b1._invMass * P1;
b1._sweep.a += b1._invI * MathUtils.Cross(r1, P1);
b1.SynchronizeTransform();
}
if (_limitState2 == LimitState.AtUpper)
{
XForm xf2;
b2.GetXForm(out xf2);
Vector2 r2 = MathUtils.Multiply(ref xf2.R, _localAnchor2 - b2.GetLocalCenter());
Vector2 p2 = b2._sweep.c + r2;
_u2 = p2 - s2;
float length2 = _u2.Length();
if (length2 > Settings.b2_linearSlop)
{
_u2 *= 1.0f / length2;
}
else
{
_u2 = Vector2.Zero;
}
float C = _maxLength2 - length2;
linearError = Math.Max(linearError, -C);
C = MathUtils.Clamp(C + Settings.b2_linearSlop, -Settings.b2_maxLinearCorrection, 0.0f);
float impulse = -_limitMass2 * C;
Vector2 P2 = -impulse * _u2;
b2._sweep.c += b2._invMass * P2;
b2._sweep.a += b2._invI * MathUtils.Cross(r2, P2);
b2.SynchronizeTransform();
}
return(linearError < Settings.b2_linearSlop);
}
public bool SolvePositionConstraints(float baumgarte)
{
float minSeparation = 0.0f;
int constraintCount = _constraints.Count;
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(ref c);
Vector2 normal = psm._normal;
// Solve normal constraints
for (int j = 0; j < c.pointCount; ++j)
{
ContactConstraintPoint ccp = c.points[j];
Vector2 point = psm._points[j];
float separation = psm._separations[j];
Vector2 rA = point - bodyA._sweep.c;
Vector2 rB = point - bodyB._sweep.c;
// Track max raint error.
minSeparation = Math.Min(minSeparation, separation);
// Prevent large corrections and allow slop.
float C = baumgarte * MathUtils.Clamp(separation + Settings.b2_linearSlop, -Settings.b2_maxLinearCorrection, 0.0f);
// Compute normal impulse
float impulse = -ccp.equalizedMass * C;
#if MATH_OVERLOADS
Vector2 P = impulse * normal;
bodyA._sweep.c -= invMassA * P;
bodyA._sweep.a -= invIA * MathUtils.Cross(rA, P);
bodyB._sweep.c += invMassB * P;
bodyB._sweep.a += invIB * MathUtils.Cross(rB, P);
#else
Vector2 P = new Vector2(impulse * normal.X, impulse * normal.Y);
bodyA._sweep.c.X -= invMassA * P.X;
bodyA._sweep.c.Y -= invMassA * P.Y;
bodyA._sweep.a -= invIA * (rA.X * P.Y - rA.Y * P.X);
bodyB._sweep.c.X += invMassB * P.X;
bodyB._sweep.c.Y += invMassB * P.Y;
bodyB._sweep.a += invIB * (rB.X * P.Y - rB.Y * P.X);
#endif
bodyA.SynchronizeTransform();
bodyB.SynchronizeTransform();
}
}
// We can't expect minSpeparation >= -Settings.b2_linearSlop because we don't
// push the separation above -Settings.b2_linearSlop.
return(minSeparation >= -1.5f * Settings.b2_linearSlop);
}
public void Solve(ref TimeStep step, Vector2 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 = new Vector2(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 *= MathUtils.Clamp(1.0f - step.dt * b._linearDamping, 0.0f, 1.0f);
b._angularVelocity *= MathUtils.Clamp(1.0f - step.dt * b._angularDamping, 0.0f, 1.0f);
}
_contactSolver.Reset(ref step, _contacts);
// Initialize velocity raints.
_contactSolver.InitVelocityConstraints(ref step);
for (int i = 0; i < _jointCount; ++i)
{
_joints[i].InitVelocityConstraints(ref step);
}
// Solve velocity raints.
for (int i = 0; i < step.velocityIterations; ++i)
{
for (int j = 0; j < _jointCount; ++j)
{
_joints[j].SolveVelocityConstraints(ref 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.
Vector2 translation = step.dt * b._linearVelocity;
if (Vector2.Dot(translation, translation) > Settings.b2_maxTranslationSquared)
{
translation.Normalize();
b._linearVelocity = (Settings.b2_maxTranslation * step.inv_dt) * translation;
}
float rotation = step.dt * b._angularVelocity;
if (rotation * rotation > Settings.b2_maxRotationSquared)
{
if (rotation < 0.0)
{
b._angularVelocity = -step.inv_dt * Settings.b2_maxRotation;
}
else
{
b._angularVelocity = step.inv_dt * Settings.b2_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 raints.
for (int i = 0; i < step.positionIterations; ++i)
{
bool contactsOkay = _contactSolver.SolvePositionConstraints(Settings.b2_contactBaumgarte);
bool jointsOkay = true;
for (int j = 0; j < _jointCount; ++j)
{
bool jointOkay = _joints[j].SolvePositionConstraints(Settings.b2_contactBaumgarte);
jointsOkay = jointsOkay && jointOkay;
}
if (contactsOkay && jointsOkay)
{
// Exit early if the position errors are small.
break;
}
}
Report(_contactSolver._constraints);
if (allowSleep)
{
float minSleepTime = Settings.b2_FLT_MAX;
#if !TARGET_float_IS_FIXED
float linTolSqr = Settings.b2_linearSleepTolerance * Settings.b2_linearSleepTolerance;
float angTolSqr = Settings.b2_angularSleepTolerance * Settings.b2_angularSleepTolerance;
#endif
for (int i = 0; i < _bodyCount; ++i)
{
Body b = _bodies[i];
if (b._invMass == 0.0f)
{
continue;
}
if ((b._flags & BodyFlags.AllowSleep) == 0)
{
b._sleepTime = 0.0f;
minSleepTime = 0.0f;
}
if ((b._flags & BodyFlags.AllowSleep) == 0 ||
#if TARGET_float_IS_FIXED
MathUtils.Abs(b._angularVelocity) > Settings.b2_angularSleepTolerance ||
MathUtils.Abs(b._linearVelocity.X) > Settings.b2_linearSleepTolerance ||
MathUtils.Abs(b._linearVelocity.Y) > Settings.b2_linearSleepTolerance)
#else
b._angularVelocity *b._angularVelocity > angTolSqr ||
Vector2.Dot(b._linearVelocity, b._linearVelocity) > linTolSqr)
#endif
{
b._sleepTime = 0.0f;
minSleepTime = 0.0f;
}
else
{
b._sleepTime += step.dt;
minSleepTime = Math.Min(minSleepTime, b._sleepTime);
}
}
internal override bool SolvePositionConstraints(float baumgarte)
{
// TODO_ERIN block solve with limit. COME ON ERIN
Body b1 = _bodyA;
Body b2 = _bodyB;
float angularError = 0.0f;
float positionError = 0.0f;
// Solve angular limit raint.
if (_enableLimit && _limitState != LimitState.Inactive)
{
float angle = b2._sweep.a - b1._sweep.a - _referenceAngle;
float limitImpulse = 0.0f;
if (_limitState == LimitState.Equal)
{
// Prevent large angular corrections
float C = MathUtils.Clamp(angle - _lowerAngle, -Settings.b2_maxAngularCorrection, Settings.b2_maxAngularCorrection);
limitImpulse = -_motorMass * C;
angularError = Math.Abs(C);
}
else if (_limitState == LimitState.AtLower)
{
float C = angle - _lowerAngle;
angularError = -C;
// Prevent large angular corrections and allow some slop.
C = MathUtils.Clamp(C + Settings.b2_angularSlop, -Settings.b2_maxAngularCorrection, 0.0f);
limitImpulse = -_motorMass * C;
}
else if (_limitState == LimitState.AtUpper)
{
float C = angle - _upperAngle;
angularError = C;
// Prevent large angular corrections and allow some slop.
C = MathUtils.Clamp(C - Settings.b2_angularSlop, 0.0f, Settings.b2_maxAngularCorrection);
limitImpulse = -_motorMass * C;
}
b1._sweep.a -= b1._invI * limitImpulse;
b2._sweep.a += b2._invI * limitImpulse;
b1.SynchronizeTransform();
b2.SynchronizeTransform();
}
// Solve point-to-point raint.
{
XForm xf1, xf2;
b1.GetXForm(out xf1);
b2.GetXForm(out xf2);
Vector2 r1 = MathUtils.Multiply(ref xf1.R, _localAnchor1 - b1.GetLocalCenter());
Vector2 r2 = MathUtils.Multiply(ref xf2.R, _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.b2_linearSlop;
if (C.LengthSquared() > k_allowedStretch * k_allowedStretch)
{
// Use a particle solution (no rotation).
Vector2 u = C; u.Normalize();
float k = invMass1 + invMass2;
Debug.Assert(k > Settings.b2_FLT_EPSILON);
float m = 1.0f / k;
Vector2 impulse2 = m * (-C);
float k_beta = 0.5f;
b1._sweep.c -= k_beta * invMass1 * impulse2;
b2._sweep.c += k_beta * invMass2 * impulse2;
C = b2._sweep.c + r2 - b1._sweep.c - r1;
}
Mat22 K1 = new Mat22(new Vector2(invMass1 + invMass2, 0.0f), new Vector2(0.0f, invMass1 + invMass2));
Mat22 K2 = new Mat22(new Vector2(invI1 * r1.Y * r1.Y, -invI1 * r1.X * r1.Y), new Vector2(-invI1 * r1.X * r1.Y, invI1 * r1.X * r1.X));
Mat22 K3 = new Mat22(new Vector2(invI2 * r2.Y * r2.Y, -invI2 * r2.X * r2.Y), new Vector2(-invI2 * r2.X * r2.Y, invI2 * r2.X * r2.X));
Mat22 Ka;
Mat22 K;
Mat22.Add(ref K1, ref K2, out Ka);
Mat22.Add(ref Ka, ref K3, out K);
Vector2 impulse = K.Solve(-C);
b1._sweep.c -= b1._invMass * impulse;
b1._sweep.a -= b1._invI * MathUtils.Cross(r1, impulse);
b2._sweep.c += b2._invMass * impulse;
b2._sweep.a += b2._invI * MathUtils.Cross(r2, impulse);
b1.SynchronizeTransform();
b2.SynchronizeTransform();
}
return(positionError <= Settings.b2_linearSlop && angularError <= Settings.b2_angularSlop);
}
