/// <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;
}
/// <summary>
/// Initializes a new instance of the <see cref="Controller" /> class
/// </summary>
/// <param name="world">The world</param>
public Controller(World world)
{
BodyList = null;
BodyCount = 0;
World = world;
}
/// <summary>
/// Removes all bodies from the controller list.
/// </summary>
public void Clear()
{
ControllerEdge current = BodyList;
while (current != null)
{
ControllerEdge edge = current;
//Remove edge from controller list
BodyList = edge.NextBody;
//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 == edge.Body.ControllerList)
{
edge.Body.ControllerList = edge.NextController;
}
//Free the edge
//m_world->m_blockAllocator.Free(edge, sizeof(b2ControllerEdge));
}
BodyCount = 0;
}
/// <summary>
/// Steps the step
/// </summary>
/// <param name="step">The step</param>
public override void Step(TimeStep step)
{
float timestep = step.Dt;
if (timestep <= Settings.FltEpsilon)
{
return;
}
if (timestep > maxTimestep && maxTimestep > 0)
{
timestep = maxTimestep;
}
for (ControllerEdge i = BodyList; i != null; i = i.NextBody)
{
Body body = i.Body;
if (body.IsSleeping())
{
continue;
}
Vec2 damping = body.GetWorldVector(Math.Mul(T, body.GetLocalVector(body.GetLinearVelocity())));
body.SetLinearVelocity(body.GetLinearVelocity() + timestep * damping);
}
}
/// <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;
}
/// <summary>
/// Steps the step
/// </summary>
/// <param name="step">The step</param>
public override void Step(TimeStep step)
{
for (ControllerEdge i = BodyList; i != null; i = i.NextBody)
{
Body body = i.Body;
if (body.IsSleeping())
{
continue;
}
body.SetLinearVelocity(body.GetLinearVelocity() + step.Dt * A);
}
}
/// <summary>
/// Steps the step
/// </summary>
/// <param name="step">The step</param>
public override void Step(TimeStep step)
{
//B2_NOT_USED(step);
for (ControllerEdge i = BodyList; i != null; i = i.NextBody)
{
Body body = i.Body;
if (body.IsSleeping())
{
continue;
}
body.ApplyForce(f, body.GetWorldCenter());
}
}
/// <summary>
/// Steps the step
/// </summary>
/// <param name="step">The step</param>
public override void Step(TimeStep step)
{
//B2_NOT_USED(step);
if (InvSqr)
{
for (ControllerEdge i = BodyList; i != null; i = i.NextBody)
{
Body body1 = i.Body;
for (ControllerEdge j = BodyList; j != i; j = j.NextBody)
{
Body body2 = j.Body;
Vec2 d = body2.GetWorldCenter() - body1.GetWorldCenter();
float r2 = d.LengthSquared();
if (r2 < Settings.FltEpsilon)
{
continue;
}
Vec2 f = G / r2 / Math.Sqrt(r2) * body1.GetMass() * body2.GetMass() * d;
body1.ApplyForce(f, body1.GetWorldCenter());
body2.ApplyForce(-1.0f * f, body2.GetWorldCenter());
}
}
}
else
{
for (ControllerEdge i = BodyList; i != null; i = i.NextBody)
{
Body body1 = i.Body;
for (ControllerEdge j = BodyList; j != i; j = j.NextBody)
{
Body body2 = j.Body;
Vec2 d = body2.GetWorldCenter() - body1.GetWorldCenter();
float r2 = d.LengthSquared();
if (r2 < Settings.FltEpsilon)
{
continue;
}
Vec2 f = G / r2 * body1.GetMass() * body2.GetMass() * d;
body1.ApplyForce(f, body1.GetWorldCenter());
body2.ApplyForce(-1.0f * f, body2.GetWorldCenter());
}
}
}
}
/// <summary>
/// Initializes a new instance of the <see cref="Controller" /> class
/// </summary>
public Controller()
{
BodyList = null;
BodyCount = 0;
}
/// <summary>
/// Steps the step
/// </summary>
/// <param name="step">The step</param>
public override void Step(TimeStep step)
{
//B2_NOT_USED(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 (Fixture shape = body.GetFixtureList(); shape != null; shape = shape.Next)
{
Vec2 sc;
float sarea = shape.ComputeSubmergedArea(Normal, Offset, 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;
//Vec2 localCentroid = Math.MulT(body.GetXForm(), areac);
massc.X /= mass;
massc.Y /= mass;
if (area < Settings.FltEpsilon)
{
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);
}
}
