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()); }