--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/trunk/chipmunkd/constraints/cpRotaryLimitJoint.d	Thu Dec 02 22:26:04 2010 +0100
@@ -0,0 +1,123 @@
+
+// written in the D programming language
+
+module chipmunkd.constraints.cpRotaryLimitJoint;
+
+import chipmunkd.chipmunk;
+import chipmunkd.constraints.util;
+
+//const cpConstraintClass *cpRotaryLimitJointGetClass();
+
+struct cpRotaryLimitJoint {
+	cpConstraint constraint;
+	cpFloat min, max;
+	
+	cpFloat iSum;
+		
+	cpFloat bias;
+	cpFloat jAcc, jMax;
+}
+
+//cpRotaryLimitJoint *cpRotaryLimitJointAlloc(void);
+//cpRotaryLimitJoint *cpRotaryLimitJointInit(cpRotaryLimitJoint *joint, cpBody *a, cpBody *b, cpFloat min, cpFloat max);
+//cpConstraint *cpRotaryLimitJointNew(cpBody *a, cpBody *b, cpFloat min, cpFloat max);
+//
+//CP_DefineConstraintProperty(cpRotaryLimitJoint, cpFloat, min, Min);
+//CP_DefineConstraintProperty(cpRotaryLimitJoint, cpFloat, max, Max);
+
+// cpRotaryLimitJoint.c ---------------------------------
+
+static void
+preStep(cpRotaryLimitJoint *joint, cpFloat dt, cpFloat dt_inv)
+{
+	mixin(CONSTRAINT_BEGIN!("joint", "a", "b"));
+	
+	cpFloat dist = b.a - a.a;
+	cpFloat pdist = 0.0f;
+	if(dist > joint.max) {
+		pdist = joint.max - dist;
+	} else if(dist < joint.min) {
+		pdist = joint.min - dist;
+	}
+	
+	// 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(cpRotaryLimitJoint *joint)
+{
+	if(!joint.bias) return; // early exit
+
+	mixin(CONSTRAINT_BEGIN!("joint", "a", "b"));
+	
+	// compute relative rotational velocity
+	cpFloat wr = b.w - a.w;
+	
+	// compute normal impulse	
+	cpFloat j = -(joint.bias + wr)*joint.iSum;
+	cpFloat jOld = joint.jAcc;
+	if(joint.bias < 0.0f){
+		joint.jAcc = cpfclamp(jOld + j, 0.0f, joint.jMax);
+	} else {
+		joint.jAcc = cpfclamp(jOld + j, -joint.jMax, 0.0f);
+	}
+	j = joint.jAcc - jOld;
+	
+	// apply impulse
+	a.w -= j*a.i_inv;
+	b.w += j*b.i_inv;
+}
+
+static cpFloat
+getImpulse(cpRotaryLimitJoint *joint)
+{
+	return cpfabs(joint.jAcc);
+}
+
+static /+const+/ cpConstraintClass klass = {
+	cast(cpConstraintPreStepFunction)&preStep,
+	cast(cpConstraintApplyImpulseFunction)&applyImpulse,
+	cast(cpConstraintGetImpulseFunction)&getImpulse,
+};
+mixin(CP_DefineClassGetter!("cpRotaryLimitJoint"));
+
+cpRotaryLimitJoint *
+cpRotaryLimitJointAlloc()
+{
+	return cast(cpRotaryLimitJoint *)cpmalloc(cpRotaryLimitJoint.sizeof);
+}
+
+cpRotaryLimitJoint *
+cpRotaryLimitJointInit(cpRotaryLimitJoint *joint, cpBody *a, cpBody *b, cpFloat min, cpFloat max)
+{
+	cpConstraintInit(cast(cpConstraint *)joint, &klass, a, b);
+	
+	joint.min = min;
+	joint.max  = max;
+	
+	joint.jAcc = 0.0f;
+	
+	return joint;
+}
+
+cpConstraint *
+cpRotaryLimitJointNew(cpBody *a, cpBody *b, cpFloat min, cpFloat max)
+{
+	return cast(cpConstraint *)cpRotaryLimitJointInit(cpRotaryLimitJointAlloc(), a, b, min, max);
+}