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); }