public static FP Distance(TSVector2 value1, TSVector2 value2)
{
FP fP;
TSVector2.DistanceSquared(ref value1, ref value2, out fP);
return(FP.Sqrt(fP));
}
/// <summary>
/// Returns FP precison distanve between two vectors
/// </summary>
/// <param name="value1">
/// A <see cref="TSVector2"/>
/// </param>
/// <param name="value2">
/// A <see cref="TSVector2"/>
/// </param>
/// <returns>
/// A <see cref="System.Single"/>
/// </returns>
public static FP Distance(TSVector2 value1, TSVector2 value2)
{
FP result;
DistanceSquared(ref value1, ref value2, out result);
return((FP)FP.Sqrt(result));
}
public override void SupportMapping(ref TSVector direction, out TSVector result)
{
FP fP = FP.Sqrt(direction.x * direction.x + direction.z * direction.z);
bool flag = direction.y > direction.magnitude * this.sina;
if (flag)
{
result.x = FP.Zero;
result.y = 2 * FP.One / 3 * this.height;
result.z = FP.Zero;
}
else
{
bool flag2 = fP > FP.Zero;
if (flag2)
{
result.x = this.radius * direction.x / fP;
result.y = -(FP.One / 3) * this.height;
result.z = this.radius * direction.z / fP;
}
else
{
result.x = FP.Zero;
result.y = -(FP.One / 3) * this.height;
result.z = FP.Zero;
}
}
}
public override void SupportMapping(ref TSVector direction, out TSVector result)
{
FP fP = FP.Sqrt(direction.x * direction.x + direction.z * direction.z);
bool flag = FP.Abs(direction.y) > FP.Zero;
if (flag)
{
TSVector tSVector;
TSVector.Normalize(ref direction, out tSVector);
TSVector.Multiply(ref tSVector, this.radius, out result);
result.y += FP.Sign(direction.y) * FP.Half * this.length;
}
else
{
bool flag2 = fP > FP.Zero;
if (flag2)
{
result.x = direction.x / fP * this.radius;
result.y = FP.Zero;
result.z = direction.z / fP * this.radius;
}
else
{
result.x = FP.Zero;
result.y = FP.Zero;
result.z = FP.Zero;
}
}
}
public static TSQuaternion FromToRotation(TSVector fromVector, TSVector toVector)
{
TSVector tSVector = TSVector.Cross(fromVector, toVector);
TSQuaternion result = new TSQuaternion(tSVector.x, tSVector.y, tSVector.z, TSVector.Dot(fromVector, toVector));
result.w += FP.Sqrt(fromVector.sqrMagnitude * toVector.sqrMagnitude);
result.Normalize();
return(result);
}
public static void Normalize(ref TSVector2 value, out TSVector2 result)
{
FP fP;
TSVector2.DistanceSquared(ref value, ref TSVector2.zeroVector, out fP);
fP = 1f / FP.Sqrt(fP);
result.x = value.x * fP;
result.y = value.y * fP;
}
/// <summary>
/// Normalizes the given vector.
/// </summary>
/// <param name="value">The vector which should be normalized.</param>
/// <param name="result">A normalized vector.</param>
public static void Normalize(ref TSVector value, out TSVector result)
{
FP num2 = ((value.x * value.x) + (value.y * value.y)) + (value.z * value.z);
FP num = FP.One / FP.Sqrt(num2);
result.x = value.x * num;
result.y = value.y * num;
result.z = value.z * num;
}
/// <summary>
/// Normalizes this vector.
/// </summary>
public void Normalize()
{
FP num2 = ((this.x * this.x) + (this.y * this.y)) + (this.z * this.z);
FP num = FP.One / FP.Sqrt(num2);
this.x *= num;
this.y *= num;
this.z *= num;
}
public void Normalize()
{
FP fP = this.x * this.x + this.y * this.y + this.z * this.z;
FP fP2 = FP.One / FP.Sqrt(fP);
this.x *= fP2;
this.y *= fP2;
this.z *= fP2;
}
public static void Normalize(ref TSVector value, out TSVector result)
{
FP fP = value.x * value.x + value.y * value.y + value.z * value.z;
FP fP2 = FP.One / FP.Sqrt(fP);
result.x = value.x * fP2;
result.y = value.y * fP2;
result.z = value.z * fP2;
}
public static void Normalize(ref TSVector2 value, out TSVector2 result)
{
TSVector2 value1 = NormalizeCheck(value);
FP num2 = (value1.x * value.x) + (value1.y * value1.y);
FP num = FP.One / FP.Sqrt(num2);
result.x = value1.x * num;
result.y = value1.y * num;
}
public static void Normalize(ref TSVector2 value, out TSVector2 result)
{
FP factor;
DistanceSquared(ref value, ref zeroVector, out factor);
factor = 1f / (FP)FP.Sqrt(factor);
result.x = value.x * factor;
result.y = value.y * factor;
}
/// <summary>
/// Sets the length of the quaternion to one.
/// </summary>
#region public void Normalize()
public void Normalize()
{
FP num2 = (((this.x * this.x) + (this.y * this.y)) + (this.z * this.z)) + (this.w * this.w);
FP num = 1 / (FP.Sqrt(num2));
this.x *= num;
this.y *= num;
this.z *= num;
this.w *= num;
}
public void Normalize()
{
FP fP = this.x * this.x + this.y * this.y + this.z * this.z + this.w * this.w;
FP fP2 = 1 / FP.Sqrt(fP);
this.x *= fP2;
this.y *= fP2;
this.z *= fP2;
this.w *= fP2;
}
public static TSQuaternion FromToRotation(TSVector fromVector, TSVector toVector)
{
TSVector w = TSVector.Cross(fromVector, toVector);
TSQuaternion q = new TSQuaternion(w.x, w.y, w.z, TSVector.Dot(fromVector, toVector));
q.w += FP.Sqrt(fromVector.sqrMagnitude * toVector.sqrMagnitude);
q.Normalize();
return(q);
}
public void Normalize()
{
this = NormalizeCheck(this);
FP num2 = ((this.x * this.x) + (this.y * this.y)) + (this.z * this.z) + (this.w * this.w);
FP num = FP.One / FP.Sqrt(num2);
this.x *= num;
this.y *= num;
this.z *= num;
this.w *= num;
}
public static void Normalize(ref TSVector4 value, out TSVector4 result)
{
TSVector4 value1 = NormalizeCheck(value);
FP num2 = ((value1.x * value1.x) + (value1.y * value1.y)) + (value1.z * value1.z) + (value1.w * value1.w);
FP num = FP.One / FP.Sqrt(num2);
result.x = value1.x * num;
result.y = value1.y * num;
result.z = value1.z * num;
result.w = value1.w * num;
}
public static void CreateFromMatrix(ref TSMatrix matrix, out TSQuaternion result)
{
FP fP = matrix.M11 + matrix.M22 + matrix.M33;
bool flag = fP > FP.Zero;
if (flag)
{
FP fP2 = FP.Sqrt(fP + FP.One);
result.w = fP2 * FP.Half;
fP2 = FP.Half / fP2;
result.x = (matrix.M23 - matrix.M32) * fP2;
result.y = (matrix.M31 - matrix.M13) * fP2;
result.z = (matrix.M12 - matrix.M21) * fP2;
}
else
{
bool flag2 = matrix.M11 >= matrix.M22 && matrix.M11 >= matrix.M33;
if (flag2)
{
FP fP3 = FP.Sqrt(FP.One + matrix.M11 - matrix.M22 - matrix.M33);
FP fP4 = FP.Half / fP3;
result.x = FP.Half * fP3;
result.y = (matrix.M12 + matrix.M21) * fP4;
result.z = (matrix.M13 + matrix.M31) * fP4;
result.w = (matrix.M23 - matrix.M32) * fP4;
}
else
{
bool flag3 = matrix.M22 > matrix.M33;
if (flag3)
{
FP fP5 = FP.Sqrt(FP.One + matrix.M22 - matrix.M11 - matrix.M33);
FP fP6 = FP.Half / fP5;
result.x = (matrix.M21 + matrix.M12) * fP6;
result.y = FP.Half * fP5;
result.z = (matrix.M32 + matrix.M23) * fP6;
result.w = (matrix.M31 - matrix.M13) * fP6;
}
else
{
FP fP7 = FP.Sqrt(FP.One + matrix.M33 - matrix.M11 - matrix.M22);
FP fP8 = FP.Half / fP7;
result.x = (matrix.M31 + matrix.M13) * fP8;
result.y = (matrix.M32 + matrix.M23) * fP8;
result.z = FP.Half * fP7;
result.w = (matrix.M12 - matrix.M21) * fP8;
}
}
}
}
public override void SupportMapping(ref TSVector direction, out TSVector result)
{
FP fP = FP.Sqrt(direction.x * direction.x + direction.z * direction.z);
bool flag = fP > FP.Zero;
if (flag)
{
result.x = direction.x / fP * this.radius;
result.y = FP.Sign(direction.y) * this.height * FP.Half;
result.z = direction.z / fP * this.radius;
}
else
{
result.x = FP.Zero;
result.y = FP.Sign(direction.y) * this.height * FP.Half;
result.z = FP.Zero;
}
}
/// <summary>
/// Creates a quaternion from a matrix.
/// </summary>
/// <param name="matrix">A matrix representing an orientation.</param>
/// <param name="result">JQuaternion representing an orientation.</param>
public static void CreateFromMatrix(ref TSMatrix matrix, out TSQuaternion result)
{
FP num8 = (matrix.M11 + matrix.M22) + matrix.M33;
if (num8 > FP.Zero)
{
FP num = FP.Sqrt((num8 + FP.One));
result.w = num * FP.Half;
num = FP.Half / num;
result.x = (matrix.M23 - matrix.M32) * num;
result.y = (matrix.M31 - matrix.M13) * num;
result.z = (matrix.M12 - matrix.M21) * num;
}
else if ((matrix.M11 >= matrix.M22) && (matrix.M11 >= matrix.M33))
{
FP num7 = FP.Sqrt((((FP.One + matrix.M11) - matrix.M22) - matrix.M33));
FP num4 = FP.Half / num7;
result.x = FP.Half * num7;
result.y = (matrix.M12 + matrix.M21) * num4;
result.z = (matrix.M13 + matrix.M31) * num4;
result.w = (matrix.M23 - matrix.M32) * num4;
}
else if (matrix.M22 > matrix.M33)
{
FP num6 = FP.Sqrt((((FP.One + matrix.M22) - matrix.M11) - matrix.M33));
FP num3 = FP.Half / num6;
result.x = (matrix.M21 + matrix.M12) * num3;
result.y = FP.Half * num6;
result.z = (matrix.M32 + matrix.M23) * num3;
result.w = (matrix.M31 - matrix.M13) * num3;
}
else
{
FP num5 = FP.Sqrt((((FP.One + matrix.M33) - matrix.M11) - matrix.M22));
FP num2 = FP.Half / num5;
result.x = (matrix.M31 + matrix.M13) * num2;
result.y = (matrix.M32 + matrix.M23) * num2;
result.z = FP.Half * num5;
result.w = (matrix.M12 - matrix.M21) * num2;
}
}
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 void UpdateShape()
{
this.sina = this.radius / FP.Sqrt(this.radius * this.radius + this.height * this.height);
base.UpdateShape();
}
public static FP Distance(TSVector v1, TSVector v2)
{
return(FP.Sqrt((v1.x - v2.x) * (v1.x - v2.x) + (v1.y - v2.y) * (v1.y - v2.y) + (v1.z - v2.z) * (v1.z - v2.z)));
}
/// <summary>
/// Gets the square root.
/// </summary>
/// <param name="number">The number to get the square root from.</param>
/// <returns></returns>
#region public static FP Sqrt(FP number)
public static FP Sqrt(FP number)
{
return(FP.Sqrt(number));
}
public static FP Distance2D(TSVector3 v1, TSVector3 v2)
{
return(FP.Sqrt((v1.x - v2.x) * (v1.x - v2.x) + (v1.z - v2.z) * (v1.z - v2.z)));
}
public static void Distance(ref TSVector2 value1, ref TSVector2 value2, out FP result)
{
DistanceSquared(ref value1, ref value2, out result);
result = (FP)FP.Sqrt(result);
}