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