public cpConstraint(cpBody a, cpBody b)
{
/// The first body connected to this constraint.
this.a = a;
/// The second body connected to this constraint.
this.b = b;
this.space = null;
this.next_a = null;
this.next_b = null;
/// The maximum force that this constraint is allowed to use.
this.maxForce = cp.Infinity;
/// The rate at which joint error is corrected.
/// Defaults to pow(1 - 0.1, 60) meaning that it will
/// correct 10% of the error every 1/60th of a second.
this.errorBias = cp.cpfpow(1f - 0.1f, 60f);
/// The maximum rate at which joint error is corrected.
this.maxBias = cp.Infinity;
this.collideBodies = true;
//Not clear
preSolve = DefaultPreSolve;
postSolve = DefaultPostSolve;
}
public void eachConstraint(cpBodyConstraintIteratorFunc func, object data)
{
for (cpConstraint var = this.constraintList; var != null; var = var.Next(this))
{
func(var, data);
}
}
/// Remove a constraint from the simulation.
public void RemoveConstraint(cpConstraint constraint)
{
cp.AssertSoft(this.ContainsConstraint(constraint),
"Cannot remove a constraint that was not added to the space. (Removed twice maybe?)");
cp.AssertSpaceUnlocked(this);
constraint.a.Activate();
constraint.b.Activate();
this.constraints.Remove(constraint);
constraint.a.RemoveConstraint(constraint);
constraint.b.RemoveConstraint(constraint);
constraint.space = null;
}
public static cpConstraint filterConstraints(cpConstraint node, cpBody body, cpConstraint filter)
{
if (node == filter)
{
return(node.Next(body));
}
else if (node.a == body)
{
node.next_a = filterConstraints(node.next_a, body, filter);
}
else
{
node.next_b = filterConstraints(node.next_b, body, filter);
}
return(node);
}
/// <summary>
/// CREATES A BODY WITH MASS AND INERTIA
/// </summary>
/// <param name="mass"></param>
/// <param name="moment"></param>
public cpBody(float mass, float moment)
{
transform = new cpTransform();
this.cog = cpVect.Zero;
this.space = null;
this.shapeList = null;
this.arbiterList = null; // These are both wacky linked lists.
this.constraintList = null;
velocity_func = UpdateVelocity;
position_func = UpdatePosition;
// This stuff is used to track information on the collision graph.
this.nodeRoot = null;
this.nodeNext = null;
this.nodeIdleTime = 0;
/// Position of the rigid body's center of gravity.
this.p = cpVect.Zero;
/// Velocity of the rigid body's center of gravity.
this.v = cpVect.Zero;
/// Force acting on the rigid body's center of gravity.
this.f = cpVect.Zero;
/// Angular velocity of the body around it's center of gravity in radians/second.
this.w = 0;
/// Torque applied to the body around it's center of gravity.
this.t = 0;
// This stuff is all private.
this.v_bias = cpVect.Zero; //x = this.v_biasy = 0;
this.w_bias = 0;
this.userData = null;
this.SetMass(mass);
this.SetMoment(moment);
this.SetAngle(0.0f);
}
/// 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 RemoveConstraint(cpConstraint constraint)
{
this.constraintList = cp.filterConstraints(constraintList, this, constraint);
}
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;
}
}
}
/// Test if a constraint has been added to the space.
public bool ContainsConstraint(cpConstraint constraint)
{
return(constraint.space == this);
}
