Extending Phaser 3 Geom.Rectangle to Box2D-Lite, Part XX: Pulley Joint

The PulleyJoint class represents a joint connecting two bodies by a fixed length “rope.” The PulleyJoint only limits translation along the “rope” axes and can limit the length of the “rope.” Setting the ratio to something other than 1.0 causes the pulley to emulate a block-and-tackle.

This joint requires a world space fixed point, pa1 and

pa2, and a world space body point, a1 and a2 for each body.

The lengths of the “ropes” are computed by:

PulleyJoint(T body1, T body2, Vector2 pulleyAnchor1, Vector2 pulleyAnchor2, Vector2 bodyAnchor1, Vector2 bodyAnchor2) {
		super(body1, body2);
		
		// verify the pulley anchor points are not null
		if (pulleyAnchor1 == null) 
			throw new ArgumentNullException("pulleyAnchor1");
		
		if (pulleyAnchor2 == null) 
			throw new ArgumentNullException("pulleyAnchor2");
		
		// verify the body anchor points are not null
		if (bodyAnchor1 == null) 
			throw new ArgumentNullException("bodyAnchor1");
		
		if (bodyAnchor2 == null) 
			throw new ArgumentNullException("bodyAnchor2");
		
		// set the pulley anchor points
		this.pulleyAnchor1 = pulleyAnchor1.copy();
		this.pulleyAnchor2 = pulleyAnchor2.copy();
		// get the local anchor points
		this.localAnchor1 = body1.getLocalPoint(bodyAnchor1);
		this.localAnchor2 = body2.getLocalPoint(bodyAnchor2);
		// default the ratio and minimum length
		this.ratio = 1.0;
		// compute the lengths
		double length1 = bodyAnchor1.distance(pulleyAnchor1);
		double length2 = bodyAnchor2.distance(pulleyAnchor2);
		// compute the lengths
		// length = l1 + ratio * l2
		this.length = length1 + length2;
		// initialize the other fields
		this.impulse = 0.0;
		// initialize the slack parameters
		this.slackEnabled = false;
		this.overLength = false;
	}

initializeConstraints(TimeStep step, Settings settings) {
		double linearTolerance = settings.getLinearTolerance();
		
		Transform t1 = this.body1.getTransform();
		Transform t2 = this.body2.getTransform();
		Mass m1 = this.body1.getMass();
		Mass m2 = this.body2.getMass();
		
		double invM1 = m1.getInverseMass();
		double invM2 = m2.getInverseMass();
		double invI1 = m1.getInverseInertia();
		double invI2 = m2.getInverseInertia();
		
		// put the body anchors in world space
		Vector2 r1 = t1.getTransformedR(this.body1.getLocalCenter().to(this.localAnchor1));
		Vector2 r2 = t2.getTransformedR(this.body2.getLocalCenter().to(this.localAnchor2));
		Vector2 p1 = r1.sum(this.body1.getWorldCenter());
		Vector2 p2 = r2.sum(this.body2.getWorldCenter());
		
		Vector2 s1 = this.pulleyAnchor1;
		Vector2 s2 = this.pulleyAnchor2;
		
		// compute the axes
		this.n1 = s1.to(p1);
		this.n2 = s2.to(p2);
		
		// get the lengths
		double l1 = this.n1.normalize();
		double l2 = this.n2.normalize();
		
		// get the current total length
		double l = l1 + l2;
		
		// check if we need to solve the constraint
		if (l > this.length || !this.slackEnabled) {
			this.overLength = true;
			
			// check for near zero length
			if (l1 <= 10.0 * linearTolerance) {
				// zero out the axis
				this.n1.zero();
			}
			
			// check for near zero length		
			if (l2 <= 10.0 * linearTolerance) {
				// zero out the axis
				this.n2.zero();
			}
			
			// compute the inverse effective masses (K matrix, in this case its a scalar) for the constraints
			double r1CrossN1 = r1.cross(this.n1);
			double r2CrossN2 = r2.cross(this.n2);
			double pm1 = invM1 + invI1 * r1CrossN1 * r1CrossN1;
			double pm2 = invM2 + invI2 * r2CrossN2 * r2CrossN2;
			this.invK = pm1 + this.ratio * this.ratio * pm2;
			// make sure we can invert it
			if (this.invK > Epsilon.E) {
				this.invK = 1.0 / this.invK;
			} else {
				this.invK = 0.0;
			}
			
			if (settings.isWarmStartingEnabled()) {
				// warm start the constraints taking
				// variable time steps into account
				double dtRatio = step.getDeltaTimeRatio();
				this.impulse *= dtRatio;
				
				// compute the impulse along the axes
				Vector2 J1 = this.n1.product(-this.impulse);
				Vector2 J2 = this.n2.product(-this.ratio * this.impulse);
				
				// apply the impulse
				this.body1.getLinearVelocity().add(J1.product(invM1));
				this.body1.setAngularVelocity(this.body1.getAngularVelocity() + invI1 * r1.cross(J1));
				this.body2.getLinearVelocity().add(J2.product(invM2));
				this.body2.setAngularVelocity(this.body2.getAngularVelocity() + invI2 * r2.cross(J2));
			} else {
				this.impulse = 0.0;
			}
		} else {
			// clear the impulse and don't solve anything
			this.impulse = 0;
			this.overLength = false;
		}
	}
	

solveVelocityConstraints(TimeStep step, Settings settings) {
		if (this.overLength || !this.slackEnabled) {
			Transform t1 = this.body1.getTransform();
			Transform t2 = this.body2.getTransform();
			Mass m1 = this.body1.getMass();
			Mass m2 = this.body2.getMass();
			
			double invM1 = m1.getInverseMass();
			double invM2 = m2.getInverseMass();
			double invI1 = m1.getInverseInertia();
			double invI2 = m2.getInverseInertia();
			
			// compute r1 and r2
			Vector2 r1 = t1.getTransformedR(this.body1.getLocalCenter().to(this.localAnchor1));
			Vector2 r2 = t2.getTransformedR(this.body2.getLocalCenter().to(this.localAnchor2));
			
			// compute the relative velocity
			Vector2 v1 = this.body1.getLinearVelocity().sum(r1.cross(this.body1.getAngularVelocity()));
			Vector2 v2 = this.body2.getLinearVelocity().sum(r2.cross(this.body2.getAngularVelocity()));
			
			// compute Jv + b
			double C = -this.n1.dot(v1) - this.ratio * this.n2.dot(v2);
			// compute the impulse
			double impulse = this.invK * (-C);
			this.impulse += impulse;
			
			// compute the impulse along each axis
			Vector2 J1 = this.n1.product(-impulse);
			Vector2 J2 = this.n2.product(-impulse * this.ratio);
			
			// apply the impulse
			this.body1.getLinearVelocity().add(J1.product(invM1));
			this.body1.setAngularVelocity(this.body1.getAngularVelocity() + invI1 * r1.cross(J1));
			this.body2.getLinearVelocity().add(J2.product(invM2));
			this.body2.setAngularVelocity(this.body2.getAngularVelocity() + invI2 * r2.cross(J2));
		}
	}