public void GetMassProperties(float density, out float mass, out Vector3 centerOfMass, out Matrix inertiaTensor)
{
inertiaTensor = Matrix.Identity;
// if polygon trace model
if (_type == TraceModelType.Polygon)
{
idTraceModel traceModel = VolumeFromPolygon(1.0f);
traceModel.GetMassProperties(density, out mass, out centerOfMass, out inertiaTensor);
}
else
{
VolumeIntegrals integrals = GetVolumeIntegrals();
// if no volume
if (integrals.T0 == 0.0f)
{
mass = 1.0f;
centerOfMass = Vector3.Zero;
inertiaTensor = Matrix.Identity;
}
else
{
// mass of model
mass = density * integrals.T0;
// center of mass
centerOfMass = integrals.T1 / integrals.T0;
// compute inertia tensor
inertiaTensor.M11 = density * (integrals.T2.Y + integrals.T2.Z);
inertiaTensor.M22 = density * (integrals.T2.Z + integrals.T2.X);
inertiaTensor.M33 = density * (integrals.T2.X + integrals.T2.Y);
inertiaTensor.M12 = inertiaTensor.M21 = -density * integrals.TP.X;
inertiaTensor.M23 = inertiaTensor.M32 = -density * integrals.TP.Y;
inertiaTensor.M31 = inertiaTensor.M13 = -density * integrals.TP.Z;
// translate inertia tensor to center of mass
inertiaTensor.M11 -= mass * (centerOfMass.Y * centerOfMass.Y + centerOfMass.Z * centerOfMass.Z);
inertiaTensor.M22 -= mass * (centerOfMass.Z * centerOfMass.Z + centerOfMass.X * centerOfMass.X);
inertiaTensor.M33 -= mass * (centerOfMass.X * centerOfMass.X + centerOfMass.Y * centerOfMass.Y);
inertiaTensor.M12 = inertiaTensor.M21 += mass * centerOfMass.X * centerOfMass.Y;
inertiaTensor.M23 = inertiaTensor.M32 += mass * centerOfMass.Y * centerOfMass.Z;
inertiaTensor.M31 = inertiaTensor.M13 += mass * centerOfMass.Z * centerOfMass.X;
}
}
}
private VolumeIntegrals GetVolumeIntegrals()
{
VolumeIntegrals integrals = new VolumeIntegrals();
int polyCount = _polygons.Length;
for(int i = 0; i < polyCount; i++)
{
TraceModelPolygon poly = _polygons[i];
float nx = idMath.Abs(poly.Normal.X);
float ny = idMath.Abs(poly.Normal.Y);
float nz = idMath.Abs(poly.Normal.Z);
int c = 0;
if((nx > ny) && (nx > nz))
{
c = 0;
}
else
{
c = (ny > nz) ? 1 : 2;
}
int a = (c + 1) % 3;
int b = (a + 1) % 3;
PolygonIntegrals pi = GetPolygonIntegrals(i, a, b, c);
integrals.T0 += poly.Normal.X * ((a == 0) ? pi.Fa : ((b == 0) ? pi.Fb : pi.Fc));
if(a == 0)
{
integrals.T1.X += poly.Normal.X * pi.Faa;
integrals.T2.X += poly.Normal.X * pi.Faaa;
integrals.TP.X += poly.Normal.X * pi.Faab;
}
else if(a == 1)
{
integrals.T1.Y += poly.Normal.Y * pi.Faa;
integrals.T2.Y += poly.Normal.Y * pi.Faaa;
integrals.TP.Y += poly.Normal.Y * pi.Faab;
}
else if(a == 2)
{
integrals.T1.Z += poly.Normal.Z * pi.Faa;
integrals.T2.Z += poly.Normal.Z * pi.Faaa;
integrals.TP.Z += poly.Normal.Z * pi.Faab;
}
if(b == 0)
{
integrals.T1.X += poly.Normal.X * pi.Fbb;
integrals.T2.X += poly.Normal.X * pi.Fbbb;
integrals.TP.X += poly.Normal.X * pi.Fbbc;
}
else if(b == 1)
{
integrals.T1.Y += poly.Normal.Y * pi.Fbb;
integrals.T2.Y += poly.Normal.Y * pi.Fbbb;
integrals.TP.Y += poly.Normal.Y * pi.Fbbc;
}
else if(b == 2)
{
integrals.T1.Z += poly.Normal.Z * pi.Fbb;
integrals.T2.Z += poly.Normal.Z * pi.Fbbb;
integrals.TP.Z += poly.Normal.Z * pi.Fbbc;
}
if(c == 0)
{
integrals.T1.X += poly.Normal.X * pi.Fcc;
integrals.T2.X += poly.Normal.X * pi.Fccc;
integrals.TP.X += poly.Normal.X * pi.Fcca;
}
else if(c == 1)
{
integrals.T1.Y += poly.Normal.Y * pi.Fcc;
integrals.T2.Y += poly.Normal.Y * pi.Fccc;
integrals.TP.Y += poly.Normal.Y * pi.Fcca;
}
else if(c == 2)
{
integrals.T1.Z += poly.Normal.Z * pi.Fcc;
integrals.T2.Z += poly.Normal.Z * pi.Fccc;
integrals.TP.Z += poly.Normal.Z * pi.Fcca;
}
}
integrals.T1 *= 0.5f;
integrals.T2 *= (1.0f / 3.0f);
integrals.TP *= 0.5f;
return integrals;
}
private VolumeIntegrals GetVolumeIntegrals()
{
VolumeIntegrals integrals = new VolumeIntegrals();
int polyCount = _polygons.Length;
for (int i = 0; i < polyCount; i++)
{
TraceModelPolygon poly = _polygons[i];
float nx = idMath.Abs(poly.Normal.X);
float ny = idMath.Abs(poly.Normal.Y);
float nz = idMath.Abs(poly.Normal.Z);
int c = 0;
if ((nx > ny) && (nx > nz))
{
c = 0;
}
else
{
c = (ny > nz) ? 1 : 2;
}
int a = (c + 1) % 3;
int b = (a + 1) % 3;
PolygonIntegrals pi = GetPolygonIntegrals(i, a, b, c);
integrals.T0 += poly.Normal.X * ((a == 0) ? pi.Fa : ((b == 0) ? pi.Fb : pi.Fc));
if (a == 0)
{
integrals.T1.X += poly.Normal.X * pi.Faa;
integrals.T2.X += poly.Normal.X * pi.Faaa;
integrals.TP.X += poly.Normal.X * pi.Faab;
}
else if (a == 1)
{
integrals.T1.Y += poly.Normal.Y * pi.Faa;
integrals.T2.Y += poly.Normal.Y * pi.Faaa;
integrals.TP.Y += poly.Normal.Y * pi.Faab;
}
else if (a == 2)
{
integrals.T1.Z += poly.Normal.Z * pi.Faa;
integrals.T2.Z += poly.Normal.Z * pi.Faaa;
integrals.TP.Z += poly.Normal.Z * pi.Faab;
}
if (b == 0)
{
integrals.T1.X += poly.Normal.X * pi.Fbb;
integrals.T2.X += poly.Normal.X * pi.Fbbb;
integrals.TP.X += poly.Normal.X * pi.Fbbc;
}
else if (b == 1)
{
integrals.T1.Y += poly.Normal.Y * pi.Fbb;
integrals.T2.Y += poly.Normal.Y * pi.Fbbb;
integrals.TP.Y += poly.Normal.Y * pi.Fbbc;
}
else if (b == 2)
{
integrals.T1.Z += poly.Normal.Z * pi.Fbb;
integrals.T2.Z += poly.Normal.Z * pi.Fbbb;
integrals.TP.Z += poly.Normal.Z * pi.Fbbc;
}
if (c == 0)
{
integrals.T1.X += poly.Normal.X * pi.Fcc;
integrals.T2.X += poly.Normal.X * pi.Fccc;
integrals.TP.X += poly.Normal.X * pi.Fcca;
}
else if (c == 1)
{
integrals.T1.Y += poly.Normal.Y * pi.Fcc;
integrals.T2.Y += poly.Normal.Y * pi.Fccc;
integrals.TP.Y += poly.Normal.Y * pi.Fcca;
}
else if (c == 2)
{
integrals.T1.Z += poly.Normal.Z * pi.Fcc;
integrals.T2.Z += poly.Normal.Z * pi.Fccc;
integrals.TP.Z += poly.Normal.Z * pi.Fcca;
}
}
integrals.T1 *= 0.5f;
integrals.T2 *= (1.0f / 3.0f);
integrals.TP *= 0.5f;
return(integrals);
}
