public override void Step(TimeStep step)
{
if (_bodyList == null) return;
if (useWorldGravity)
{
gravity = _world.Gravity;
}
for (ControllerEdge i = _bodyList; i != null; i = i.nextBody)
{
Body body = i.body;
if (body.IsSleeping())
{
//Buoyancy force is just a function of position,
//so unlike most forces, it is safe to ignore sleeping bodes
continue;
}
Vec2 areac = new Vec2(0, 0);
Vec2 massc = new Vec2(0, 0);
float area = 0;
float mass = 0;
for (Shape shape = body.GetShapeList(); shape != null; shape = shape.GetNext())
{
Vec2 sc;
float sarea = shape.ComputeSubmergedArea(normal, offset, body.GetXForm(), out sc);
area += sarea;
areac.X += sarea * sc.X;
areac.Y += sarea * sc.Y;
float shapeDensity = 0;
if (useDensity)
{
//TODO: Expose density publicly
shapeDensity = shape.Density;
}
else
{
shapeDensity = 1;
}
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 < Box2DX.Common.Settings.FLT_EPSILON)
continue;
//Buoyancy
Vec2 buoyancyForce = -density * area * gravity;
body.ApplyForce(buoyancyForce, massc);
//Linear drag
Vec2 dragForce = body.GetLinearVelocityFromWorldPoint(areac) - velocity;
dragForce *= -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);
}
}