public static TSVector3 NormalizeCheck(TSVector3 v)
{
//防止溢出
FP div = TSMath.Max(TSMath.Abs(v.x), TSMath.Abs(v.y), TSMath.Abs(v.z));
return(v / div);
}
public override int Prepare(ref TSBBox box)
{
bool flag = box.min.x < this.boundings.min.x;
if (flag)
{
this.minX = 0;
}
else
{
this.minX = (int)((long)FP.Floor((box.min.x - this.sphericalExpansion) / this.scaleX));
this.minX = (int)((long)TSMath.Max(this.minX, 0));
}
bool flag2 = box.max.x > this.boundings.max.x;
if (flag2)
{
this.maxX = this.heightsLength0 - 1;
}
else
{
this.maxX = (int)((long)FP.Ceiling((box.max.x + this.sphericalExpansion) / this.scaleX));
this.maxX = (int)((long)TSMath.Min(this.maxX, this.heightsLength0 - 1));
}
bool flag3 = box.min.z < this.boundings.min.z;
if (flag3)
{
this.minZ = 0;
}
else
{
this.minZ = (int)((long)FP.Floor((box.min.z - this.sphericalExpansion) / this.scaleZ));
this.minZ = (int)((long)TSMath.Max(this.minZ, 0));
}
bool flag4 = box.max.z > this.boundings.max.z;
if (flag4)
{
this.maxZ = this.heightsLength1 - 1;
}
else
{
this.maxZ = (int)((long)FP.Ceiling((box.max.z + this.sphericalExpansion) / this.scaleZ));
this.maxZ = (int)((long)TSMath.Min(this.maxZ, this.heightsLength1 - 1));
}
this.numX = this.maxX - this.minX;
this.numZ = this.maxZ - this.minZ;
return(this.numX * this.numZ * 2);
}
public override void Iterate()
{
bool flag = this.skipConstraint;
if (!flag)
{
FP x = TSVector.Dot(ref this.body1.linearVelocity, ref this.jacobian[0]);
x += TSVector.Dot(ref this.body2.linearVelocity, ref this.jacobian[1]);
FP y = this.accumulatedImpulse * this.softnessOverDt;
FP fP = -this.effectiveMass * (x + this.bias + y);
bool flag2 = this.behavior == SoftBody.Spring.DistanceBehavior.LimitMinimumDistance;
if (flag2)
{
FP y2 = this.accumulatedImpulse;
this.accumulatedImpulse = TSMath.Max(this.accumulatedImpulse + fP, 0);
fP = this.accumulatedImpulse - y2;
}
else
{
bool flag3 = this.behavior == SoftBody.Spring.DistanceBehavior.LimitMaximumDistance;
if (flag3)
{
FP y3 = this.accumulatedImpulse;
this.accumulatedImpulse = TSMath.Min(this.accumulatedImpulse + fP, 0);
fP = this.accumulatedImpulse - y3;
}
else
{
this.accumulatedImpulse += fP;
}
}
bool flag4 = !this.body1.isStatic;
if (flag4)
{
TSVector tSVector;
TSVector.Multiply(ref this.jacobian[0], fP * this.body1.inverseMass, out tSVector);
TSVector.Add(ref tSVector, ref this.body1.linearVelocity, out this.body1.linearVelocity);
}
bool flag5 = !this.body2.isStatic;
if (flag5)
{
TSVector tSVector;
TSVector.Multiply(ref this.jacobian[1], fP * this.body2.inverseMass, out tSVector);
TSVector.Add(ref tSVector, ref this.body2.linearVelocity, out this.body2.linearVelocity);
}
}
}
public void Initialize(RigidBody body1, RigidBody body2, ref TSVector point1, ref TSVector point2, ref TSVector n, FP penetration, bool newContact, ContactSettings settings)
{
this.body1 = body1;
this.body2 = body2;
this.normal = n;
this.normal.Normalize();
this.p1 = point1;
this.p2 = point2;
this.newContact = newContact;
TSVector.Subtract(ref this.p1, ref body1.position, out this.relativePos1);
TSVector.Subtract(ref this.p2, ref body2.position, out this.relativePos2);
TSVector.Transform(ref this.relativePos1, ref body1.invOrientation, out this.realRelPos1);
TSVector.Transform(ref this.relativePos2, ref body2.invOrientation, out this.realRelPos2);
this.initialPen = penetration;
this.penetration = penetration;
this.body1IsMassPoint = body1.isParticle;
this.body2IsMassPoint = body2.isParticle;
if (newContact)
{
this.treatBody1AsStatic = body1.isStatic;
this.treatBody2AsStatic = body2.isStatic;
this.accumulatedNormalImpulse = FP.Zero;
this.accumulatedTangentImpulse = FP.Zero;
this.lostSpeculativeBounce = FP.Zero;
switch (settings.MaterialCoefficientMixing)
{
case ContactSettings.MaterialCoefficientMixingType.TakeMaximum:
this.staticFriction = TSMath.Max(body1.material.staticFriction, body2.material.staticFriction);
this.dynamicFriction = TSMath.Max(body1.material.kineticFriction, body2.material.kineticFriction);
this.restitution = TSMath.Max(body1.material.restitution, body2.material.restitution);
break;
case ContactSettings.MaterialCoefficientMixingType.TakeMinimum:
this.staticFriction = TSMath.Min(body1.material.staticFriction, body2.material.staticFriction);
this.dynamicFriction = TSMath.Min(body1.material.kineticFriction, body2.material.kineticFriction);
this.restitution = TSMath.Min(body1.material.restitution, body2.material.restitution);
break;
case ContactSettings.MaterialCoefficientMixingType.UseAverage:
this.staticFriction = (body1.material.staticFriction + body2.material.staticFriction) / (2 * FP.One);
this.dynamicFriction = (body1.material.kineticFriction + body2.material.kineticFriction) / (2 * FP.One);
this.restitution = (body1.material.restitution + body2.material.restitution) / (2 * FP.One);
break;
}
}
this.settings = settings;
}
public override void Iterate()
{
bool flag = this.skipConstraint;
if (!flag)
{
FP x = this.body1.linearVelocity * this.jacobian[0] + this.body1.angularVelocity * this.jacobian[1] + this.body2.linearVelocity * this.jacobian[2] + this.body2.angularVelocity * this.jacobian[3];
FP y = this.accumulatedImpulse * this.softnessOverDt;
FP fP = -this.effectiveMass * (x + this.bias + y);
bool flag2 = this.behavior == PointPointDistance.DistanceBehavior.LimitMinimumDistance;
if (flag2)
{
FP y2 = this.accumulatedImpulse;
this.accumulatedImpulse = TSMath.Max(this.accumulatedImpulse + fP, 0);
fP = this.accumulatedImpulse - y2;
}
else
{
bool flag3 = this.behavior == PointPointDistance.DistanceBehavior.LimitMaximumDistance;
if (flag3)
{
FP y3 = this.accumulatedImpulse;
this.accumulatedImpulse = TSMath.Min(this.accumulatedImpulse + fP, 0);
fP = this.accumulatedImpulse - y3;
}
else
{
this.accumulatedImpulse += fP;
}
}
bool flag4 = !this.body1.isStatic;
if (flag4)
{
this.body1.linearVelocity += this.body1.inverseMass * fP * this.jacobian[0];
this.body1.angularVelocity += TSVector.Transform(fP * this.jacobian[1], this.body1.invInertiaWorld);
}
bool flag5 = !this.body2.isStatic;
if (flag5)
{
this.body2.linearVelocity += this.body2.inverseMass * fP * this.jacobian[2];
this.body2.angularVelocity += TSVector.Transform(fP * this.jacobian[3], this.body2.invInertiaWorld);
}
}
}
public virtual void MakeHull(ref List <TSVector> triangleList, int generationThreshold)
{
FP zero = FP.Zero;
bool flag = generationThreshold < 0;
if (flag)
{
generationThreshold = 4;
}
Stack <Shape.ClipTriangle> stack = new Stack <Shape.ClipTriangle>();
TSVector[] array = new TSVector[]
{
new TSVector(-1, 0, 0),
new TSVector(1, 0, 0),
new TSVector(0, -1, 0),
new TSVector(0, 1, 0),
new TSVector(0, 0, -1),
new TSVector(0, 0, 1)
};
int[,] array2 = new int[, ]
{
{
5,
1,
3
},
{
4,
3,
1
},
{
3,
4,
0
},
{
0,
5,
3
},
{
5,
2,
1
},
{
4,
1,
2
},
{
2,
0,
4
},
{
0,
2,
5
}
};
for (int i = 0; i < 8; i++)
{
stack.Push(new Shape.ClipTriangle
{
n1 = array[array2[i, 0]],
n2 = array[array2[i, 1]],
n3 = array[array2[i, 2]],
generation = 0
});
}
while (stack.Count > 0)
{
Shape.ClipTriangle clipTriangle = stack.Pop();
TSVector tSVector;
this.SupportMapping(ref clipTriangle.n1, out tSVector);
TSVector tSVector2;
this.SupportMapping(ref clipTriangle.n2, out tSVector2);
TSVector tSVector3;
this.SupportMapping(ref clipTriangle.n3, out tSVector3);
FP sqrMagnitude = (tSVector2 - tSVector).sqrMagnitude;
FP sqrMagnitude2 = (tSVector3 - tSVector2).sqrMagnitude;
FP sqrMagnitude3 = (tSVector - tSVector3).sqrMagnitude;
bool flag2 = TSMath.Max(TSMath.Max(sqrMagnitude, sqrMagnitude2), sqrMagnitude3) > zero && clipTriangle.generation < generationThreshold;
if (flag2)
{
Shape.ClipTriangle item = default(Shape.ClipTriangle);
Shape.ClipTriangle item2 = default(Shape.ClipTriangle);
Shape.ClipTriangle item3 = default(Shape.ClipTriangle);
Shape.ClipTriangle item4 = default(Shape.ClipTriangle);
item.generation = clipTriangle.generation + 1;
item2.generation = clipTriangle.generation + 1;
item3.generation = clipTriangle.generation + 1;
item4.generation = clipTriangle.generation + 1;
item.n1 = clipTriangle.n1;
item2.n2 = clipTriangle.n2;
item3.n3 = clipTriangle.n3;
TSVector tSVector4 = FP.Half * (clipTriangle.n1 + clipTriangle.n2);
tSVector4.Normalize();
item.n2 = tSVector4;
item2.n1 = tSVector4;
item4.n3 = tSVector4;
tSVector4 = FP.Half * (clipTriangle.n2 + clipTriangle.n3);
tSVector4.Normalize();
item2.n3 = tSVector4;
item3.n2 = tSVector4;
item4.n1 = tSVector4;
tSVector4 = FP.Half * (clipTriangle.n3 + clipTriangle.n1);
tSVector4.Normalize();
item.n3 = tSVector4;
item3.n1 = tSVector4;
item4.n2 = tSVector4;
stack.Push(item);
stack.Push(item2);
stack.Push(item3);
stack.Push(item4);
}
else
{
bool flag3 = ((tSVector3 - tSVector) % (tSVector2 - tSVector)).sqrMagnitude > TSMath.Epsilon;
if (flag3)
{
triangleList.Add(tSVector);
triangleList.Add(tSVector2);
triangleList.Add(tSVector3);
}
}
}
}
public static void Max(ref TSVector2 value1, ref TSVector2 value2, out TSVector2 result)
{
result.x = TSMath.Max(value1.x, value2.x);
result.y = TSMath.Max(value1.y, value2.y);
}
public static TSVector2 Max(TSVector2 value1, TSVector2 value2)
{
return(new TSVector2(
TSMath.Max(value1.x, value2.x),
TSMath.Max(value1.y, value2.y)));
}
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;
}
