public override void Step(b2TimeStep step)
{
if (m_bodyList == null)
{
return;
}
if (useWorldGravity)
{
gravity = GetWorld().GetGravity().Copy();
}
for (b2ControllerEdge i = m_bodyList; i != null; i = i.nextBody)
{
b2Body body = i.body;
if (body.IsAwake() == false)
{
//Buoyancy force is just a function of position,
//so unlike most forces, it is safe to ignore sleeping bodes
continue;
}
b2Vec2 areac = new b2Vec2();
b2Vec2 massc = new b2Vec2();
float area = 0.0f;
float mass = 0.0f;
for (b2Fixture fixture = body.GetFixtureList(); fixture != null; fixture = fixture.GetNext())
{
b2Vec2 sc = new b2Vec2();
float sarea = fixture.GetShape().ComputeSubmergedArea(normal, offset, body.GetTransform(), sc);
area += sarea;
areac.x += sarea * sc.x;
areac.y += sarea * sc.y;
float shapeDensity;
if (useDensity)
{
//TODO: Figure out what to do now density is gone
shapeDensity = 1.0f;
}
else
{
shapeDensity = 1.0f;
}
mass += sarea * shapeDensity;
massc.x += sarea * sc.x * shapeDensity;
massc.y += sarea * sc.y * shapeDensity;
}
areac.x /= area;
areac.y /= area;
massc.x /= mass;
massc.y /= mass;
if (area < float.MinValue)
{
continue;
}
//Buoyancy
b2Vec2 buoyancyForce = gravity.GetNegative();
buoyancyForce.Multiply(density * area);
body.ApplyForce(buoyancyForce, massc);
//Linear drag
b2Vec2 dragForce = body.GetLinearVelocityFromWorldPoint(areac);
dragForce.Subtract(velocity);
dragForce.Multiply(-linearDrag * area);
body.ApplyForce(dragForce, areac);
//Angular drag
//TODO: Something that makes more physical sense?
body.ApplyTorque(-body.GetInertia() / body.GetMass() * area * body.GetAngularVelocity() * angularDrag);
}
}