public void ComputeMass(out MassData md)
{
// Calculate inertia tensor
double ex2 = 4 * e.x * e.x;
double ey2 = 4 * e.y * e.y;
double ez2 = 4 * e.z * e.z;
double mass = 8 * e.x * e.y * e.z * density;
double x = 1 / 12.0 * mass * (ey2 + ez2);
double y = 1 / 12.0 * mass * (ex2 + ez2);
double z = 1 / 12.0 * mass * (ex2 + ey2);
Mat3 I = Mat3.Diagonal(x, y, z);
// Transform tensor to local space
I = local.rotation * I * Mat3.Transpose(local.rotation);
I += (Mat3.Identity * Vec3.Dot(local.position, local.position) - Mat3.OuterProduct(local.position, local.position)) * mass;
md.center = local.position;
md.inertia = I;
md.mass = mass;
}
void CalculateMassData()
{
Mat3 inertia = Mat3.Diagonal(0);
InvInertiaModel = Mat3.Diagonal(0);
InvInertiaWorld = Mat3.Diagonal(0);
InvMass = 0;
Mass = 0;
double mass = 0;
if ((Flags & eStatic) > 0 || (Flags & eKinematic) > 0)
{
Vec3.Identity(ref LocalCenter);
WorldCenter = Tx.position;
return;
}
Vec3 lc = new Vec3();
Vec3.Identity(ref lc);
foreach (var box in Boxes)
{
if (box.density == 0)
{
continue;
}
MassData md;
box.ComputeMass(out md);
mass += md.mass;
inertia += md.inertia;
lc += md.center * md.mass;
}
if (mass > 0)
{
Mass = mass;
InvMass = 1 / mass;
lc *= InvMass;
inertia -= (Mat3.Identity * Vec3.Dot(lc, lc) - Mat3.OuterProduct(lc, lc)) * mass;
InvInertiaModel = Mat3.Inverse(inertia);
if ((Flags & eLockAxisX) > 0)
{
Vec3.Identity(ref InvInertiaModel.ex);
}
if ((Flags & eLockAxisY) > 0)
{
Vec3.Identity(ref InvInertiaModel.ey);
}
if ((Flags & eLockAxisZ) > 0)
{
Vec3.Identity(ref InvInertiaModel.ez);
}
}
else
{
// Force all dynamic bodies to have some mass
InvMass = 1;
InvInertiaModel = Mat3.Diagonal(0);
InvInertiaWorld = Mat3.Diagonal(0);
}
LocalCenter = lc;
WorldCenter = Transform.Mul(Tx, lc);
}