private void SetInertiaTensor()
{
if (!IsStatic)
{
InitCenterOfMass = ShapeCommonUtilities.CalculateCenterOfMass(
Vertices,
ObjectGeometry.BaseGeometry.Triangle,
MassInfo.Mass);
Matrix3x3 baseTensors = ShapeCommonUtilities.GetInertiaTensor(
Vertices,
ObjectGeometry.BaseGeometry.Triangle,
InitCenterOfMass,
MassInfo.Mass).InertiaTensor;
MassInfo.InverseBaseInertiaTensor = Matrix3x3.Invert(baseTensors);
MassInfo.InverseInertiaTensor = (RotationMatrix * MassInfo.InverseBaseInertiaTensor) *
Matrix3x3.Transpose(RotationMatrix);
}
else
{
MassInfo.InverseBaseInertiaTensor = Matrix3x3.IdentityMatrix(0.0);
MassInfo.InverseInertiaTensor = Matrix3x3.IdentityMatrix(0.0);
}
}
private void SetShapesProperties()
{
Matrix3x3 baseTensors = new Matrix3x3();
int totalVertex = 0;
InitCenterOfMass = CalculateCenterOfMass();
for (int i = 0; i < ShapesGeometry.Length; i++)
{
var vertices = ShapesGeometry[i].GetVertices();
baseTensors += ShapeCommonUtilities.GetInertiaTensor(
vertices,
ShapesGeometry[i].BaseGeometry.Triangle,
InitCenterOfMass,
PartialMass[i]).InertiaTensor;
Vector3d[] vertexPosition = vertices;
totalVertex += ShapesGeometry[i].BaseGeometry.VerticesIdx.Length;
}
RotationMatrix = Quaternion.ConvertToMatrix(Quaternion.Normalize(RotationStatus));
SetRelativePosition();
MassInfo.InertiaTensor = baseTensors;
MassInfo.InverseBaseInertiaTensor = Matrix3x3.Invert(baseTensors);
MassInfo.InverseInertiaTensor = (RotationMatrix * MassInfo.InverseBaseInertiaTensor) *
Matrix3x3.Transpose(RotationMatrix);
}
private Vector3d CalculateCenterOfMass()
{
Vector3d startPosition = new Vector3d();
for (int i = 0; i < ShapesGeometry.Length; i++)
{
Vector3d[] vertices = ShapesGeometry[i].GetVertices();
var centerOfMass = ShapeCommonUtilities.CalculateCenterOfMass(
vertices,
ShapesGeometry[i].BaseGeometry.Triangle,
PartialMass[i]);
startPosition += centerOfMass * PartialMass[i];
}
if (MassInfo.Mass > 0.0)
{
return(startPosition / MassInfo.Mass);
}
return(Vector3d.ToZero());
}