/// <summary>
/// Adds a body to the controller list.
/// </summary>
public void AddBody(Body body)
{
ControllerEdge edge = new ControllerEdge();
edge.body = body;
edge.controller = this;
//Add edge to controller list
edge.nextBody = _bodyList;
edge.prevBody = null;
if (_bodyList != null)
{
_bodyList.prevBody = edge;
}
_bodyList = edge;
++_bodyCount;
//Add edge to body list
edge.nextController = body._controllerList;
edge.prevController = null;
if (body._controllerList != null)
{
body._controllerList.prevController = edge;
}
body._controllerList = edge;
}
public Controller(PhysicsSimulator world)
{
_bodyList = null;
_bodyCount = 0;
_world = world;
}
/// <summary>
/// Removes a body from the controller list.
/// </summary>
public void RemoveBody(Body body)
{
//Assert that the controller is not empty
//Box2DXDebug.Assert(_bodyCount > 0);
//Find the corresponding edge
ControllerEdge edge = _bodyList;
while (edge != null && edge.body != body)
{
edge = edge.nextBody;
}
//Assert that we are removing a body that is currently attached to the controller
//Box2DXDebug.Assert(edge != null);
//Remove edge from controller list
if (edge.prevBody != null)
{
edge.prevBody.nextBody = edge.nextBody;
}
if (edge.nextBody != null)
{
edge.nextBody.prevBody = edge.prevBody;
}
if (edge == _bodyList)
{
_bodyList = edge.nextBody;
}
--_bodyCount;
//Remove edge from body list
if (edge.prevController != null)
{
edge.prevController.nextController = edge.nextController;
}
if (edge.nextController != null)
{
edge.nextController.prevController = edge.prevController;
}
if (edge == body._controllerList)
{
body._controllerList = edge.nextController;
}
//Free the edge
edge = null;
}
public Controller()
{
_bodyList = null;
_bodyCount = 0;
}
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;
}
Vector2 areac = new Vector2(0, 0);
Vector2 massc = new Vector2(0, 0);
float area = 0;
float mass = 0;
for (Shape shape = body.GetShapeList(); shape != null; shape = shape.GetNext())
{
Vector2 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 < Settings.FLT_EPSILON)
{
continue;
}
//Buoyancy
Vector2 buoyancyForce = -density * area * gravity;
body.ApplyForce(buoyancyForce, massc);
//Linear drag
Vector2 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);
}
}
