--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/trunk/chipmunkd/constraints/cpRatchetJoint.d	Thu Dec 02 22:26:04 2010 +0100
@@ -0,0 +1,129 @@
+
+// written in the D programming language
+
+module chipmunkd.constraints.cpRatchetJoint;
+
+import chipmunkd.chipmunk;
+import chipmunkd.constraints.util;
+
+//const cpConstraintClass *cpRatchetJointGetClass();
+
+struct cpRatchetJoint {
+	cpConstraint constraint;
+	cpFloat angle, phase, ratchet;
+	
+	cpFloat iSum;
+		
+	cpFloat bias;
+	cpFloat jAcc, jMax;
+}
+
+//cpRatchetJoint *cpRatchetJointAlloc(void);
+//cpRatchetJoint *cpRatchetJointInit(cpRatchetJoint *joint, cpBody *a, cpBody *b, cpFloat phase, cpFloat ratchet);
+//cpConstraint *cpRatchetJointNew(cpBody *a, cpBody *b, cpFloat phase, cpFloat ratchet);
+//
+//CP_DefineConstraintProperty(cpRatchetJoint, cpFloat, angle, Angle);
+//CP_DefineConstraintProperty(cpRatchetJoint, cpFloat, phase, Phase);
+//CP_DefineConstraintProperty(cpRatchetJoint, cpFloat, ratchet, Ratchet);
+
+// cpRatchetJoint.c ---------------------------------
+
+static void
+preStep(cpRatchetJoint *joint, cpFloat dt, cpFloat dt_inv)
+{
+	mixin(CONSTRAINT_BEGIN!("joint", "a", "b"));
+	
+	cpFloat angle = joint.angle;
+	cpFloat phase = joint.phase;
+	cpFloat ratchet = joint.ratchet;
+	
+	cpFloat delta = b.a - a.a;
+	cpFloat diff = angle - delta;
+	cpFloat pdist = 0.0f;
+	
+	if(diff*ratchet > 0.0f){
+		pdist = diff;
+	} else {
+		joint.angle = cpffloor((delta - phase)/ratchet)*ratchet + phase;
+	}
+	
+	// calculate moment of inertia coefficient.
+	joint.iSum = 1.0f/(a.i_inv + b.i_inv);
+	
+	// calculate bias velocity
+	cpFloat maxBias = joint.constraint.maxBias;
+	joint.bias = cpfclamp(-joint.constraint.biasCoef*dt_inv*pdist, -maxBias, maxBias);
+	
+	// compute max impulse
+	joint.jMax = mixin(J_MAX!("joint", "dt"));
+
+	// If the bias is 0, the joint is not at a limit. Reset the impulse.
+	if(!joint.bias)
+		joint.jAcc = 0.0f;
+
+	// apply joint torque
+	a.w -= joint.jAcc*a.i_inv;
+	b.w += joint.jAcc*b.i_inv;
+}
+
+static void
+applyImpulse(cpRatchetJoint *joint)
+{
+	if(!joint.bias) return; // early exit
+
+	mixin(CONSTRAINT_BEGIN!("joint", "a", "b"));
+	
+	// compute relative rotational velocity
+	cpFloat wr = b.w - a.w;
+	cpFloat ratchet = joint.ratchet;
+	
+	// compute normal impulse	
+	cpFloat j = -(joint.bias + wr)*joint.iSum;
+	cpFloat jOld = joint.jAcc;
+	joint.jAcc = cpfclamp((jOld + j)*ratchet, 0.0f, joint.jMax*cpfabs(ratchet))/ratchet;
+	j = joint.jAcc - jOld;
+	
+	// apply impulse
+	a.w -= j*a.i_inv;
+	b.w += j*b.i_inv;
+}
+
+static cpFloat
+getImpulse(cpRatchetJoint *joint)
+{
+	return cpfabs(joint.jAcc);
+}
+
+static /+const+/ cpConstraintClass klass = {
+	cast(cpConstraintPreStepFunction)&preStep,
+	cast(cpConstraintApplyImpulseFunction)&applyImpulse,
+	cast(cpConstraintGetImpulseFunction)&getImpulse,
+};
+mixin(CP_DefineClassGetter!("cpRatchetJoint"));
+
+cpRatchetJoint *
+cpRatchetJointAlloc()
+{
+	return cast(cpRatchetJoint *)cpmalloc(cpRatchetJoint.sizeof);
+}
+
+cpRatchetJoint *
+cpRatchetJointInit(cpRatchetJoint *joint, cpBody *a, cpBody *b, cpFloat phase, cpFloat ratchet)
+{
+	cpConstraintInit(cast(cpConstraint *)joint, &klass, a, b);
+	
+	joint.angle = 0.0f;
+	joint.phase = phase;
+	joint.ratchet = ratchet;
+	
+	// STATIC_BODY_CHECK
+	joint.angle = (b ? b.a : 0.0f) - (a ? a.a : 0.0f);
+	
+	return joint;
+}
+
+cpConstraint *
+cpRatchetJointNew(cpBody *a, cpBody *b, cpFloat phase, cpFloat ratchet)
+{
+	return cast(cpConstraint *)cpRatchetJointInit(cpRatchetJointAlloc(), a, b, phase, ratchet);
+}