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;
}
/// <summary>
/// Calculates the inertia of the shape relative to the center of mass.
/// </summary>
/// <param name="shape"></param>
/// <param name="centerOfMass"></param>
/// <param name="inertia">Returns the inertia relative to the center of mass, not to the origin</param>
/// <returns></returns>
#region public static FP CalculateMassInertia(Shape shape, out JVector centerOfMass, out JMatrix inertia)
public static FP CalculateMassInertia(Shape shape, out TSVector centerOfMass,
out TSMatrix inertia)
{
FP mass = FP.Zero;
centerOfMass = TSVector.zero; inertia = TSMatrix.Zero;
if (shape is Multishape)
{
throw new ArgumentException("Can't calculate inertia of multishapes.", "shape");
}
// build a triangle hull around the shape
List <TSVector> hullTriangles = new List <TSVector>();
shape.MakeHull(ref hullTriangles, 3);
// create inertia of tetrahedron with vertices at
// (0,0,0) (1,0,0) (0,1,0) (0,0,1)
FP a = FP.One / (60 * FP.One), b = FP.One / (120 * FP.One);
TSMatrix C = new TSMatrix(a, b, b, b, a, b, b, b, a);
for (int i = 0; i < hullTriangles.Count; i += 3)
{
TSVector column0 = hullTriangles[i + 0];
TSVector column1 = hullTriangles[i + 1];
TSVector column2 = hullTriangles[i + 2];
TSMatrix A = new TSMatrix(column0.x, column1.x, column2.x,
column0.y, column1.y, column2.y,
column0.z, column1.z, column2.z);
FP detA = A.Determinant();
// now transform this canonical tetrahedron to the target tetrahedron
// inertia by a linear transformation A
TSMatrix tetrahedronInertia = TSMatrix.Multiply(A * C * TSMatrix.Transpose(A), detA);
TSVector tetrahedronCOM = (FP.One / (4 * FP.One)) * (hullTriangles[i + 0] + hullTriangles[i + 1] + hullTriangles[i + 2]);
FP tetrahedronMass = (FP.One / (6 * FP.One)) * detA;
inertia += tetrahedronInertia;
centerOfMass += tetrahedronMass * tetrahedronCOM;
mass += tetrahedronMass;
}
inertia = TSMatrix.Multiply(TSMatrix.Identity, inertia.Trace()) - inertia;
centerOfMass = centerOfMass * (FP.One / mass);
FP x = centerOfMass.x;
FP y = centerOfMass.y;
FP z = centerOfMass.z;
// now translate the inertia by the center of mass
TSMatrix t = new TSMatrix(
-mass * (y * y + z * z), mass * x * y, mass * x * z,
mass * y * x, -mass * (z * z + x * x), mass * y * z,
mass * z * x, mass * z * y, -mass * (x * x + y * y));
TSMatrix.Add(ref inertia, ref t, out inertia);
return(mass);
}