/// <summary>
/// Adds a body to the controller list.
/// </summary>
public void AddBody(Body body)
{
ControllerEdge edge = new ControllerEdge
{
body = body,
controller = this,
nextBody = _bodyList,
prevBody = null
};
//Add edge to controller list
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>
/// Removes all bodies from the controller list.
/// </summary>
public void Clear()
{
#warning "Check this"
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;
}
public Controller(World world)
{
_bodyList = null;
_bodyCount = 0;
_world = world;
}
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);
}
}
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>
/// Removes a body from the controller list.
/// </summary>
public void RemoveBody(Body body)
{
//Assert that the controller is not empty
Box2DNetDebug.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
Box2DNetDebug.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 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;
Vector2 d = body2.GetWorldCenter() - body1.GetWorldCenter();
float r2 = d.LengthSquared();
if (r2 < Settings.FLT_EPSILON)
{
continue;
}
Vector2 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;
Vector2 d = body2.GetWorldCenter() - body1.GetWorldCenter();
float r2 = d.LengthSquared();
if (r2 < Settings.FLT_EPSILON)
{
continue;
}
Vector2 f = G / r2 * body1.GetMass() * body2.GetMass() * d;
body1.ApplyForce(f, body1.GetWorldCenter());
body2.ApplyForce(-1.0f * f, body2.GetWorldCenter());
}
}
}
}
public override void Step(TimeStep step)
{
float timestep = step.Dt;
if (timestep <= Settings.FLT_EPSILON)
{
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);
}
}
protected Controller()
{
_bodyList = null;
_bodyCount = 0;
}
public Controller()
{
_bodyList = null;
_bodyCount = 0;
}
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.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);
}
}
