--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/trunk/chipmunkd/constraints/cpDampedSpring.d	Thu Dec 02 02:11:26 2010 +0100
@@ -0,0 +1,127 @@
+
+// written in the D programming language
+
+module chipmunkd.constraints.cpDampedSpring;
+
+import chipmunkd.chipmunk;
+import chipmunkd.constraints.util;
+
+alias cpFloat function(cpConstraint *spring, cpFloat dist) cpDampedSpringForceFunc;
+
+//const cpConstraintClass *cpDampedSpringGetClass();
+
+struct cpDampedSpring {
+	cpConstraint constraint;
+	cpVect anchr1, anchr2;
+	cpFloat restLength;
+	cpFloat stiffness;
+	cpFloat damping;
+	cpDampedSpringForceFunc springForceFunc;
+	
+	cpFloat target_vrn;
+	cpFloat v_coef;
+	
+	cpVect r1, r2;
+	cpFloat nMass;
+	cpVect n;
+}
+
+//cpDampedSpring *cpDampedSpringAlloc(void);
+//cpDampedSpring *cpDampedSpringInit(cpDampedSpring *joint, cpBody *a, cpBody *b, cpVect anchr1, cpVect anchr2, cpFloat restLength, cpFloat stiffness, cpFloat damping);
+//cpConstraint *cpDampedSpringNew(cpBody *a, cpBody *b, cpVect anchr1, cpVect anchr2, cpFloat restLength, cpFloat stiffness, cpFloat damping);
+
+mixin(CP_DefineConstraintProperty!("cpDampedSpring", "cpVect", "anchr1", "Anchr1"));
+mixin(CP_DefineConstraintProperty!("cpDampedSpring", "cpVect", "anchr2", "Anchr2"));
+mixin(CP_DefineConstraintProperty!("cpDampedSpring", "cpFloat", "restLength", "RestLength"));
+mixin(CP_DefineConstraintProperty!("cpDampedSpring", "cpFloat", "stiffness", "Stiffness"));
+mixin(CP_DefineConstraintProperty!("cpDampedSpring", "cpFloat", "damping", "Damping"));
+mixin(CP_DefineConstraintProperty!("cpDampedSpring", "cpDampedSpringForceFunc", "springForceFunc", "SpringForceFunc"));
+
+static cpFloat
+defaultSpringForce(cpDampedSpring *spring, cpFloat dist){
+	return (spring.restLength - dist)*spring.stiffness;
+}
+
+static void
+preStep(cpDampedSpring *spring, cpFloat dt, cpFloat dt_inv)
+{
+	mixin(CONSTRAINT_BEGIN!("spring", "a", "b"));
+	
+	spring.r1 = cpvrotate(spring.anchr1, a.rot);
+	spring.r2 = cpvrotate(spring.anchr2, b.rot);
+	
+	cpVect delta = cpvsub(cpvadd(b.p, spring.r2), cpvadd(a.p, spring.r1));
+	cpFloat dist = cpvlength(delta);
+	spring.n = cpvmult(delta, 1.0f/(dist ? dist : INFINITY));
+	
+	cpFloat k = k_scalar(a, b, spring.r1, spring.r2, spring.n);
+	spring.nMass = 1.0f/k;
+	
+	spring.target_vrn = 0.0f;
+	spring.v_coef = 1.0f - cpfexp(-spring.damping*dt*k);
+
+	// apply spring force
+	cpFloat f_spring = spring.springForceFunc(cast(cpConstraint *)spring, dist);
+	apply_impulses(a, b, spring.r1, spring.r2, cpvmult(spring.n, f_spring*dt));
+}
+
+static void
+applyImpulse(cpDampedSpring *spring)
+{
+	mixin(CONSTRAINT_BEGIN!("spring", "a", "b"));
+	
+	cpVect n = spring.n;
+	cpVect r1 = spring.r1;
+	cpVect r2 = spring.r2;
+
+	// compute relative velocity
+	cpFloat vrn = 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 v_damp = -vrn*spring.v_coef;
+	spring.target_vrn = vrn + v_damp;
+	
+	apply_impulses(a, b, spring.r1, spring.r2, cpvmult(spring.n, v_damp*spring.nMass));
+}
+
+static cpFloat
+getImpulse(cpConstraint *constraint)
+{
+	return 0.0f;
+}
+
+static /+const+/ cpConstraintClass klass = {
+	cast(cpConstraintPreStepFunction)&preStep,
+	cast(cpConstraintApplyImpulseFunction)&applyImpulse,
+	cast(cpConstraintGetImpulseFunction)&getImpulse,
+};
+mixin(CP_DefineClassGetter!("cpDampedSpring"));
+
+cpDampedSpring *
+cpDampedSpringAlloc()
+{
+	return cast(cpDampedSpring *)cpmalloc(cpDampedSpring.sizeof);
+}
+
+cpDampedSpring *
+cpDampedSpringInit(cpDampedSpring *spring, cpBody *a, cpBody *b, cpVect anchr1, cpVect anchr2, cpFloat restLength, cpFloat stiffness, cpFloat damping)
+{
+	cpConstraintInit(cast(cpConstraint *)spring, cpDampedSpringGetClass(), a, b);
+	
+	spring.anchr1 = anchr1;
+	spring.anchr2 = anchr2;
+	
+	spring.restLength = restLength;
+	spring.stiffness = stiffness;
+	spring.damping = damping;
+	spring.springForceFunc = cast(cpDampedSpringForceFunc)&defaultSpringForce;
+	
+	return spring;
+}
+
+cpConstraint *
+cpDampedSpringNew(cpBody *a, cpBody *b, cpVect anchr1, cpVect anchr2, cpFloat restLength, cpFloat stiffness, cpFloat damping)
+{
+	return cast(cpConstraint *)cpDampedSpringInit(cpDampedSpringAlloc(), a, b, anchr1, anchr2, restLength, stiffness, damping);
+}