internal override bool SolvePositionConstraints(float baumgarte)
{
Body b1 = _body1;
Body b2 = _body2;
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);
Vec2 impulse;
float C1;
C1 = Vec2.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.Set(k11, k12);
_K.Col2.Set(k12, k22);
Vec2 C = new Vec2();
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;
}
Vec2 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;
}