public override void Update(float dt)
{
_uniqueBodies.Clear();
World.QueryAABB(fixture =>
{
if (fixture.Body.IsStatic || !fixture.Body.Awake)
{
return(true);
}
if (!_uniqueBodies.ContainsKey(fixture.Body.BodyId))
{
_uniqueBodies.Add(fixture.Body.BodyId, fixture.Body);
}
return(true);
}, ref _container);
foreach (KeyValuePair <int, FSBody> kv in _uniqueBodies)
{
FSBody body = kv.Value;
FVector2 areac = FVector2.Zero;
FVector2 massc = FVector2.Zero;
float area = 0;
float mass = 0;
for (int j = 0; j < body.FixtureList.Count; j++)
{
FSFixture fixture = body.FixtureList[j];
if (fixture.Shape.ShapeType != ShapeType.Polygon && fixture.Shape.ShapeType != ShapeType.Circle)
{
continue;
}
Shape shape = fixture.Shape;
FVector2 sc;
float sarea = shape.ComputeSubmergedArea(_normal, _offset, body.Xf, out sc);
area += sarea;
areac.X += sarea * sc.X;
areac.Y += sarea * sc.Y;
mass += sarea * shape.Density;
massc.X += sarea * sc.X * shape.Density;
massc.Y += sarea * sc.Y * shape.Density;
}
areac.X /= area;
areac.Y /= area;
massc.X /= mass;
massc.Y /= mass;
if (area < FSSettings.Epsilon)
{
continue;
}
//Buoyancy
FVector2 buoyancyForce = -Density * area * _gravity;
body.ApplyForce(buoyancyForce, massc);
//Linear drag
FVector2 dragForce = body.GetLinearVelocityFromWorldPoint(areac) - Velocity;
dragForce *= -LinearDragCoefficient * area;
body.ApplyForce(dragForce, areac);
//Angular drag
body.ApplyTorque(-body.Inertia / body.Mass * area * body.AngularVelocity * AngularDragCoefficient);
}
}