--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/trunk/chipmunkd/constraints/cpDampedRotarySpring.d	Thu Dec 02 22:26:04 2010 +0100
@@ -0,0 +1,114 @@
+
+// written in the D programming language
+
+module chipmunkd.constraints.cpDampedRotarySpring;
+
+import chipmunkd.chipmunk;
+import chipmunkd.constraints.util;
+
+alias cpFloat function(cpConstraint *spring, cpFloat relativeAngle) cpDampedRotarySpringTorqueFunc;
+
+//const cpConstraintClass *cpDampedRotarySpringGetClass();
+
+struct cpDampedRotarySpring {
+	cpConstraint constraint;
+	cpFloat restAngle;
+	cpFloat stiffness;
+	cpFloat damping;
+	cpDampedRotarySpringTorqueFunc springTorqueFunc;
+	
+	cpFloat target_wrn;
+	cpFloat w_coef;
+	
+	cpFloat iSum;
+}
+
+//cpDampedRotarySpring *cpDampedRotarySpringAlloc(void);
+//cpDampedRotarySpring *cpDampedRotarySpringInit(cpDampedRotarySpring *joint, cpBody *a, cpBody *b, cpFloat restAngle, cpFloat stiffness, cpFloat damping);
+//cpConstraint *cpDampedRotarySpringNew(cpBody *a, cpBody *b, cpFloat restAngle, cpFloat stiffness, cpFloat damping);
+//
+//CP_DefineConstraintProperty(cpDampedRotarySpring, cpFloat, restAngle, RestAngle);
+//CP_DefineConstraintProperty(cpDampedRotarySpring, cpFloat, stiffness, Stiffness);
+//CP_DefineConstraintProperty(cpDampedRotarySpring, cpFloat, damping, Damping);
+//CP_DefineConstraintProperty(cpDampedRotarySpring, cpDampedRotarySpringTorqueFunc, springTorqueFunc, SpringTorqueFunc);
+
+// cpDampedRotarySpring.c ---------------------------------
+
+static cpFloat
+defaultSpringTorque(cpDampedRotarySpring *spring, cpFloat relativeAngle){
+	return (relativeAngle - spring.restAngle)*spring.stiffness;
+}
+
+static void
+preStep(cpDampedRotarySpring *spring, cpFloat dt, cpFloat dt_inv)
+{
+	mixin(CONSTRAINT_BEGIN!("spring", "a", "b"));
+	
+	cpFloat moment = a.i_inv + b.i_inv;
+	spring.iSum = 1.0f/moment;
+
+	spring.w_coef = 1.0f - cpfexp(-spring.damping*dt*moment);
+	spring.target_wrn = 0.0f;
+
+	// apply spring torque
+	cpFloat j_spring = spring.springTorqueFunc(cast(cpConstraint *)spring, a.a - b.a)*dt;
+	a.w -= j_spring*a.i_inv;
+	b.w += j_spring*b.i_inv;
+}
+
+static void
+applyImpulse(cpDampedRotarySpring *spring)
+{
+	mixin(CONSTRAINT_BEGIN!("spring", "a", "b"));
+	
+	// compute relative velocity
+	cpFloat wrn = a.w - b.w;//normal_relative_velocity(a, b, r1, r2, n) - spring.target_vrn;
+	
+	// compute velocity loss from drag
+	// not 100% certain this is derived correctly, though it makes sense
+	cpFloat w_damp = wrn*spring.w_coef;
+	spring.target_wrn = wrn - w_damp;
+	
+	//apply_impulses(a, b, spring.r1, spring.r2, cpvmult(spring.n, v_damp*spring.nMass));
+	cpFloat j_damp = w_damp*spring.iSum;
+	a.w -= j_damp*a.i_inv;
+	b.w += j_damp*b.i_inv;
+}
+
+static cpFloat
+getImpulse(cpConstraint *constraint)
+{
+	return 0.0f;
+}
+
+static /+const+/ cpConstraintClass klass = {
+	cast(cpConstraintPreStepFunction)&preStep,
+	cast(cpConstraintApplyImpulseFunction)&applyImpulse,
+	cast(cpConstraintGetImpulseFunction)&getImpulse,
+};
+mixin(CP_DefineClassGetter!("cpDampedRotarySpring"));
+
+cpDampedRotarySpring *
+cpDampedRotarySpringAlloc()
+{
+	return cast(cpDampedRotarySpring *)cpmalloc(cpDampedRotarySpring.sizeof);
+}
+
+cpDampedRotarySpring *
+cpDampedRotarySpringInit(cpDampedRotarySpring *spring, cpBody *a, cpBody *b, cpFloat restAngle, cpFloat stiffness, cpFloat damping)
+{
+	cpConstraintInit(cast(cpConstraint *)spring, &klass, a, b);
+	
+	spring.restAngle = restAngle;
+	spring.stiffness = stiffness;
+	spring.damping = damping;
+	spring.springTorqueFunc = cast(cpDampedRotarySpringTorqueFunc)&defaultSpringTorque;
+	
+	return spring;
+}
+
+cpConstraint *
+cpDampedRotarySpringNew(cpBody *a, cpBody *b, cpFloat restAngle, cpFloat stiffness, cpFloat damping)
+{
+	return cast(cpConstraint *)cpDampedRotarySpringInit(cpDampedRotarySpringAlloc(), a, b, restAngle, stiffness, damping);
+}