/// <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));
}
