public override bool Raycast(RigidBody body, TSVector rayOrigin, TSVector rayDirection, out TSVector normal, out FP fraction)
        {
            fraction = FP.MaxValue;
            normal   = TSVector.zero;
            bool flag = !body.BoundingBox.RayIntersect(ref rayOrigin, ref rayDirection);
            bool result;

            if (flag)
            {
                result = false;
            }
            else
            {
                bool flag2 = body.Shape is Multishape;
                if (flag2)
                {
                    Multishape multishape = (body.Shape as Multishape).RequestWorkingClone();
                    bool       flag3      = false;
                    TSVector   tSVector;
                    TSVector.Subtract(ref rayOrigin, ref body.position, out tSVector);
                    TSVector.Transform(ref tSVector, ref body.invOrientation, out tSVector);
                    TSVector tSVector2;
                    TSVector.Transform(ref rayDirection, ref body.invOrientation, out tSVector2);
                    int num = multishape.Prepare(ref tSVector, ref tSVector2);
                    for (int i = 0; i < num; i++)
                    {
                        multishape.SetCurrentShape(i);
                        FP       fP;
                        TSVector tSVector3;
                        bool     flag4 = GJKCollide.Raycast(multishape, ref body.orientation, ref body.invOrientation, ref body.position, ref rayOrigin, ref rayDirection, out fP, out tSVector3);
                        if (flag4)
                        {
                            bool flag5 = fP < fraction;
                            if (flag5)
                            {
                                bool flag6 = this.useTerrainNormal && multishape is TerrainShape;
                                if (flag6)
                                {
                                    (multishape as TerrainShape).CollisionNormal(out tSVector3);
                                    TSVector.Transform(ref tSVector3, ref body.orientation, out tSVector3);
                                    tSVector3.Negate();
                                }
                                else
                                {
                                    bool flag7 = this.useTriangleMeshNormal && multishape is TriangleMeshShape;
                                    if (flag7)
                                    {
                                        (multishape as TriangleMeshShape).CollisionNormal(out tSVector3);
                                        TSVector.Transform(ref tSVector3, ref body.orientation, out tSVector3);
                                        tSVector3.Negate();
                                    }
                                }
                                normal   = tSVector3;
                                fraction = fP;
                                flag3    = true;
                            }
                        }
                    }
                    multishape.ReturnWorkingClone();
                    result = flag3;
                }
                else
                {
                    result = GJKCollide.Raycast(body.Shape, ref body.orientation, ref body.invOrientation, ref body.position, ref rayOrigin, ref rayDirection, out fraction, out normal);
                }
            }
            return(result);
        }
示例#2
0
        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;
        }
        public override bool Raycast(TSVector rayOrigin, TSVector rayDirection, RaycastCallback raycast, out RigidBody body, out TSVector normal, out FP fraction)
        {
            body     = null;
            normal   = TSVector.zero;
            fraction = FP.MaxValue;
            bool result = false;

            foreach (IBroadphaseEntity current in this.bodyList)
            {
                bool flag = current is SoftBody;
                if (flag)
                {
                    SoftBody softBody = current as SoftBody;
                    foreach (RigidBody current2 in softBody.VertexBodies)
                    {
                        TSVector tSVector;
                        FP       fP;
                        bool     flag2 = this.Raycast(current2, rayOrigin, rayDirection, out tSVector, out fP);
                        if (flag2)
                        {
                            bool flag3 = fP < fraction && (raycast == null || raycast(current2, tSVector, fP));
                            if (flag3)
                            {
                                body     = current2;
                                normal   = tSVector;
                                fraction = fP;
                                result   = true;
                            }
                        }
                    }
                }
                else
                {
                    RigidBody rigidBody = current as RigidBody;
                    TSVector  tSVector;
                    FP        fP;
                    bool      flag4 = this.Raycast(rigidBody, rayOrigin, rayDirection, out tSVector, out fP);
                    if (flag4)
                    {
                        bool flag5 = fP < fraction && (raycast == null || raycast(rigidBody, tSVector, fP));
                        if (flag5)
                        {
                            body     = rigidBody;
                            normal   = tSVector;
                            fraction = fP;
                            result   = true;
                        }
                    }
                }
            }
            return(result);
        }
示例#4
0
        public void Iterate()
        {
            bool flag = this.treatBody1AsStatic && this.treatBody2AsStatic;

            if (!flag)
            {
                FP   fP    = this.body2.linearVelocity.x - this.body1.linearVelocity.x;
                FP   fP2   = this.body2.linearVelocity.y - this.body1.linearVelocity.y;
                FP   fP3   = this.body2.linearVelocity.z - this.body1.linearVelocity.z;
                bool flag2 = !this.body1IsMassPoint;
                if (flag2)
                {
                    fP  = fP - this.body1.angularVelocity.y * this.relativePos1.z + this.body1.angularVelocity.z * this.relativePos1.y;
                    fP2 = fP2 - this.body1.angularVelocity.z * this.relativePos1.x + this.body1.angularVelocity.x * this.relativePos1.z;
                    fP3 = fP3 - this.body1.angularVelocity.x * this.relativePos1.y + this.body1.angularVelocity.y * this.relativePos1.x;
                }
                bool flag3 = !this.body2IsMassPoint;
                if (flag3)
                {
                    fP  = fP + this.body2.angularVelocity.y * this.relativePos2.z - this.body2.angularVelocity.z * this.relativePos2.y;
                    fP2 = fP2 + this.body2.angularVelocity.z * this.relativePos2.x - this.body2.angularVelocity.x * this.relativePos2.z;
                    fP3 = fP3 + this.body2.angularVelocity.x * this.relativePos2.y - this.body2.angularVelocity.y * this.relativePos2.x;
                }
                bool flag4 = fP * fP + fP2 * fP2 + fP3 * fP3 < this.settings.minVelocity * this.settings.minVelocity;
                if (!flag4)
                {
                    FP x   = this.normal.x * fP + this.normal.y * fP2 + this.normal.z * fP3;
                    FP y   = this.massNormal * (-x + this.restitutionBias + this.speculativeVelocity);
                    FP fP4 = this.accumulatedNormalImpulse;
                    this.accumulatedNormalImpulse = fP4 + y;
                    bool flag5 = this.accumulatedNormalImpulse < FP.Zero;
                    if (flag5)
                    {
                        this.accumulatedNormalImpulse = FP.Zero;
                    }
                    y = this.accumulatedNormalImpulse - fP4;
                    FP x2  = fP * this.tangent.x + fP2 * this.tangent.y + fP3 * this.tangent.z;
                    FP fP5 = this.friction * this.accumulatedNormalImpulse;
                    FP y2  = this.massTangent * -x2;
                    FP fP6 = this.accumulatedTangentImpulse;
                    this.accumulatedTangentImpulse = fP6 + y2;
                    bool flag6 = this.accumulatedTangentImpulse < -fP5;
                    if (flag6)
                    {
                        this.accumulatedTangentImpulse = -fP5;
                    }
                    else
                    {
                        bool flag7 = this.accumulatedTangentImpulse > fP5;
                        if (flag7)
                        {
                            this.accumulatedTangentImpulse = fP5;
                        }
                    }
                    y2 = this.accumulatedTangentImpulse - fP6;
                    TSVector tSVector;
                    tSVector.x = this.normal.x * y + this.tangent.x * y2;
                    tSVector.y = this.normal.y * y + this.tangent.y * y2;
                    tSVector.z = this.normal.z * y + this.tangent.z * y2;
                    bool flag8 = !this.treatBody1AsStatic;
                    if (flag8)
                    {
                        RigidBody expr_4F9_cp_0_cp_0 = this.body1;
                        expr_4F9_cp_0_cp_0.linearVelocity.x = expr_4F9_cp_0_cp_0.linearVelocity.x - tSVector.x * this.body1.inverseMass;
                        RigidBody expr_530_cp_0_cp_0 = this.body1;
                        expr_530_cp_0_cp_0.linearVelocity.y = expr_530_cp_0_cp_0.linearVelocity.y - tSVector.y * this.body1.inverseMass;
                        RigidBody expr_567_cp_0_cp_0 = this.body1;
                        expr_567_cp_0_cp_0.linearVelocity.z = expr_567_cp_0_cp_0.linearVelocity.z - tSVector.z * this.body1.inverseMass;
                        bool flag9 = !this.body1IsMassPoint;
                        if (flag9)
                        {
                            FP        x3 = this.relativePos1.y * tSVector.z - this.relativePos1.z * tSVector.y;
                            FP        x4 = this.relativePos1.z * tSVector.x - this.relativePos1.x * tSVector.z;
                            FP        x5 = this.relativePos1.x * tSVector.y - this.relativePos1.y * tSVector.x;
                            FP        y3 = x3 * this.body1.invInertiaWorld.M11 + x4 * this.body1.invInertiaWorld.M21 + x5 * this.body1.invInertiaWorld.M31;
                            FP        y4 = x3 * this.body1.invInertiaWorld.M12 + x4 * this.body1.invInertiaWorld.M22 + x5 * this.body1.invInertiaWorld.M32;
                            FP        y5 = x3 * this.body1.invInertiaWorld.M13 + x4 * this.body1.invInertiaWorld.M23 + x5 * this.body1.invInertiaWorld.M33;
                            RigidBody expr_743_cp_0_cp_0 = this.body1;
                            expr_743_cp_0_cp_0.angularVelocity.x = expr_743_cp_0_cp_0.angularVelocity.x - y3;
                            RigidBody expr_765_cp_0_cp_0 = this.body1;
                            expr_765_cp_0_cp_0.angularVelocity.y = expr_765_cp_0_cp_0.angularVelocity.y - y4;
                            RigidBody expr_787_cp_0_cp_0 = this.body1;
                            expr_787_cp_0_cp_0.angularVelocity.z = expr_787_cp_0_cp_0.angularVelocity.z - y5;
                        }
                    }
                    bool flag10 = !this.treatBody2AsStatic;
                    if (flag10)
                    {
                        RigidBody expr_7BE_cp_0_cp_0 = this.body2;
                        expr_7BE_cp_0_cp_0.linearVelocity.x = expr_7BE_cp_0_cp_0.linearVelocity.x + tSVector.x * this.body2.inverseMass;
                        RigidBody expr_7F5_cp_0_cp_0 = this.body2;
                        expr_7F5_cp_0_cp_0.linearVelocity.y = expr_7F5_cp_0_cp_0.linearVelocity.y + tSVector.y * this.body2.inverseMass;
                        RigidBody expr_82C_cp_0_cp_0 = this.body2;
                        expr_82C_cp_0_cp_0.linearVelocity.z = expr_82C_cp_0_cp_0.linearVelocity.z + tSVector.z * this.body2.inverseMass;
                        bool flag11 = !this.body2IsMassPoint;
                        if (flag11)
                        {
                            FP        x6 = this.relativePos2.y * tSVector.z - this.relativePos2.z * tSVector.y;
                            FP        x7 = this.relativePos2.z * tSVector.x - this.relativePos2.x * tSVector.z;
                            FP        x8 = this.relativePos2.x * tSVector.y - this.relativePos2.y * tSVector.x;
                            FP        y6 = x6 * this.body2.invInertiaWorld.M11 + x7 * this.body2.invInertiaWorld.M21 + x8 * this.body2.invInertiaWorld.M31;
                            FP        y7 = x6 * this.body2.invInertiaWorld.M12 + x7 * this.body2.invInertiaWorld.M22 + x8 * this.body2.invInertiaWorld.M32;
                            FP        y8 = x6 * this.body2.invInertiaWorld.M13 + x7 * this.body2.invInertiaWorld.M23 + x8 * this.body2.invInertiaWorld.M33;
                            RigidBody expr_A08_cp_0_cp_0 = this.body2;
                            expr_A08_cp_0_cp_0.angularVelocity.x = expr_A08_cp_0_cp_0.angularVelocity.x + y6;
                            RigidBody expr_A2A_cp_0_cp_0 = this.body2;
                            expr_A2A_cp_0_cp_0.angularVelocity.y = expr_A2A_cp_0_cp_0.angularVelocity.y + y7;
                            RigidBody expr_A4C_cp_0_cp_0 = this.body2;
                            expr_A4C_cp_0_cp_0.angularVelocity.z = expr_A4C_cp_0_cp_0.angularVelocity.z + y8;
                        }
                    }
                }
            }
        }