public void PrepareForIteration(FP timestep) { FP fP = this.body2.angularVelocity.y * this.relativePos2.z - this.body2.angularVelocity.z * this.relativePos2.y + this.body2.linearVelocity.x; FP fP2 = this.body2.angularVelocity.z * this.relativePos2.x - this.body2.angularVelocity.x * this.relativePos2.z + this.body2.linearVelocity.y; FP fP3 = this.body2.angularVelocity.x * this.relativePos2.y - this.body2.angularVelocity.y * this.relativePos2.x + this.body2.linearVelocity.z; fP = fP - this.body1.angularVelocity.y * this.relativePos1.z + this.body1.angularVelocity.z * this.relativePos1.y - this.body1.linearVelocity.x; fP2 = fP2 - this.body1.angularVelocity.z * this.relativePos1.x + this.body1.angularVelocity.x * this.relativePos1.z - this.body1.linearVelocity.y; fP3 = fP3 - this.body1.angularVelocity.x * this.relativePos1.y + this.body1.angularVelocity.y * this.relativePos1.x - this.body1.linearVelocity.z; FP fP4 = FP.Zero; TSVector zero = TSVector.zero; bool flag = !this.treatBody1AsStatic; if (flag) { fP4 += this.body1.inverseMass; bool flag2 = !this.body1IsMassPoint; if (flag2) { zero.x = this.relativePos1.y * this.normal.z - this.relativePos1.z * this.normal.y; zero.y = this.relativePos1.z * this.normal.x - this.relativePos1.x * this.normal.z; zero.z = this.relativePos1.x * this.normal.y - this.relativePos1.y * this.normal.x; FP x = zero.x * this.body1.invInertiaWorld.M11 + zero.y * this.body1.invInertiaWorld.M21 + zero.z * this.body1.invInertiaWorld.M31; FP y = zero.x * this.body1.invInertiaWorld.M12 + zero.y * this.body1.invInertiaWorld.M22 + zero.z * this.body1.invInertiaWorld.M32; FP z = zero.x * this.body1.invInertiaWorld.M13 + zero.y * this.body1.invInertiaWorld.M23 + zero.z * this.body1.invInertiaWorld.M33; zero.x = x; zero.y = y; zero.z = z; x = zero.y * this.relativePos1.z - zero.z * this.relativePos1.y; y = zero.z * this.relativePos1.x - zero.x * this.relativePos1.z; z = zero.x * this.relativePos1.y - zero.y * this.relativePos1.x; zero.x = x; zero.y = y; zero.z = z; } } TSVector zero2 = TSVector.zero; bool flag3 = !this.treatBody2AsStatic; if (flag3) { fP4 += this.body2.inverseMass; bool flag4 = !this.body2IsMassPoint; if (flag4) { zero2.x = this.relativePos2.y * this.normal.z - this.relativePos2.z * this.normal.y; zero2.y = this.relativePos2.z * this.normal.x - this.relativePos2.x * this.normal.z; zero2.z = this.relativePos2.x * this.normal.y - this.relativePos2.y * this.normal.x; FP x2 = zero2.x * this.body2.invInertiaWorld.M11 + zero2.y * this.body2.invInertiaWorld.M21 + zero2.z * this.body2.invInertiaWorld.M31; FP y2 = zero2.x * this.body2.invInertiaWorld.M12 + zero2.y * this.body2.invInertiaWorld.M22 + zero2.z * this.body2.invInertiaWorld.M32; FP z2 = zero2.x * this.body2.invInertiaWorld.M13 + zero2.y * this.body2.invInertiaWorld.M23 + zero2.z * this.body2.invInertiaWorld.M33; zero2.x = x2; zero2.y = y2; zero2.z = z2; x2 = zero2.y * this.relativePos2.z - zero2.z * this.relativePos2.y; y2 = zero2.z * this.relativePos2.x - zero2.x * this.relativePos2.z; z2 = zero2.x * this.relativePos2.y - zero2.y * this.relativePos2.x; zero2.x = x2; zero2.y = y2; zero2.z = z2; } } bool flag5 = !this.treatBody1AsStatic; if (flag5) { fP4 += zero.x * this.normal.x + zero.y * this.normal.y + zero.z * this.normal.z; } bool flag6 = !this.treatBody2AsStatic; if (flag6) { fP4 += zero2.x * this.normal.x + zero2.y * this.normal.y + zero2.z * this.normal.z; } this.massNormal = FP.One / fP4; FP fP5 = fP * this.normal.x + fP2 * this.normal.y + fP3 * this.normal.z; this.tangent.x = fP - this.normal.x * fP5; this.tangent.y = fP2 - this.normal.y * fP5; this.tangent.z = fP3 - this.normal.z * fP5; fP5 = this.tangent.x * this.tangent.x + this.tangent.y * this.tangent.y + this.tangent.z * this.tangent.z; bool flag7 = fP5 != FP.Zero; if (flag7) { fP5 = FP.Sqrt(fP5); this.tangent.x = this.tangent.x / fP5; this.tangent.y = this.tangent.y / fP5; this.tangent.z = this.tangent.z / fP5; } FP fP6 = FP.Zero; bool flag8 = this.treatBody1AsStatic; if (flag8) { zero.MakeZero(); } else { fP6 += this.body1.inverseMass; bool flag9 = !this.body1IsMassPoint; if (flag9) { zero.x = this.relativePos1.y * this.tangent.z - this.relativePos1.z * this.tangent.y; zero.y = this.relativePos1.z * this.tangent.x - this.relativePos1.x * this.tangent.z; zero.z = this.relativePos1.x * this.tangent.y - this.relativePos1.y * this.tangent.x; FP x3 = zero.x * this.body1.invInertiaWorld.M11 + zero.y * this.body1.invInertiaWorld.M21 + zero.z * this.body1.invInertiaWorld.M31; FP y3 = zero.x * this.body1.invInertiaWorld.M12 + zero.y * this.body1.invInertiaWorld.M22 + zero.z * this.body1.invInertiaWorld.M32; FP z3 = zero.x * this.body1.invInertiaWorld.M13 + zero.y * this.body1.invInertiaWorld.M23 + zero.z * this.body1.invInertiaWorld.M33; zero.x = x3; zero.y = y3; zero.z = z3; x3 = zero.y * this.relativePos1.z - zero.z * this.relativePos1.y; y3 = zero.z * this.relativePos1.x - zero.x * this.relativePos1.z; z3 = zero.x * this.relativePos1.y - zero.y * this.relativePos1.x; zero.x = x3; zero.y = y3; zero.z = z3; } } bool flag10 = this.treatBody2AsStatic; if (flag10) { zero2.MakeZero(); } else { fP6 += this.body2.inverseMass; bool flag11 = !this.body2IsMassPoint; if (flag11) { zero2.x = this.relativePos2.y * this.tangent.z - this.relativePos2.z * this.tangent.y; zero2.y = this.relativePos2.z * this.tangent.x - this.relativePos2.x * this.tangent.z; zero2.z = this.relativePos2.x * this.tangent.y - this.relativePos2.y * this.tangent.x; FP x4 = zero2.x * this.body2.invInertiaWorld.M11 + zero2.y * this.body2.invInertiaWorld.M21 + zero2.z * this.body2.invInertiaWorld.M31; FP y4 = zero2.x * this.body2.invInertiaWorld.M12 + zero2.y * this.body2.invInertiaWorld.M22 + zero2.z * this.body2.invInertiaWorld.M32; FP z4 = zero2.x * this.body2.invInertiaWorld.M13 + zero2.y * this.body2.invInertiaWorld.M23 + zero2.z * this.body2.invInertiaWorld.M33; zero2.x = x4; zero2.y = y4; zero2.z = z4; x4 = zero2.y * this.relativePos2.z - zero2.z * this.relativePos2.y; y4 = zero2.z * this.relativePos2.x - zero2.x * this.relativePos2.z; z4 = zero2.x * this.relativePos2.y - zero2.y * this.relativePos2.x; zero2.x = x4; zero2.y = y4; zero2.z = z4; } } bool flag12 = !this.treatBody1AsStatic; if (flag12) { fP6 += TSVector.Dot(ref zero, ref this.tangent); } bool flag13 = !this.treatBody2AsStatic; if (flag13) { fP6 += TSVector.Dot(ref zero2, ref this.tangent); } this.massTangent = FP.One / fP6; this.restitutionBias = this.lostSpeculativeBounce; this.speculativeVelocity = FP.Zero; FP y5 = this.normal.x * fP + this.normal.y * fP2 + this.normal.z * fP3; bool flag14 = this.Penetration > this.settings.allowedPenetration; if (flag14) { this.restitutionBias = this.settings.bias * (FP.One / timestep) * TSMath.Max(FP.Zero, this.Penetration - this.settings.allowedPenetration); this.restitutionBias = TSMath.Clamp(this.restitutionBias, FP.Zero, this.settings.maximumBias); } FP y6 = timestep / this.lastTimeStep; this.accumulatedNormalImpulse *= y6; this.accumulatedTangentImpulse *= y6; FP y7 = -(this.tangent.x * fP + this.tangent.y * fP2 + this.tangent.z * fP3); FP x5 = this.massTangent * y7; FP y8 = -this.staticFriction * this.accumulatedNormalImpulse; bool flag15 = x5 < y8; if (flag15) { this.friction = this.dynamicFriction; } else { this.friction = this.staticFriction; } this.restitutionBias = TSMath.Max(-this.restitution * y5, this.restitutionBias); bool flag16 = this.penetration < -this.settings.allowedPenetration; if (flag16) { this.speculativeVelocity = this.penetration / timestep; this.lostSpeculativeBounce = this.restitutionBias; this.restitutionBias = FP.Zero; } else { this.lostSpeculativeBounce = FP.Zero; } TSVector tSVector; tSVector.x = this.normal.x * this.accumulatedNormalImpulse + this.tangent.x * this.accumulatedTangentImpulse; tSVector.y = this.normal.y * this.accumulatedNormalImpulse + this.tangent.y * this.accumulatedTangentImpulse; tSVector.z = this.normal.z * this.accumulatedNormalImpulse + this.tangent.z * this.accumulatedTangentImpulse; bool flag17 = !this.treatBody1AsStatic; if (flag17) { RigidBody expr_13D4_cp_0_cp_0 = this.body1; expr_13D4_cp_0_cp_0.linearVelocity.x = expr_13D4_cp_0_cp_0.linearVelocity.x - tSVector.x * this.body1.inverseMass; RigidBody expr_140B_cp_0_cp_0 = this.body1; expr_140B_cp_0_cp_0.linearVelocity.y = expr_140B_cp_0_cp_0.linearVelocity.y - tSVector.y * this.body1.inverseMass; RigidBody expr_1442_cp_0_cp_0 = this.body1; expr_1442_cp_0_cp_0.linearVelocity.z = expr_1442_cp_0_cp_0.linearVelocity.z - tSVector.z * this.body1.inverseMass; bool flag18 = !this.body1IsMassPoint; if (flag18) { FP x6 = this.relativePos1.y * tSVector.z - this.relativePos1.z * tSVector.y; FP x7 = this.relativePos1.z * tSVector.x - this.relativePos1.x * tSVector.z; FP x8 = this.relativePos1.x * tSVector.y - this.relativePos1.y * tSVector.x; FP y9 = x6 * this.body1.invInertiaWorld.M11 + x7 * this.body1.invInertiaWorld.M21 + x8 * this.body1.invInertiaWorld.M31; FP y10 = x6 * this.body1.invInertiaWorld.M12 + x7 * this.body1.invInertiaWorld.M22 + x8 * this.body1.invInertiaWorld.M32; FP y11 = x6 * this.body1.invInertiaWorld.M13 + x7 * this.body1.invInertiaWorld.M23 + x8 * this.body1.invInertiaWorld.M33; RigidBody expr_161E_cp_0_cp_0 = this.body1; expr_161E_cp_0_cp_0.angularVelocity.x = expr_161E_cp_0_cp_0.angularVelocity.x - y9; RigidBody expr_1640_cp_0_cp_0 = this.body1; expr_1640_cp_0_cp_0.angularVelocity.y = expr_1640_cp_0_cp_0.angularVelocity.y - y10; RigidBody expr_1662_cp_0_cp_0 = this.body1; expr_1662_cp_0_cp_0.angularVelocity.z = expr_1662_cp_0_cp_0.angularVelocity.z - y11; } } bool flag19 = !this.treatBody2AsStatic; if (flag19) { RigidBody expr_1699_cp_0_cp_0 = this.body2; expr_1699_cp_0_cp_0.linearVelocity.x = expr_1699_cp_0_cp_0.linearVelocity.x + tSVector.x * this.body2.inverseMass; RigidBody expr_16D0_cp_0_cp_0 = this.body2; expr_16D0_cp_0_cp_0.linearVelocity.y = expr_16D0_cp_0_cp_0.linearVelocity.y + tSVector.y * this.body2.inverseMass; RigidBody expr_1707_cp_0_cp_0 = this.body2; expr_1707_cp_0_cp_0.linearVelocity.z = expr_1707_cp_0_cp_0.linearVelocity.z + tSVector.z * this.body2.inverseMass; bool flag20 = !this.body2IsMassPoint; if (flag20) { FP x9 = this.relativePos2.y * tSVector.z - this.relativePos2.z * tSVector.y; FP x10 = this.relativePos2.z * tSVector.x - this.relativePos2.x * tSVector.z; FP x11 = this.relativePos2.x * tSVector.y - this.relativePos2.y * tSVector.x; FP y12 = x9 * this.body2.invInertiaWorld.M11 + x10 * this.body2.invInertiaWorld.M21 + x11 * this.body2.invInertiaWorld.M31; FP y13 = x9 * this.body2.invInertiaWorld.M12 + x10 * this.body2.invInertiaWorld.M22 + x11 * this.body2.invInertiaWorld.M32; FP y14 = x9 * this.body2.invInertiaWorld.M13 + x10 * this.body2.invInertiaWorld.M23 + x11 * this.body2.invInertiaWorld.M33; RigidBody expr_18E3_cp_0_cp_0 = this.body2; expr_18E3_cp_0_cp_0.angularVelocity.x = expr_18E3_cp_0_cp_0.angularVelocity.x + y12; RigidBody expr_1905_cp_0_cp_0 = this.body2; expr_1905_cp_0_cp_0.angularVelocity.y = expr_1905_cp_0_cp_0.angularVelocity.y + y13; RigidBody expr_1927_cp_0_cp_0 = this.body2; expr_1927_cp_0_cp_0.angularVelocity.z = expr_1927_cp_0_cp_0.angularVelocity.z + y14; } } this.lastTimeStep = timestep; this.newContact = false; }