public static TSMatrix Multiply(TSMatrix matrix1, long scaleFactor)
{
TSMatrix result;
TSMatrix.Multiply(ref matrix1, scaleFactor, out result);
return(result);
}
public static TSMatrix operator -(TSMatrix value1, TSMatrix value2)
{
TSMatrix result; TSMatrix.Multiply(ref value2, -FP.One, out value2);
TSMatrix.Add(ref value1, ref value2, out result);
return(result);
}
public static TSMatrix Multiply(TSMatrix matrix1, TSMatrix matrix2)
{
TSMatrix result;
TSMatrix.Multiply(ref matrix1, ref matrix2, out result);
return(result);
}
public static TSMatrix operator *(TSMatrix value1, TSMatrix value2)
{
TSMatrix result;
TSMatrix.Multiply(ref value1, ref value2, out result);
return(result);
}
public virtual void Update()
{
bool flag = this.isParticle;
if (flag)
{
this.inertia = TSMatrix.Zero;
this.invInertia = (this.invInertiaWorld = TSMatrix.Zero);
this.invOrientation = (this.orientation = TSMatrix.Identity);
this.boundingBox = this.shape.boundingBox;
TSVector.Add(ref this.boundingBox.min, ref this.position, out this.boundingBox.min);
TSVector.Add(ref this.boundingBox.max, ref this.position, out this.boundingBox.max);
this.angularVelocity.MakeZero();
}
else
{
TSMatrix.Transpose(ref this.orientation, out this.invOrientation);
this.Shape.GetBoundingBox(ref this.orientation, out this.boundingBox);
TSVector.Add(ref this.boundingBox.min, ref this.position, out this.boundingBox.min);
TSVector.Add(ref this.boundingBox.max, ref this.position, out this.boundingBox.max);
bool flag2 = !this.isStatic;
if (flag2)
{
TSMatrix.Multiply(ref this.invOrientation, ref this.invInertia, out this.invInertiaWorld);
TSMatrix.Multiply(ref this.invInertiaWorld, ref this.orientation, out this.invInertiaWorld);
}
}
}
public static FP CalculateMassInertia(Shape shape, out TSVector centerOfMass, out TSMatrix inertia)
{
FP fP = FP.Zero;
centerOfMass = TSVector.zero;
inertia = TSMatrix.Zero;
bool flag = shape is Multishape;
if (flag)
{
throw new ArgumentException("Can't calculate inertia of multishapes.", "shape");
}
List <TSVector> list = new List <TSVector>();
shape.MakeHull(ref list, 3);
FP fP2 = FP.One / (60 * FP.One);
FP fP3 = FP.One / (120 * FP.One);
TSMatrix value = new TSMatrix(fP2, fP3, fP3, fP3, fP2, fP3, fP3, fP3, fP2);
for (int i = 0; i < list.Count; i += 3)
{
TSVector tSVector = list[i];
TSVector tSVector2 = list[i + 1];
TSVector tSVector3 = list[i + 2];
TSMatrix tSMatrix = new TSMatrix(tSVector.x, tSVector2.x, tSVector3.x, tSVector.y, tSVector2.y, tSVector3.y, tSVector.z, tSVector2.z, tSVector3.z);
FP fP4 = tSMatrix.Determinant();
TSMatrix value2 = TSMatrix.Multiply(tSMatrix * value * TSMatrix.Transpose(tSMatrix), fP4);
TSVector value3 = FP.One / (4 * FP.One) * (list[i] + list[i + 1] + list[i + 2]);
FP fP5 = FP.One / (6 * FP.One) * fP4;
inertia += value2;
centerOfMass += fP5 * value3;
fP += fP5;
}
inertia = TSMatrix.Multiply(TSMatrix.Identity, inertia.Trace()) - inertia;
centerOfMass *= FP.One / fP;
FP x = centerOfMass.x;
FP y = centerOfMass.y;
FP z = centerOfMass.z;
TSMatrix tSMatrix2 = new TSMatrix(-fP * (y * y + z * z), fP * x * y, fP * x * z, fP * y * x, -fP * (z * z + x * x), fP * y * z, fP * z * x, fP * z * y, -fP * (x * x + y * y));
TSMatrix.Add(ref inertia, ref tSMatrix2, out inertia);
return(fP);
}
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);
}
}
