public override void PrepareForIteration(FP timestep)
{
this.effectiveMass = this.body1.invInertiaWorld + this.body2.invInertiaWorld;
this.softnessOverDt = this.softness / timestep;
this.effectiveMass.M11 = this.effectiveMass.M11 + this.softnessOverDt;
this.effectiveMass.M22 = this.effectiveMass.M22 + this.softnessOverDt;
this.effectiveMass.M33 = this.effectiveMass.M33 + this.softnessOverDt;
TSMatrix.Inverse(ref this.effectiveMass, out this.effectiveMass);
TSMatrix value;
TSMatrix.Multiply(ref this.initialOrientation1, ref this.initialOrientation2, out value);
TSMatrix.Transpose(ref value, out value);
TSMatrix tSMatrix = value * this.body2.invOrientation * this.body1.orientation;
FP fP = tSMatrix.M32 - tSMatrix.M23;
FP fP2 = tSMatrix.M13 - tSMatrix.M31;
FP fP3 = tSMatrix.M21 - tSMatrix.M12;
FP fP4 = TSMath.Sqrt(fP * fP + fP2 * fP2 + fP3 * fP3);
FP x = tSMatrix.M11 + tSMatrix.M22 + tSMatrix.M33;
FP value2 = FP.Atan2(fP4, x - 1);
TSVector value3 = new TSVector(fP, fP2, fP3) * value2;
bool flag = fP4 != FP.Zero;
if (flag)
{
value3 *= FP.One / fP4;
}
this.bias = value3 * this.biasFactor * (-FP.One / timestep);
bool flag2 = !this.body1.IsStatic;
if (flag2)
{
this.body1.angularVelocity += TSVector.Transform(this.accumulatedImpulse, this.body1.invInertiaWorld);
}
bool flag3 = !this.body2.IsStatic;
if (flag3)
{
this.body2.angularVelocity += TSVector.Transform(-FP.One * this.accumulatedImpulse, this.body2.invInertiaWorld);
}
}
/// <summary>
/// Returns the arc tan of coordinates x-y.
/// </summary>
public static FP Atan2(FP y, FP x)
{
return(FP.Atan2(y, x));
}