/// Remove a rigid body from the simulation.
public void RemoveBody(cpBody body)
{
cp.AssertHard(body != GetStaticBody(), "Cannot remove the designated static body for the space.");
cp.AssertHard(ContainsBody(body), "Cannot remove a body that was not added to the space. (Removed twice maybe?)");
// cpAssertHard(body->shapeList == NULL, "Cannot remove a body from the space before removing the bodies attached to it.");
// cpAssertHard(body->constraintList == NULL, "Cannot remove a body from the space before removing the constraints attached to it.");
cp.AssertSpaceUnlocked(this);
body.Activate();
ArrayForBodyType(body.bodyType).Remove(body);
body.space = null;
}
/// Add a constraint to the simulation.
public cpConstraint AddConstraint(cpConstraint constraint)
{
cp.AssertHard(constraint.space != this, "You have already added this constraint to this space. You must not add it a second time.");
cp.AssertHard(constraint.space == null, "You have already added this constraint to another space. You cannot add it to a second.");
cp.AssertSpaceUnlocked(this);
cpBody a = constraint.a, b = constraint.b;
cp.AssertHard(a != null && b != null, "Constraint is attached to a NULL body.");
a.Activate();
b.Activate();
this.constraints.Add(constraint);
// Push onto the heads of the bodies' constraint lists
constraint.next_a = a.constraintList; a.constraintList = constraint;
constraint.next_b = b.constraintList; b.constraintList = constraint;
constraint.space = this;
return(constraint);
}
public void SetFilter(cpShapeFilter filter)
{
body.Activate();
this.filter = filter;
}
public void ProcessComponents(float dt)
{
var sleep = (this.sleepTimeThreshold != cp.Infinity);
var bodies = this.dynamicBodies;
// These checks can be removed at some stage (if DEBUG == undefined)
for (var i = 0; i < bodies.Count; i++)
{
var body = bodies[i];
cp.AssertSoft(body.nodeNext == null, "Internal Error: Dangling next pointer detected in contact graph.");
cp.AssertSoft(body.nodeRoot == null, "Internal Error: Dangling root pointer detected in contact graph.");
}
// Calculate the kinetic energy of all the bodies
if (sleep)
{
var dv = this.idleSpeedThreshold;
var dvsq = (dv != 0 ? dv * dv : cpVect.cpvlengthsq(this.gravity) * dt * dt);
for (var i = 0; i < bodies.Count; i++)
{
// TODO should make a separate array for kinematic bodies.
if (bodies[i].bodyType != cpBodyType.DYNAMIC)
{
continue;
}
// Need to deal with infinite mass objects
var keThreshold = (dvsq > 0 ? bodies[i].m * dvsq : 0.0f);
bodies[i].nodeIdleTime = (bodies[i].KineticEnergy() > keThreshold ? 0 : bodies[i].nodeIdleTime + dt);
}
}
// Awaken any sleeping bodies found and then push arbiters to the bodies' lists.
List <cpArbiter> arbiters = this.arbiters; // new List<cpArbiter>();
var count = arbiters.Count; //FIX: we cannot read the count values of the array because it changes inside
for (int i = 0; i < count; i++)
{
cpArbiter arb = arbiters[i];
cpBody a = arb.body_a, b = arb.body_b;
if (sleep)
{
if (b.bodyType == cpBodyType.KINEMATIC || a.IsSleeping())
{
a.Activate();
}
if (a.bodyType == cpBodyType.KINEMATIC || b.IsSleeping())
{
b.Activate();
}
}
a.PushArbiter(arb);
b.PushArbiter(arb);
}
if (sleep)
{
// Bodies should be held active if connected by a joint to a non-static rouge body.
var constraints = this.constraints;
for (var i = 0; i < constraints.Count; i++)
{
cpConstraint constraint = constraints[i];
cpBody a = constraint.a, b = constraint.b;
if (b.bodyType == cpBodyType.KINEMATIC)
{
a.Activate();
}
if (a.bodyType == cpBodyType.KINEMATIC)
{
b.Activate();
}
}
// Generate components and deactivate sleeping ones
for (var i = 0; i < bodies.Count;)
{
var body = bodies[i];
if (cp.ComponentRoot(body) == null)
{
// Body not in a component yet. Perform a DFS to flood fill mark
// the component in the contact graph using this body as the root.
FloodFillComponent(body, body);
// Check if the component should be put to sleep.
if (!ComponentActive(body, this.sleepTimeThreshold))
{
this.sleepingComponents.Add(body);
//CP_BODY_FOREACH_COMPONENT
for (var other = body; other != null; other = other.nodeNext)
{
this.DeactivateBody(other);
}
// deactivateBody() removed the current body from the list.
// Skip incrementing the index counter.
continue;
}
}
i++;
// Only sleeping bodies retain their component node pointers.
body.nodeRoot = null;
body.nodeNext = null;
}
}
}