--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/trunk/chipmunkd/constraints/cpGrooveJoint.d	Thu Dec 02 22:26:04 2010 +0100
@@ -0,0 +1,175 @@
+
+// written in the D programming language
+
+module chipmunkd.constraints.cpGrooveJoint;
+
+import chipmunkd.chipmunk;
+import chipmunkd.constraints.util;
+
+//const cpConstraintClass *cpGrooveJointGetClass();
+
+struct cpGrooveJoint {
+	cpConstraint constraint;
+	cpVect grv_n, grv_a, grv_b;
+	cpVect  anchr2;
+	
+	cpVect grv_tn;
+	cpFloat clamp;
+	cpVect r1, r2;
+	cpVect k1, k2;
+	
+	cpVect jAcc;
+	cpFloat jMaxLen;
+	cpVect bias;
+}
+
+//cpGrooveJoint *cpGrooveJointAlloc(void);
+//cpGrooveJoint *cpGrooveJointInit(cpGrooveJoint *joint, cpBody *a, cpBody *b, cpVect groove_a, cpVect groove_b, cpVect anchr2);
+//cpConstraint *cpGrooveJointNew(cpBody *a, cpBody *b, cpVect groove_a, cpVect groove_b, cpVect anchr2);
+//
+//
+//CP_DefineConstraintGetter(cpGrooveJoint, cpVect, grv_a, GrooveA);
+//void cpGrooveJointSetGrooveA(cpConstraint *constraint, cpVect value);
+//CP_DefineConstraintGetter(cpGrooveJoint, cpVect, grv_b, GrooveB);
+//void cpGrooveJointSetGrooveB(cpConstraint *constraint, cpVect value);
+//CP_DefineConstraintProperty(cpGrooveJoint, cpVect, anchr2, Anchr2);
+
+// cpGrooveJoint.c ---------------------------------
+
+static void
+preStep(cpGrooveJoint *joint, cpFloat dt, cpFloat dt_inv)
+{
+	mixin(CONSTRAINT_BEGIN!("joint", "a", "b"));
+	
+	// calculate endpoints in worldspace
+	cpVect ta = cpBodyLocal2World(a, joint.grv_a);
+	cpVect tb = cpBodyLocal2World(a, joint.grv_b);
+
+	// calculate axis
+	cpVect n = cpvrotate(joint.grv_n, a.rot);
+	cpFloat d = cpvdot(ta, n);
+	
+	joint.grv_tn = n;
+	joint.r2 = cpvrotate(joint.anchr2, b.rot);
+	
+	// calculate tangential distance along the axis of r2
+	cpFloat td = cpvcross(cpvadd(b.p, joint.r2), n);
+	// calculate clamping factor and r2
+	if(td <= cpvcross(ta, n)){
+		joint.clamp = 1.0f;
+		joint.r1 = cpvsub(ta, a.p);
+	} else if(td >= cpvcross(tb, n)){
+		joint.clamp = -1.0f;
+		joint.r1 = cpvsub(tb, a.p);
+	} else {
+		joint.clamp = 0.0f;
+		joint.r1 = cpvsub(cpvadd(cpvmult(cpvperp(n), -td), cpvmult(n, d)), a.p);
+	}
+	
+	// Calculate mass tensor
+	k_tensor(a, b, joint.r1, joint.r2, &joint.k1, &joint.k2);	
+	
+	// compute max impulse
+	joint.jMaxLen = mixin(J_MAX!("joint", "dt"));
+	
+	// calculate bias velocity
+	cpVect delta = cpvsub(cpvadd(b.p, joint.r2), cpvadd(a.p, joint.r1));
+	joint.bias = cpvclamp(cpvmult(delta, -joint.constraint.biasCoef*dt_inv), joint.constraint.maxBias);
+	
+	// apply accumulated impulse
+	apply_impulses(a, b, joint.r1, joint.r2, joint.jAcc);
+}
+
+static cpVect
+grooveConstrain(cpGrooveJoint *joint, cpVect j){
+	cpVect n = joint.grv_tn;
+	cpVect jClamp = (joint.clamp*cpvcross(j, n) > 0.0f) ? j : cpvproject(j, n);
+	return cpvclamp(jClamp, joint.jMaxLen);
+}
+
+static void
+applyImpulse(cpGrooveJoint *joint)
+{
+	mixin(CONSTRAINT_BEGIN!("joint", "a", "b"));
+	
+	cpVect r1 = joint.r1;
+	cpVect r2 = joint.r2;
+	
+	// compute impulse
+	cpVect vr = relative_velocity(a, b, r1, r2);
+
+	cpVect j = mult_k(cpvsub(joint.bias, vr), joint.k1, joint.k2);
+	cpVect jOld = joint.jAcc;
+	joint.jAcc = grooveConstrain(joint, cpvadd(jOld, j));
+	j = cpvsub(joint.jAcc, jOld);
+	
+	// apply impulse
+	apply_impulses(a, b, joint.r1, joint.r2, j);
+}
+
+static cpFloat
+getImpulse(cpGrooveJoint *joint)
+{
+	return cpvlength(joint.jAcc);
+}
+
+static /+const+/ cpConstraintClass klass = {
+	cast(cpConstraintPreStepFunction)&preStep,
+	cast(cpConstraintApplyImpulseFunction)&applyImpulse,
+	cast(cpConstraintGetImpulseFunction)&getImpulse,
+};
+mixin(CP_DefineClassGetter!("cpGrooveJoint"));
+
+cpGrooveJoint *
+cpGrooveJointAlloc()
+{
+	return cast(cpGrooveJoint *)cpmalloc(cpGrooveJoint.sizeof);
+}
+
+cpGrooveJoint *
+cpGrooveJointInit(cpGrooveJoint *joint, cpBody *a, cpBody *b, cpVect groove_a, cpVect groove_b, cpVect anchr2)
+{
+	cpConstraintInit(cast(cpConstraint *)joint, &klass, a, b);
+	
+	joint.grv_a = groove_a;
+	joint.grv_b = groove_b;
+	joint.grv_n = cpvperp(cpvnormalize(cpvsub(groove_b, groove_a)));
+	joint.anchr2 = anchr2;
+	
+	joint.jAcc = cpvzero;
+	
+	return joint;
+}
+
+cpConstraint *
+cpGrooveJointNew(cpBody *a, cpBody *b, cpVect groove_a, cpVect groove_b, cpVect anchr2)
+{
+	return cast(cpConstraint *)cpGrooveJointInit(cpGrooveJointAlloc(), a, b, groove_a, groove_b, anchr2);
+}
+
+void
+cpGrooveJointSetGrooveA(cpConstraint *constraint, cpVect value)
+{
+	cpGrooveJoint *g = cast(cpGrooveJoint *)constraint;
+	//TODO:
+	//cpConstraintCheckCast(constraint, cpGrooveJoint);
+	
+	g.grv_a = value;
+	g.grv_n = cpvperp(cpvnormalize(cpvsub(g.grv_b, value)));
+	
+	cpConstraintActivateBodies(constraint);
+}
+
+void
+cpGrooveJointSetGrooveB(cpConstraint *constraint, cpVect value)
+{
+	cpGrooveJoint *g = cast(cpGrooveJoint *)constraint;
+	//TODO:
+	//cpConstraintCheckCast(constraint, cpGrooveJoint);
+	
+	g.grv_b = value;
+	g.grv_n = cpvperp(cpvnormalize(cpvsub(value, g.grv_a)));
+	
+	cpConstraintActivateBodies(constraint);
+}
+