public static void Invert(ref TSMatrix matrix, out TSMatrix result)
{
FP determinantInverse = 1 / matrix.Determinant();
FP m11 = (matrix.M22 * matrix.M33 - matrix.M23 * matrix.M32) * determinantInverse;
FP m12 = (matrix.M13 * matrix.M32 - matrix.M33 * matrix.M12) * determinantInverse;
FP m13 = (matrix.M12 * matrix.M23 - matrix.M22 * matrix.M13) * determinantInverse;
FP m21 = (matrix.M23 * matrix.M31 - matrix.M21 * matrix.M33) * determinantInverse;
FP m22 = (matrix.M11 * matrix.M33 - matrix.M13 * matrix.M31) * determinantInverse;
FP m23 = (matrix.M13 * matrix.M21 - matrix.M11 * matrix.M23) * determinantInverse;
FP m31 = (matrix.M21 * matrix.M32 - matrix.M22 * matrix.M31) * determinantInverse;
FP m32 = (matrix.M12 * matrix.M31 - matrix.M11 * matrix.M32) * determinantInverse;
FP m33 = (matrix.M11 * matrix.M22 - matrix.M12 * matrix.M21) * determinantInverse;
result.M11 = m11;
result.M12 = m12;
result.M13 = m13;
result.M21 = m21;
result.M22 = m22;
result.M23 = m23;
result.M31 = m31;
result.M32 = m32;
result.M33 = m33;
}
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 static void Invert(ref TSMatrix matrix, out TSMatrix result)
{
FP y = 1 / matrix.Determinant();
FP m = (matrix.M22 * matrix.M33 - matrix.M23 * matrix.M32) * y;
FP m2 = (matrix.M13 * matrix.M32 - matrix.M33 * matrix.M12) * y;
FP m3 = (matrix.M12 * matrix.M23 - matrix.M22 * matrix.M13) * y;
FP m4 = (matrix.M23 * matrix.M31 - matrix.M21 * matrix.M33) * y;
FP m5 = (matrix.M11 * matrix.M33 - matrix.M13 * matrix.M31) * y;
FP m6 = (matrix.M13 * matrix.M21 - matrix.M11 * matrix.M23) * y;
FP m7 = (matrix.M21 * matrix.M32 - matrix.M22 * matrix.M31) * y;
FP m8 = (matrix.M12 * matrix.M31 - matrix.M11 * matrix.M32) * y;
FP m9 = (matrix.M11 * matrix.M22 - matrix.M12 * matrix.M21) * y;
result.M11 = m;
result.M12 = m2;
result.M13 = m3;
result.M21 = m4;
result.M22 = m5;
result.M23 = m6;
result.M31 = m7;
result.M32 = m8;
result.M33 = m9;
}