internal override bool SolvePositionConstraints(float baumgarte) { Body b1 = _body1; Body b2 = _body2; Vector2 s1 = _ground.GetXForm().Position + _groundAnchor1; Vector2 s2 = _ground.GetXForm().Position + _groundAnchor2; float linearError = 0.0f; if (_state == LimitState.AtUpperLimit) { Vector2 r1 = CommonMath.Mul(b1.GetXForm().R, _localAnchor1 - b1.GetLocalCenter()); Vector2 r2 = CommonMath.Mul(b2.GetXForm().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.LinearSlop) { _u1 *= 1.0f / length1; } else { _u1 = Vector2.Zero; } if (length2 > Settings.LinearSlop) { _u2 *= 1.0f / length2; } else { _u2 = Vector2.Zero; } float C = _constant - length1 - _ratio * length2; linearError = CommonMath.Max(linearError, -C); C = CommonMath.Clamp(C + Settings.LinearSlop, -Settings.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 * CommonMath.Cross(ref r1, ref P1); b2._sweep.C += b2._invMass * P2; b2._sweep.A += b2._invI * CommonMath.Cross(ref r2, ref P2); b1.SynchronizeTransform(); b2.SynchronizeTransform(); } if (_limitState1 == LimitState.AtUpperLimit) { Vector2 r1 = CommonMath.Mul(b1.GetXForm().R, _localAnchor1 - b1.GetLocalCenter()); Vector2 p1 = b1._sweep.C + r1; _u1 = p1 - s1; float length1 = _u1.Length(); if (length1 > Settings.LinearSlop) { _u1 *= 1.0f / length1; } else { _u1 = Vector2.Zero; } float C = _maxLength1 - length1; linearError = CommonMath.Max(linearError, -C); C = CommonMath.Clamp(C + Settings.LinearSlop, -Settings.MaxLinearCorrection, 0.0f); float impulse = -_limitMass1 * C; Vector2 P1 = -impulse * _u1; b1._sweep.C += b1._invMass * P1; b1._sweep.A += b1._invI * CommonMath.Cross(ref r1, ref P1); b1.SynchronizeTransform(); } if (_limitState2 == LimitState.AtUpperLimit) { Vector2 r2 = CommonMath.Mul(b2.GetXForm().R, _localAnchor2 - b2.GetLocalCenter()); Vector2 p2 = b2._sweep.C + r2; _u2 = p2 - s2; float length2 = _u2.Length(); if (length2 > Settings.LinearSlop) { _u2 *= 1.0f / length2; } else { _u2 = Vector2.Zero; } float C = _maxLength2 - length2; linearError = CommonMath.Max(linearError, -C); C = CommonMath.Clamp(C + Settings.LinearSlop, -Settings.MaxLinearCorrection, 0.0f); float impulse = -_limitMass2 * C; Vector2 P2 = -impulse * _u2; b2._sweep.C += b2._invMass * P2; b2._sweep.A += b2._invI * CommonMath.Cross(ref r2, ref P2); b2.SynchronizeTransform(); } return(linearError < Settings.LinearSlop); }