/// <summary>This returns true if the position errors are within tolerance, allowing an early exit from the iteration loop.</summary> /// <param name="positions">The positions of the related bodies.</param> /// <returns> /// <c>true</c> if the position errors are within tolerance. /// </returns> internal override bool SolvePositionConstraints(Position[] positions) { var cA = positions[_tmp.IndexA].Point; var aA = positions[_tmp.IndexA].Angle; var cB = positions[_tmp.IndexB].Point; var aB = positions[_tmp.IndexB].Angle; var qA = new Rotation(aA); var qB = new Rotation(aB); var mA = _tmp.InverseMassA; var mB = _tmp.InverseMassB; var iA = _tmp.InverseInertiaA; var iB = _tmp.InverseInertiaB; // Compute fresh Jacobians var rA = qA * (_localAnchorA - _tmp.LocalCenterA); var rB = qB * (_localAnchorB - _tmp.LocalCenterB); // ReSharper disable RedundantCast Necessary for FarPhysics. var d = (Vector2)(cB - cA) + (rB - rA); // ReSharper restore RedundantCast var axis = qA * _localXAxisA; var a1 = Vector2Util.Cross(d + rA, axis); var a2 = Vector2Util.Cross(rB, axis); var perp = qA * _localYAxisA; var s1 = Vector2Util.Cross(d + rA, perp); var s2 = Vector2Util.Cross(rB, perp); Vector3 impulse; Vector2 c1; c1.X = Vector2Util.Dot(ref perp, ref d); c1.Y = aB - aA - _referenceAngle; var linearError = System.Math.Abs(c1.X); var angularError = System.Math.Abs(c1.Y); var active = false; var c2 = 0.0f; if (_enableLimit) { var translation = Vector2Util.Dot(ref axis, ref d); if (System.Math.Abs(_upperTranslation - _lowerTranslation) < 2.0f * Settings.LinearSlop) { // Prevent large angular corrections c2 = MathHelper.Clamp(translation, -Settings.MaxLinearCorrection, Settings.MaxLinearCorrection); linearError = System.Math.Max(linearError, System.Math.Abs(translation)); active = true; } else if (translation <= _lowerTranslation) { // Prevent large linear corrections and allow some slop. c2 = MathHelper.Clamp( translation - _lowerTranslation + Settings.LinearSlop, -Settings.MaxLinearCorrection, 0.0f); linearError = System.Math.Max(linearError, _lowerTranslation - translation); active = true; } else if (translation >= _upperTranslation) { // Prevent large linear corrections and allow some slop. c2 = MathHelper.Clamp( translation - _upperTranslation - Settings.LinearSlop, 0.0f, Settings.MaxLinearCorrection); linearError = System.Math.Max(linearError, translation - _upperTranslation); active = true; } } if (active) { var k11 = mA + mB + iA * s1 * s1 + iB * s2 * s2; var k12 = iA * s1 + iB * s2; var k13 = iA * s1 * a1 + iB * s2 * a2; var k22 = iA + iB; // ReSharper disable CompareOfFloatsByEqualityOperator Intentional. if (k22 == 0.0f) // ReSharper restore CompareOfFloatsByEqualityOperator { // For fixed rotation k22 = 1.0f; } var k23 = iA * a1 + iB * a2; var k33 = mA + mB + iA * a1 * a1 + iB * a2 * a2; Matrix33 k; Vector3Util.Set(out k.Column1, k11, k12, k13); Vector3Util.Set(out k.Column2, k12, k22, k23); Vector3Util.Set(out k.Column3, k13, k23, k33); Vector3 c; c.X = c1.X; c.Y = c1.Y; c.Z = c2; impulse = k.Solve33(-c); } else { var k11 = mA + mB + iA * s1 * s1 + iB * s2 * s2; var k12 = iA * s1 + iB * s2; var k22 = iA + iB; // ReSharper disable CompareOfFloatsByEqualityOperator Intentional. if (k22 == 0.0f) // ReSharper restore CompareOfFloatsByEqualityOperator { k22 = 1.0f; } Matrix22 k; Vector2Util.Set(out k.Column1, k11, k12); Vector2Util.Set(out k.Column2, k12, k22); var impulse1 = k.Solve(-c1); impulse.X = impulse1.X; impulse.Y = impulse1.Y; impulse.Z = 0.0f; } var p = impulse.X * perp + impulse.Z * axis; var lA = impulse.X * s1 + impulse.Y + impulse.Z * a1; var lB = impulse.X * s2 + impulse.Y + impulse.Z * a2; cA -= mA * p; aA -= iA * lA; cB += mB * p; aB += iB * lB; positions[_tmp.IndexA].Point = cA; positions[_tmp.IndexA].Angle = aA; positions[_tmp.IndexB].Point = cB; positions[_tmp.IndexB].Angle = aB; return(linearError <= (Settings.LinearSlop + Settings.Epsilon) && angularError <= (Settings.AngularSlop + Settings.Epsilon)); }