private void UpdateData()
{
localCentroid = Vec3f.Zero;
Mass = 0;
for (int i = 0; i < colliders.Length; i++)
{
Collider col = colliders[i];
col.body = this;
Mass += col.mass;
localCentroid += col.mass * col.localCentroid;
}
InverseMass = 1 / Mass;
localCentroid *= InverseMass;
Mat3f localInertiaTensor = Mat3f.Zero;
for (int i = 0; i < colliders.Length; i++)
{
Collider col = colliders[i];
Vec3f r = localCentroid - col.localCentroid;
float rDotR = Vec3f.Dot(r, r);
Mat3f rOutR = Mat3f.OuterProduct(r, r);
localInertiaTensor = col.localInertiaTensor + col.mass * (rDotR * Mat3f.Identity - rOutR);
}
localInverseInertiaTensor = localInertiaTensor.Inverted();
}
private void UpdateOrientation()
{
Quatf q = orientation.GetRotation();
q.Normalize();
orientation = Mat3f.CreateRotation(q);
}
public static void Write(this BinaryWriter writer, Mat3f mat)
{
writer.Write(mat.c0);
writer.Write(mat.c1);
writer.Write(mat.c2);
}
private void UpdateInertiaTensor()
{
globalInverseInertiaTensor = orientation * localInverseInertiaTensor * inverseOrientation;
}