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 (Mathf.Abs(_upperTranslation - _lowerTranslation) < 2.0f * Settings.LinearSlop) { // Prevent large angular corrections C2 = Mathf.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 = Mathf.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 = Mathf.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; }