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