public void resetMassData()
{
// Compute mass data from shapes. Each shape has its own density.
_mass = 0.0f;
_invMass = 0.0f;
_inertia = 0.0f;
_invI = 0.0f;
_sweep.LocalCenter = Vector2.Zero;
// Kinematic bodies have zero mass.
if( bodyType == BodyType.Kinematic )
{
_sweep.C0 = _xf.p;
_sweep.C = _xf.p;
_sweep.A0 = _sweep.A;
return;
}
Debug.Assert( bodyType == BodyType.Dynamic || bodyType == BodyType.Static );
// Accumulate mass over all fixtures.
Vector2 localCenter = Vector2.Zero;
foreach( Fixture f in fixtureList )
{
if( f.shape._density == 0 )
{
continue;
}
var massData = f.shape.massData;
_mass += massData.mass;
localCenter += massData.mass * massData.centroid;
_inertia += massData.inertia;
}
//FPE: Static bodies only have mass, they don't have other properties. A little hacky tho...
if( bodyType == BodyType.Static )
{
_sweep.C0 = _sweep.C = _xf.p;
return;
}
// Compute center of mass.
if( _mass > 0.0f )
{
_invMass = 1.0f / _mass;
localCenter *= _invMass;
}
else
{
// Force all dynamic bodies to have a positive mass.
_mass = 1.0f;
_invMass = 1.0f;
}
if( _inertia > 0.0f && !_fixedRotation )
{
// Center the inertia about the center of mass.
_inertia -= _mass * Vector2.Dot( localCenter, localCenter );
Debug.Assert( _inertia > 0.0f );
_invI = 1.0f / _inertia;
}
else
{
_inertia = 0.0f;
_invI = 0.0f;
}
// Move center of mass.
var oldCenter = _sweep.C;
_sweep.LocalCenter = localCenter;
_sweep.C0 = _sweep.C = MathUtils.Mul( ref _xf, ref _sweep.LocalCenter );
// Update center of mass velocity.
var a = _sweep.C - oldCenter;
_linearVelocity += new Vector2( -_angularVelocity * a.Y, _angularVelocity * a.X );
}
