# HG changeset patch
# User Extrawurst
# Date 1291325164 -3600
# Node ID 707dd4e10c282470d7c7fd6054230b1b44fcfffa
# Parent  6dd4bcf20f03bfa46d5271f8e074fe776f22664c
ported rest of the constraints (chipmunk 5.3.2)

diff -r 6dd4bcf20f03 -r 707dd4e10c28 trunk/chipmunkd/constraints/cpConstraint.d
--- a/trunk/chipmunkd/constraints/cpConstraint.d	Thu Dec 02 02:32:22 2010 +0100
+++ b/trunk/chipmunkd/constraints/cpConstraint.d	Thu Dec 02 22:26:04 2010 +0100
@@ -109,15 +109,15 @@
 //TODO:
 //// Built in Joint types
 public import chipmunkd.constraints.cpPinJoint;
-//#include "cpSlideJoint.h"
+public import chipmunkd.constraints.cpSlideJoint;
 public import chipmunkd.constraints.cpPivotJoint;
-//#include "cpGrooveJoint.h"
+public import chipmunkd.constraints.cpGrooveJoint;
 public import chipmunkd.constraints.cpDampedSpring;
-//#include "cpDampedRotarySpring.h"
-//#include "cpRotaryLimitJoint.h"
-//#include "cpRatchetJoint.h"
-//#include "cpGearJoint.h"
-//#include "cpSimpleMotor.h"
+public import chipmunkd.constraints.cpDampedRotarySpring;
+public import chipmunkd.constraints.cpRotaryLimitJoint;
+public import chipmunkd.constraints.cpRatchetJoint;
+public import chipmunkd.constraints.cpGearJoint;
+public import chipmunkd.constraints.cpSimpleMotor;
 
 
 cpFloat cp_constraint_bias_coef = 0.1f;
diff -r 6dd4bcf20f03 -r 707dd4e10c28 trunk/chipmunkd/constraints/cpDampedRotarySpring.d
--- /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);
+}
diff -r 6dd4bcf20f03 -r 707dd4e10c28 trunk/chipmunkd/constraints/cpGearJoint.d
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/trunk/chipmunkd/constraints/cpGearJoint.d	Thu Dec 02 22:26:04 2010 +0100
@@ -0,0 +1,119 @@
+
+// written in the D programming language
+
+module chipmunkd.constraints.cpGearJoint;
+
+import chipmunkd.chipmunk;
+import chipmunkd.constraints.util;
+
+//const cpConstraintClass *cpGearJointGetClass();
+
+struct cpGearJoint {
+	cpConstraint constraint;
+	cpFloat phase, ratio;
+	cpFloat ratio_inv;
+	
+	cpFloat iSum;
+		
+	cpFloat bias;
+	cpFloat jAcc, jMax;
+}
+
+//cpGearJoint *cpGearJointAlloc(void);
+//cpGearJoint *cpGearJointInit(cpGearJoint *joint, cpBody *a, cpBody *b, cpFloat phase, cpFloat ratio);
+//cpConstraint *cpGearJointNew(cpBody *a, cpBody *b, cpFloat phase, cpFloat ratio);
+//
+//CP_DefineConstraintProperty(cpGearJoint, cpFloat, phase, Phase);
+//CP_DefineConstraintGetter(cpGearJoint, cpFloat, ratio, Ratio);
+//void cpGearJointSetRatio(cpConstraint *constraint, cpFloat value);
+
+// cpGearJoint.c ---------------------------------
+
+static void
+preStep(cpGearJoint *joint, cpFloat dt, cpFloat dt_inv)
+{
+	mixin(CONSTRAINT_BEGIN!("joint", "a", "b"));
+	
+	// calculate moment of inertia coefficient.
+	joint.iSum = 1.0f/(a.i_inv*joint.ratio_inv + joint.ratio*b.i_inv);
+	
+	// calculate bias velocity
+	cpFloat maxBias = joint.constraint.maxBias;
+	joint.bias = cpfclamp(-joint.constraint.biasCoef*dt_inv*(b.a*joint.ratio - a.a - joint.phase), -maxBias, maxBias);
+	
+	// compute max impulse
+	joint.jMax = mixin(J_MAX!("joint", "dt"));
+
+	// apply joint torque
+	cpFloat j = joint.jAcc;
+	a.w -= j*a.i_inv*joint.ratio_inv;
+	b.w += j*b.i_inv;
+}
+
+static void
+applyImpulse(cpGearJoint *joint)
+{
+	mixin(CONSTRAINT_BEGIN!("joint", "a", "b"));
+	
+	// compute relative rotational velocity
+	cpFloat wr = b.w*joint.ratio - a.w;
+	
+	// compute normal impulse	
+	cpFloat j = (joint.bias - wr)*joint.iSum;
+	cpFloat jOld = joint.jAcc;
+	joint.jAcc = cpfclamp(jOld + j, -joint.jMax, joint.jMax);
+	j = joint.jAcc - jOld;
+	
+	// apply impulse
+	a.w -= j*a.i_inv*joint.ratio_inv;
+	b.w += j*b.i_inv;
+}
+
+static cpFloat
+getImpulse(cpGearJoint *joint)
+{
+	return cpfabs(joint.jAcc);
+}
+
+static /+const+/ cpConstraintClass klass = {
+	cast(cpConstraintPreStepFunction)&preStep,
+	cast(cpConstraintApplyImpulseFunction)&applyImpulse,
+	cast(cpConstraintGetImpulseFunction)&getImpulse,
+};
+mixin(CP_DefineClassGetter!("cpGearJoint"));
+
+cpGearJoint *
+cpGearJointAlloc()
+{
+	return cast(cpGearJoint *)cpmalloc(cpGearJoint.sizeof);
+}
+
+cpGearJoint *
+cpGearJointInit(cpGearJoint *joint, cpBody *a, cpBody *b, cpFloat phase, cpFloat ratio)
+{
+	cpConstraintInit(cast(cpConstraint *)joint, &klass, a, b);
+	
+	joint.phase = phase;
+	joint.ratio = ratio;
+	joint.ratio_inv = 1.0f/ratio;
+	
+	joint.jAcc = 0.0f;
+	
+	return joint;
+}
+
+cpConstraint *
+cpGearJointNew(cpBody *a, cpBody *b, cpFloat phase, cpFloat ratio)
+{
+	return cast(cpConstraint *)cpGearJointInit(cpGearJointAlloc(), a, b, phase, ratio);
+}
+
+void
+cpGearJointSetRatio(cpConstraint *constraint, cpFloat value)
+{
+	//TODO:
+	//cpConstraintCheckCast(constraint, cpGearJoint);
+	(cast(cpGearJoint *)constraint).ratio = value;
+	(cast(cpGearJoint *)constraint).ratio_inv = 1.0f/value;
+	cpConstraintActivateBodies(constraint);
+}
diff -r 6dd4bcf20f03 -r 707dd4e10c28 trunk/chipmunkd/constraints/cpGrooveJoint.d
--- /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);
+}
+
diff -r 6dd4bcf20f03 -r 707dd4e10c28 trunk/chipmunkd/constraints/cpRatchetJoint.d
--- /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);
+}
diff -r 6dd4bcf20f03 -r 707dd4e10c28 trunk/chipmunkd/constraints/cpRotaryLimitJoint.d
--- /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);
+}
diff -r 6dd4bcf20f03 -r 707dd4e10c28 trunk/chipmunkd/constraints/cpSimpleMotor.d
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/trunk/chipmunkd/constraints/cpSimpleMotor.d	Thu Dec 02 22:26:04 2010 +0100
@@ -0,0 +1,98 @@
+
+// written in the D programming language
+
+module chipmunkd.constraints.cpSimpleMotor;
+
+import chipmunkd.chipmunk;
+import chipmunkd.constraints.util;
+
+//const cpConstraintClass *cpSimpleMotorGetClass();
+
+struct cpSimpleMotor {
+	cpConstraint constraint;
+	cpFloat rate;
+	
+	cpFloat iSum;
+		
+	cpFloat jAcc, jMax;
+}
+
+//cpSimpleMotor *cpSimpleMotorAlloc(void);
+//cpSimpleMotor *cpSimpleMotorInit(cpSimpleMotor *joint, cpBody *a, cpBody *b, cpFloat rate);
+//cpConstraint *cpSimpleMotorNew(cpBody *a, cpBody *b, cpFloat rate);
+//
+//CP_DefineConstraintProperty(cpSimpleMotor, cpFloat, rate, Rate);
+
+// cpRotaryLimitJoint.c ---------------------------------
+
+static void
+preStep(cpSimpleMotor *joint, cpFloat dt, cpFloat dt_inv)
+{
+	mixin(CONSTRAINT_BEGIN!("joint", "a", "b"));
+	
+	// calculate moment of inertia coefficient.
+	joint.iSum = 1.0f/(a.i_inv + b.i_inv);
+	
+	// compute max impulse
+	joint.jMax = mixin(J_MAX!("joint", "dt"));
+
+	// apply joint torque
+	a.w -= joint.jAcc*a.i_inv;
+	b.w += joint.jAcc*b.i_inv;
+}
+
+static void
+applyImpulse(cpSimpleMotor *joint)
+{
+	mixin(CONSTRAINT_BEGIN!("joint", "a", "b"));
+	
+	// compute relative rotational velocity
+	cpFloat wr = b.w - a.w + joint.rate;
+	
+	// compute normal impulse	
+	cpFloat j = -wr*joint.iSum;
+	cpFloat jOld = joint.jAcc;
+	joint.jAcc = cpfclamp(jOld + j, -joint.jMax, joint.jMax);
+	j = joint.jAcc - jOld;
+	
+	// apply impulse
+	a.w -= j*a.i_inv;
+	b.w += j*b.i_inv;
+}
+
+static cpFloat
+getImpulse(cpSimpleMotor *joint)
+{
+	return cpfabs(joint.jAcc);
+}
+
+static /+const+/ cpConstraintClass klass = {
+	cast(cpConstraintPreStepFunction)&preStep,
+	cast(cpConstraintApplyImpulseFunction)&applyImpulse,
+	cast(cpConstraintGetImpulseFunction)&getImpulse,
+};
+mixin(CP_DefineClassGetter!("cpSimpleMotor"));
+
+cpSimpleMotor *
+cpSimpleMotorAlloc()
+{
+	return cast(cpSimpleMotor *)cpmalloc(cpSimpleMotor.sizeof);
+}
+
+cpSimpleMotor *
+cpSimpleMotorInit(cpSimpleMotor *joint, cpBody *a, cpBody *b, cpFloat rate)
+{
+	cpConstraintInit(cast(cpConstraint *)joint, &klass, a, b);
+	
+	joint.rate = rate;
+	
+	joint.jAcc = 0.0f;
+	
+	return joint;
+}
+
+cpConstraint *
+cpSimpleMotorNew(cpBody *a, cpBody *b, cpFloat rate)
+{
+	return cast(cpConstraint *)cpSimpleMotorInit(cpSimpleMotorAlloc(), a, b, rate);
+}
diff -r 6dd4bcf20f03 -r 707dd4e10c28 trunk/chipmunkd/constraints/cpSlideJoint.d
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/trunk/chipmunkd/constraints/cpSlideJoint.d	Thu Dec 02 22:26:04 2010 +0100
@@ -0,0 +1,137 @@
+
+// written in the D programming language
+
+module chipmunkd.constraints.cpSlideJoint;
+
+import chipmunkd.chipmunk;
+import chipmunkd.constraints.util;
+
+//const cpConstraintClass *cpSlideJointGetClass();
+
+struct cpSlideJoint {
+	cpConstraint constraint;
+	cpVect anchr1, anchr2;
+	cpFloat min, max;
+	
+	cpVect r1, r2;
+	cpVect n;
+	cpFloat nMass;
+	
+	cpFloat jnAcc, jnMax;
+	cpFloat bias;
+}
+
+//cpSlideJoint *cpSlideJointAlloc(void);
+//cpSlideJoint *cpSlideJointInit(cpSlideJoint *joint, cpBody *a, cpBody *b, cpVect anchr1, cpVect anchr2, cpFloat min, cpFloat max);
+//cpConstraint *cpSlideJointNew(cpBody *a, cpBody *b, cpVect anchr1, cpVect anchr2, cpFloat min, cpFloat max);
+//
+//CP_DefineConstraintProperty(cpSlideJoint, cpVect, anchr1, Anchr1);
+//CP_DefineConstraintProperty(cpSlideJoint, cpVect, anchr2, Anchr2);
+//CP_DefineConstraintProperty(cpSlideJoint, cpFloat, min, Min);
+//CP_DefineConstraintProperty(cpSlideJoint, cpFloat, max, Max);
+
+// cpSlideJoint.c ---------------------------------
+
+static void
+preStep(cpSlideJoint *joint, cpFloat dt, cpFloat dt_inv)
+{
+	mixin(CONSTRAINT_BEGIN!("joint", "a", "b"));
+	
+	joint.r1 = cpvrotate(joint.anchr1, a.rot);
+	joint.r2 = cpvrotate(joint.anchr2, b.rot);
+	
+	cpVect delta = cpvsub(cpvadd(b.p, joint.r2), cpvadd(a.p, joint.r1));
+	cpFloat dist = cpvlength(delta);
+	cpFloat pdist = 0.0f;
+	if(dist > joint.max) {
+		pdist = dist - joint.max;
+	} else if(dist < joint.min) {
+		pdist = joint.min - dist;
+		dist = -dist;
+	}
+	joint.n = cpvmult(delta, 1.0f/(dist ? dist : cast(cpFloat)INFINITY));
+	
+	// calculate mass normal
+	joint.nMass = 1.0f/k_scalar(a, b, joint.r1, joint.r2, joint.n);
+	
+	// calculate bias velocity
+	cpFloat maxBias = joint.constraint.maxBias;
+	joint.bias = cpfclamp(-joint.constraint.biasCoef*dt_inv*(pdist), -maxBias, maxBias);
+	
+	// compute max impulse
+	joint.jnMax = mixin(J_MAX!("joint", "dt"));
+
+	// apply accumulated impulse
+	if(!joint.bias) //{
+		// if bias is 0, then the joint is not at a limit.
+		joint.jnAcc = 0.0f;
+//	} else {
+		cpVect j = cpvmult(joint.n, joint.jnAcc);
+		apply_impulses(a, b, joint.r1, joint.r2, j);
+//	}
+}
+
+static void
+applyImpulse(cpSlideJoint *joint)
+{
+	if(!joint.bias) return;  // early exit
+
+	mixin(CONSTRAINT_BEGIN!("joint", "a", "b"));
+	
+	cpVect n = joint.n;
+	cpVect r1 = joint.r1;
+	cpVect r2 = joint.r2;
+		
+	// compute relative velocity
+	cpVect vr = relative_velocity(a, b, r1, r2);
+	cpFloat vrn = cpvdot(vr, n);
+	
+	// compute normal impulse
+	cpFloat jn = (joint.bias - vrn)*joint.nMass;
+	cpFloat jnOld = joint.jnAcc;
+	joint.jnAcc = cpfclamp(jnOld + jn, -joint.jnMax, 0.0f);
+	jn = joint.jnAcc - jnOld;
+	
+	// apply impulse
+	apply_impulses(a, b, joint.r1, joint.r2, cpvmult(n, jn));
+}
+
+static cpFloat
+getImpulse(cpConstraint *joint)
+{
+	return cpfabs((cast(cpSlideJoint *)joint).jnAcc);
+}
+
+static /+const+/ cpConstraintClass klass = {
+	cast(cpConstraintPreStepFunction)&preStep,
+	cast(cpConstraintApplyImpulseFunction)&applyImpulse,
+	cast(cpConstraintGetImpulseFunction)&getImpulse,
+};
+mixin(CP_DefineClassGetter!("cpSlideJoint"));
+
+cpSlideJoint *
+cpSlideJointAlloc()
+{
+	return cast(cpSlideJoint *)cpmalloc(cpSlideJoint.sizeof);
+}
+
+cpSlideJoint *
+cpSlideJointInit(cpSlideJoint *joint, cpBody *a, cpBody *b, cpVect anchr1, cpVect anchr2, cpFloat min, cpFloat max)
+{
+	cpConstraintInit(cast(cpConstraint *)joint, &klass, a, b);
+	
+	joint.anchr1 = anchr1;
+	joint.anchr2 = anchr2;
+	joint.min = min;
+	joint.max = max;
+	
+	joint.jnAcc = 0.0f;
+	
+	return joint;
+}
+
+cpConstraint *
+cpSlideJointNew(cpBody *a, cpBody *b, cpVect anchr1, cpVect anchr2, cpFloat min, cpFloat max)
+{
+	return cast(cpConstraint *)cpSlideJointInit(cpSlideJointAlloc(), a, b, anchr1, anchr2, min, max);
+}
\ No newline at end of file