--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/trunk/chipmunkd/chipmunk.d	Thu Dec 02 02:11:26 2010 +0100
@@ -0,0 +1,174 @@
+
+// written in the D programming language
+
+module chipmunkd.chipmunk;
+//#ifndef CHIPMUNK_HEADER
+//#define CHIPMUNK_HEADER
+//
+//#ifdef __cplusplus
+//extern "C" {
+//#endif
+//
+//void cpMessage(const char *message, const char *condition, const char *file, int line, int isError);
+//#ifdef NDEBUG
+//	#define	cpAssertWarn(condition, message)
+//#else
+//	#define cpAssertWarn(condition, message) if(!(condition)) cpMessage(message, #condition, __FILE__, __LINE__, 0)
+//#endif
+
+debug
+{
+	//TODO:
+	//void cpAssertWarn(bool condition, string message){if(condition) cpMessage(message,condition,__FILE__,__LINE__, 0);}
+	void cpAssertWarn(bool condition, string message){}
+}
+else
+{
+	void cpAssertWarn(bool condition, string message){}
+}
+
+//
+//#ifdef NDEBUG
+//	#define	cpAssert(condition, message)
+//#else
+//	#define cpAssert(condition, message) if(!(condition)) cpMessage(message, #condition, __FILE__, __LINE__, 1)
+//#endif
+//
+import chipmunkd.chipmunk_types_h;
+import core.stdc.stdlib;
+//	
+//#ifndef INFINITY
+//	#ifdef _MSC_VER
+//		union MSVC_EVIL_FLOAT_HACK
+//		{
+//			unsigned __int8 Bytes[4];
+//			float Value;
+//		};
+//		static union MSVC_EVIL_FLOAT_HACK INFINITY_HACK = {{0x00, 0x00, 0x80, 0x7F}};
+//		#define INFINITY (INFINITY_HACK.Value)
+//	#endif
+//	
+//	#ifdef __GNUC__
+//		#define INFINITY (__builtin_inf())
+//	#endif
+//	
+//	#ifndef INFINITY
+//		#define INFINITY (1e1000)
+//	#endif
+//#endif
+
+enum INFINITY = cpFloat.infinity;
+
+// Maximum allocated size for various Chipmunk buffer sizes
+enum CP_BUFFER_BYTES = (32*1024);
+
+alias core.stdc.stdlib.malloc cpmalloc;
+alias core.stdc.stdlib.calloc cpcalloc;
+alias core.stdc.stdlib.realloc cprealloc;
+alias core.stdc.stdlib.free cpfree;
+
+public import chipmunkd.cpVect_h,chipmunkd.cpVect;
+public import chipmunkd.cpBB;
+public import chipmunkd.cpArray;
+public import chipmunkd.cpHashSet;
+public import chipmunkd.cpSpaceHash;
+//
+public import chipmunkd.cpBody;
+public import chipmunkd.cpShape;
+public import chipmunkd.cpPolyShape;
+//
+public import chipmunkd.cpArbiter;
+public import chipmunkd.cpCollision;
+//	
+public import chipmunkd.constraints.cpConstraint;
+//
+public import chipmunkd.cpSpace;
+public import chipmunkd.cpSpaceComponent;
+public import chipmunkd.cpSpaceQuery;
+public import chipmunkd.cpSpaceStep;
+
+public import chipmunkd.chipmunk_types_h;
+
+enum cpHashValue CP_HASH_COEF = cast(cpHashValue)(3344921057uL); // ulong to uint ??
+static cpHashValue CP_HASH_PAIR(T)(T A, T B) {return (cast(cpHashValue)(A)*CP_HASH_COEF ^ cast(cpHashValue)(B)*CP_HASH_COEF);}
+
+void
+cpMessage(string message, string condition, string file, int line, int isError)
+{
+	//TODO:
+	//fprintf(stderr, (isError ? "Aborting due to Chipmunk error: %s\n" : "Chipmunk warning: %s\n"), message);
+	//fprintf(stderr, "\tFailed condition: %s\n", condition);
+	//fprintf(stderr, "\tSource:%s:%d\n", file, line);
+	
+	if(isError) abort();
+}
+
+extern const char *cpVersionString;
+void cpInitChipmunk()
+{
+	cpInitCollisionFuncs();
+}
+
+/**
+	Calculate the moment of inertia for a circle.
+	r1 and r2 are the inner and outer diameters. A solid circle has an inner diameter of 0.
+*/
+cpFloat
+cpMomentForCircle(cpFloat m, cpFloat r1, cpFloat r2, cpVect offset)
+{
+	return (1.0f/2.0f)*m*(r1*r1 + r2*r2) + m*cpvdot(offset, offset);
+}
+/**
+	Calculate the moment of inertia for a line segment.
+	Beveling radius is not supported.
+*/
+cpFloat
+cpMomentForSegment(cpFloat m, cpVect a, cpVect b)
+{
+	cpFloat length = cpvlength(cpvsub(b, a));
+	cpVect offset = cpvmult(cpvadd(a, b), 1.0f/2.0f);
+	
+	return m*length*length/12.0f + m*cpvdot(offset, offset);
+}
+
+/**
+	Calculate the moment of inertia for a solid polygon shape.
+*/
+cpFloat
+cpMomentForPoly(cpFloat m, const int numVerts, cpVect *verts, cpVect offset)
+{
+	cpVect *tVerts = cast(cpVect *)cpcalloc(numVerts, cpVect.sizeof);
+	for(int i=0; i<numVerts; i++)
+		tVerts[i] = cpvadd(verts[i], offset);
+	
+	cpFloat sum1 = 0.0f;
+	cpFloat sum2 = 0.0f;
+	for(int i=0; i<numVerts; i++){
+		cpVect v1 = tVerts[i];
+		cpVect v2 = tVerts[(i+1)%numVerts];
+		
+		cpFloat a = cpvcross(v2, v1);
+		cpFloat b = cpvdot(v1, v1) + cpvdot(v1, v2) + cpvdot(v2, v2);
+		
+		sum1 += a*b;
+		sum2 += a;
+	}
+	
+	cpfree(tVerts);
+	return (m*sum1)/(6.0f*sum2);
+}
+
+/**
+	Calculate the moment of inertia for a solid box.
+*/
+cpFloat
+cpMomentForBox(cpFloat m, cpFloat width, cpFloat height)
+{
+	return m*(width*width + height*height)/12.0f;
+}
+
+//#ifdef __cplusplus
+//}
+//#endif
+//
+//#endif

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/trunk/chipmunkd/chipmunk_types_h.d	Thu Dec 02 02:11:26 2010 +0100
@@ -0,0 +1,159 @@
+
+// written in the D programming language
+
+module chipmunkd.chipmunk_types_h;
+
+//#ifdef __APPLE__
+//   #import "TargetConditionals.h"
+//#endif
+//
+//#ifndef CP_USE_DOUBLES
+//  // Use single precision floats on the iPhone.
+//  #if TARGET_OS_IPHONE
+//    #define CP_USE_DOUBLES 0
+//  #else
+//    // use doubles by default for higher precision
+//    #define CP_USE_DOUBLES 1
+//  #endif
+//#endif
+//
+version(CP_USE_DOUBLES){
+	alias double cpFloat;
+	//TODO:
+//	#define cpfsqrt sqrt
+//	#define cpfsin sin
+//	#define cpfcos cos
+//	#define cpfacos acos
+//	#define cpfatan2 atan2
+//	#define cpfmod fmod
+//	#define cpfexp exp
+//	#define cpfpow pow
+//	#define cpffloor floor
+//	#define cpfceil ceil
+}else{
+	extern (C) nothrow {
+		float sqrtf(float);
+		float sinf(float);
+		float cosf(float);
+		float acosf(float);
+		float atan2f(float,float);
+		float fmodf(float,float);
+		float expf(float);
+		float powf(float,float);
+		float floorf(float);
+		float ceilf(float);
+	}
+	
+	alias float cpFloat;
+	
+	alias sqrtf cpfsqrt;
+	alias sinf cpfsin;
+	alias cosf cpfcos;
+	alias acosf cpfacos;
+	alias atan2f cpfatan2;
+	alias fmodf cpfmod;
+	alias expf cpfexp;
+	alias powf cpfpow;
+	alias floorf cpffloor;
+	alias ceilf cpfceil;
+}
+
+static cpFloat
+cpfmax(cpFloat a, cpFloat b)
+{
+	return (a > b) ? a : b;
+}
+
+static cpFloat
+cpfmin(cpFloat a, cpFloat b)
+{
+	return (a < b) ? a : b;
+}
+
+static cpFloat
+cpfabs(cpFloat n)
+{
+	return (n < 0) ? -n : n;
+}
+
+static cpFloat
+cpfclamp(cpFloat f, cpFloat min, cpFloat max)
+{
+	return cpfmin(cpfmax(f, min), max);
+}
+
+static cpFloat
+cpflerp(cpFloat f1, cpFloat f2, cpFloat t)
+{
+	return f1*(1.0f - t) + f2*t;
+}
+
+static cpFloat
+cpflerpconst(cpFloat f1, cpFloat f2, cpFloat d)
+{
+	return f1 + cpfclamp(f2 - f1, -d, d);
+}
+
+//#if TARGET_OS_IPHONE
+//	// CGPoints are structurally the same, and allow
+//	// easy interoperability with other iPhone libraries
+//	#import <CoreGraphics/CGGeometry.h>
+//	typedef CGPoint cpVect;
+//#else
+	struct cpVect{cpFloat x = 0; cpFloat y=0;}
+//#endif
+
+alias uint cpHashValue;
+
+// Oh C, how we love to define our own boolean types to get compiler compatibility
+//#ifdef CP_BOOL_TYPE
+//	typedef CP_BOOL_TYPE cpBool;
+//#else
+	alias bool cpBool;
+//#endif
+
+//#ifndef cpTrue
+	enum cpTrue = true;
+//#endif
+
+//#ifndef cpFalse
+	enum cpFalse = false;
+//#endif
+
+//#ifdef CP_DATA_POINTER_TYPE
+//	typedef CP_DATA_POINTER_TYPE cpDataPointer;
+//#else
+	alias void * cpDataPointer;
+//#endif
+
+//#ifdef CP_COLLISION_TYPE_TYPE
+//	typedef CP_COLLISION_TYPE_TYPE cpCollisionType;
+//#else
+	alias uint cpCollisionType;
+//#endif
+
+//#ifdef CP_GROUP_TYPE
+//	typedef CP_GROUP_TYPE cpGroup;
+//#else
+	alias uint cpGroup;
+//#endif
+
+//#ifdef CP_LAYERS_TYPE
+//	typedef CP_GROUP_TYPE cpLayers;
+//#else
+	alias uint cpLayers;
+//#endif
+
+//#ifdef CP_TIMESTAMP_TYPE
+//	typedef CP_TIMESTAMP_TYPE cpTimestamp;
+//#else
+	alias uint cpTimestamp;
+//#endif
+
+//#ifndef CP_NO_GROUP
+	enum CP_NO_GROUP = 0;
+//#endif
+
+//#ifndef CP_ALL_LAYERS
+	enum CP_ALL_LAYERS = ~0;
+//#endif

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/trunk/chipmunkd/constraints/cpConstraint.d	Thu Dec 02 02:11:26 2010 +0100
@@ -0,0 +1,146 @@
+
+// written in the D programming language
+
+module chipmunkd.constraints.cpConstraint;
+
+import chipmunkd.chipmunk;
+import chipmunkd.chipmunk_types_h;
+import chipmunkd.cpVect,chipmunkd.cpVect_h;
+import chipmunkd.cpBody;
+
+alias void function(cpConstraint *constraint, cpFloat dt, cpFloat dt_inv) cpConstraintPreStepFunction;
+alias void function(cpConstraint *constraint) cpConstraintApplyImpulseFunction;
+alias cpFloat function(cpConstraint *constraint) cpConstraintGetImpulseFunction;
+
+struct cpConstraintClass {
+	cpConstraintPreStepFunction preStep;
+	cpConstraintApplyImpulseFunction applyImpulse;
+	cpConstraintGetImpulseFunction getImpulse;
+}
+
+
+
+struct cpConstraint {
+	cpConstraintClass *klass;
+	
+	cpBody *a;
+	cpBody *b;
+	cpFloat maxForce;
+	cpFloat biasCoef;
+	cpFloat maxBias;
+	
+	cpDataPointer data;
+}
+
+//#ifdef CP_USE_DEPRECATED_API_4
+//typedef cpConstraint cpJoint;
+//#endif
+
+//void cpConstraintDestroy(cpConstraint *constraint);
+//void cpConstraintFree(cpConstraint *constraint);
+
+static void
+cpConstraintActivateBodies(cpConstraint *constraint)
+{
+	cpBody *a = constraint.a; if(a) cpBodyActivate(a);
+	cpBody *b = constraint.b; if(b) cpBodyActivate(b);
+}
+
+static cpFloat
+cpConstraintGetImpulse(cpConstraint *constraint)
+{
+	return constraint.klass.getImpulse(constraint);
+}
+
+template cpConstraintCheckCast(string constraint, string _struct)
+{
+	enum cpConstraintCheckCast = 
+		"assert("~constraint~".klass == "~_struct~"GetClass(), \"Constraint is not a "~_struct~"\");";
+}
+
+//#define cpConstraintCheckCast(constraint, struct) \
+//	cpAssert(constraint->klass == struct##GetClass(), "Constraint is not a "#struct);
+
+template CP_DefineConstraintGetter(string _struct,string type,string member,string name)
+{
+	enum CP_DefineConstraintGetter = 
+		"static "~type~" "~_struct~"Get"~name~"(cpConstraint *constraint){"~
+		cpConstraintCheckCast!("constraint",_struct)~
+		"cpConstraintActivateBodies(constraint);"~
+		"return (cast("~_struct~"*)constraint)."~member~";"~
+		"}";
+}
+
+//#define CP_DefineConstraintGetter(struct, type, member, name) \
+//static inline type \
+//struct##Get##name(const cpConstraint *constraint){ \
+//	cpConstraintCheckCast(constraint, struct); \
+//	return ((struct *)constraint)->member; \
+//} \
+
+template CP_DefineConstraintSetter(string _struct,string type,string member,string name)
+{
+	enum CP_DefineConstraintSetter = 
+		"static void "~_struct~"Set"~name~"(cpConstraint *constraint,"~type~" value){"~
+		cpConstraintCheckCast!("constraint",_struct)~
+		"cpConstraintActivateBodies(constraint);"~
+		"(cast("~_struct~"*)constraint)."~member~" = value;"~
+		"}";
+}
+
+//#define CP_DefineConstraintSetter(struct, type, member, name) \
+//static inline void \
+//struct##Set##name(cpConstraint *constraint, type value){ \
+//	cpConstraintCheckCast(constraint, struct); \
+//	cpConstraintActivateBodies(constraint); \
+//	((struct *)constraint)->member = value; \
+//} \
+
+template CP_DefineConstraintProperty(string _struct,string type,string member,string name)
+{
+	enum CP_DefineConstraintProperty = 
+		CP_DefineConstraintGetter!(_struct,type,member,name)~CP_DefineConstraintSetter!(_struct,type,member,name);
+		
+}
+
+//#define CP_DefineConstraintProperty(struct, type, member, name) \
+//CP_DefineConstraintGetter(struct, type, member, name) \
+//CP_DefineConstraintSetter(struct, type, member, name)
+//TODO:
+//// Built in Joint types
+public import chipmunkd.constraints.cpPinJoint;
+//#include "cpSlideJoint.h"
+public import chipmunkd.constraints.cpPivotJoint;
+//#include "cpGrooveJoint.h"
+public import chipmunkd.constraints.cpDampedSpring;
+//#include "cpDampedRotarySpring.h"
+//#include "cpRotaryLimitJoint.h"
+//#include "cpRatchetJoint.h"
+//#include "cpGearJoint.h"
+//#include "cpSimpleMotor.h"
+
+
+cpFloat cp_constraint_bias_coef = 0.1f;
+
+void cpConstraintDestroy(cpConstraint *constraint){}
+
+void
+cpConstraintFree(cpConstraint *constraint)
+{
+	if(constraint){
+		cpConstraintDestroy(constraint);
+		cpfree(constraint);
+	}
+}
+
+void
+cpConstraintInit(cpConstraint *constraint, /+const+/ cpConstraintClass *klass, cpBody *a, cpBody *b)
+{
+	constraint.klass = klass;
+	constraint.a = a;
+	constraint.b = b;
+	
+	constraint.maxForce = cast(cpFloat)INFINITY;
+	constraint.biasCoef = cp_constraint_bias_coef;
+	constraint.maxBias = cast(cpFloat)INFINITY;
+}

--- /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);
+}

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/trunk/chipmunkd/constraints/cpPinJoint.d	Thu Dec 02 02:11:26 2010 +0100
@@ -0,0 +1,124 @@
+
+// written in the D programming language
+
+module chipmunkd.constraints.cpPinJoint;
+
+import chipmunkd.chipmunk;
+import chipmunkd.chipmunk_types_h;
+import chipmunkd.cpVect,chipmunkd.cpVect_h;
+import chipmunkd.cpBody;
+import chipmunkd.constraints.util;
+
+//const cpConstraintClass *cpPinJointGetClass();
+
+struct cpPinJoint {
+	cpConstraint constraint;
+	cpVect anchr1, anchr2;
+	cpFloat dist;
+	
+	cpVect r1, r2;
+	cpVect n;
+	cpFloat nMass;
+	
+	cpFloat jnAcc, jnMax;
+	cpFloat bias;
+}
+
+//cpPinJoint *cpPinJointAlloc(void);
+//cpPinJoint *cpPinJointInit(cpPinJoint *joint, cpBody *a, cpBody *b, cpVect anchr1, cpVect anchr2);
+//cpConstraint *cpPinJointNew(cpBody *a, cpBody *b, cpVect anchr1, cpVect anchr2);
+//
+//CP_DefineConstraintProperty(cpPinJoint, cpVect, anchr1, Anchr1);
+//CP_DefineConstraintProperty(cpPinJoint, cpVect, anchr2, Anchr2);
+//CP_DefineConstraintProperty(cpPinJoint, cpFloat, dist, Dist);
+
+// cpPinJoint.c -------------------------
+
+static void
+preStep(cpPinJoint *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);
+	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*(dist - joint.dist), -maxBias, maxBias);
+	
+	// compute max impulse
+	joint.jnMax = mixin(J_MAX!("joint", "dt"));
+	
+	// apply accumulated impulse
+	cpVect j = cpvmult(joint.n, joint.jnAcc);
+	apply_impulses(a, b, joint.r1, joint.r2, j);
+}
+
+static void
+applyImpulse(cpPinJoint *joint)
+{
+	mixin(CONSTRAINT_BEGIN!("joint", "a", "b"));
+	cpVect n = joint.n;
+
+	// compute relative velocity
+	cpFloat vrn = normal_relative_velocity(a, b, joint.r1, joint.r2, n);
+	
+	// compute normal impulse
+	cpFloat jn = (joint.bias - vrn)*joint.nMass;
+	cpFloat jnOld = joint.jnAcc;
+	joint.jnAcc = cpfclamp(jnOld + jn, -joint.jnMax, joint.jnMax);
+	jn = joint.jnAcc - jnOld;
+	
+	// apply impulse
+	apply_impulses(a, b, joint.r1, joint.r2, cpvmult(n, jn));
+}
+
+static cpFloat
+getImpulse(cpPinJoint *joint)
+{
+	return cpfabs(joint.jnAcc);
+}
+
+static /+const+/ cpConstraintClass klass = {
+	cast(cpConstraintPreStepFunction)&preStep,
+	cast(cpConstraintApplyImpulseFunction)&applyImpulse,
+	cast(cpConstraintGetImpulseFunction)&getImpulse,
+};
+mixin(CP_DefineClassGetter!("cpPinJoint"));
+
+cpPinJoint *
+cpPinJointAlloc()
+{
+	return cast(cpPinJoint *)cpmalloc(cpPinJoint.sizeof);
+}
+
+cpPinJoint *
+cpPinJointInit(cpPinJoint *joint, cpBody *a, cpBody *b, cpVect anchr1, cpVect anchr2)
+{
+	cpConstraintInit(cast(cpConstraint *)joint, &klass, a, b);
+	
+	joint.anchr1 = anchr1;
+	joint.anchr2 = anchr2;
+	
+	// STATIC_BODY_CHECK
+	cpVect p1 = (a ? cpvadd(a.p, cpvrotate(anchr1, a.rot)) : anchr1);
+	cpVect p2 = (b ? cpvadd(b.p, cpvrotate(anchr2, b.rot)) : anchr2);
+	joint.dist = cpvlength(cpvsub(p2, p1));
+
+	joint.jnAcc = 0.0f;
+	
+	return joint;
+}
+
+cpConstraint *
+cpPinJointNew(cpBody *a, cpBody *b, cpVect anchr1, cpVect anchr2)
+{
+	return cast(cpConstraint *)cpPinJointInit(cpPinJointAlloc(), a, b, anchr1, anchr2);
+}

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/trunk/chipmunkd/constraints/cpPivotJoint.d	Thu Dec 02 02:11:26 2010 +0100
@@ -0,0 +1,118 @@
+
+// written in the D programming language
+
+module chipmunkd.constraints.cpPivotJoint;
+
+import chipmunkd.chipmunk;
+import chipmunkd.constraints.util;
+
+//const cpConstraintClass *cpPivotJointGetClass();
+
+struct cpPivotJoint {
+	cpConstraint constraint;
+	cpVect anchr1, anchr2;
+	
+	cpVect r1, r2;
+	cpVect k1, k2;
+	
+	cpVect jAcc;
+	cpFloat jMaxLen;
+	cpVect bias;
+}
+
+//cpPivotJoint *cpPivotJointAlloc(void);
+//cpPivotJoint *cpPivotJointInit(cpPivotJoint *joint, cpBody *a, cpBody *b, cpVect anchr1, cpVect anchr2);
+//cpConstraint *cpPivotJointNew(cpBody *a, cpBody *b, cpVect pivot);
+//cpConstraint *cpPivotJointNew2(cpBody *a, cpBody *b, cpVect anchr1, cpVect anchr2);
+//
+//CP_DefineConstraintProperty(cpPivotJoint, cpVect, anchr1, Anchr1);
+//CP_DefineConstraintProperty(cpPivotJoint, cpVect, anchr2, Anchr2);
+
+static void
+preStep(cpPivotJoint *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);
+	
+	// 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 void
+applyImpulse(cpPivotJoint *joint)
+{
+	mixin(CONSTRAINT_BEGIN!("joint", "a", "b"));
+	
+	cpVect r1 = joint.r1;
+	cpVect r2 = joint.r2;
+		
+	// compute relative velocity
+	cpVect vr = relative_velocity(a, b, r1, r2);
+	
+	// compute normal impulse
+	cpVect j = mult_k(cpvsub(joint.bias, vr), joint.k1, joint.k2);
+	cpVect jOld = joint.jAcc;
+	joint.jAcc = cpvclamp(cpvadd(joint.jAcc, j), joint.jMaxLen);
+	j = cpvsub(joint.jAcc, jOld);
+	
+	// apply impulse
+	apply_impulses(a, b, joint.r1, joint.r2, j);
+}
+
+static cpFloat
+getImpulse(cpConstraint *joint)
+{
+	return cpvlength((cast(cpPivotJoint *)joint).jAcc);
+}
+
+static /+const+/ cpConstraintClass klass = {
+	cast(cpConstraintPreStepFunction)&preStep,
+	cast(cpConstraintApplyImpulseFunction)&applyImpulse,
+	cast(cpConstraintGetImpulseFunction)&getImpulse,
+};
+mixin(CP_DefineClassGetter!("cpPivotJoint"));
+
+cpPivotJoint *
+cpPivotJointAlloc()
+{
+	return cast(cpPivotJoint *)cpmalloc(cpPivotJoint.sizeof);
+}
+
+cpPivotJoint *
+cpPivotJointInit(cpPivotJoint *joint, cpBody *a, cpBody *b, cpVect anchr1, cpVect anchr2)
+{
+	cpConstraintInit(cast(cpConstraint *)joint, &klass, a, b);
+	
+	joint.anchr1 = anchr1;
+	joint.anchr2 = anchr2;
+	
+	joint.jAcc = cpvzero;
+	
+	return joint;
+}
+
+cpConstraint *
+cpPivotJointNew2(cpBody *a, cpBody *b, cpVect anchr1, cpVect anchr2)
+{
+	return cast(cpConstraint *)cpPivotJointInit(cpPivotJointAlloc(), a, b, anchr1, anchr2);
+}
+
+cpConstraint *
+cpPivotJointNew(cpBody *a, cpBody *b, cpVect pivot)
+{
+	cpVect anchr1 = (a ? cpBodyWorld2Local(a, pivot) : pivot);
+	cpVect anchr2 = (b ? cpBodyWorld2Local(b, pivot) : pivot);
+	return cpPivotJointNew2(a, b, anchr1, anchr2);
+}

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/trunk/chipmunkd/constraints/util.d	Thu Dec 02 02:11:26 2010 +0100
@@ -0,0 +1,128 @@
+
+// written in the D programming language
+
+module chipmunkd.constraints.util;
+
+import chipmunkd.cpBody;
+import chipmunkd.chipmunk_types_h;
+import chipmunkd.cpVect,chipmunkd.cpVect_h;
+
+template CP_DefineClassGetter(string t)
+{
+	enum CP_DefineClassGetter = 
+		"cpConstraintClass* "~t~"GetClass(){return cast(cpConstraintClass *)&klass;}";
+}
+
+template J_MAX(string constraint,string dt)
+{
+	enum J_MAX = "((cast(cpConstraint *)"~constraint~").maxForce*("~dt~"))";
+}
+
+template CONSTRAINT_BEGIN(string constraint,string a_var,string b_var)
+{
+	enum CONSTRAINT_BEGIN = "cpBody *"~a_var~"; cpBody *"~b_var~";
+		"~a_var~" = (cast(cpConstraint *)"~constraint~")."~a_var~"; 
+		"~b_var~" = (cast(cpConstraint *)"~constraint~")."~b_var~"; 
+		if(
+			(cpBodyIsSleeping("~a_var~") || cpBodyIsStatic("~a_var~")) && 
+			(cpBodyIsSleeping("~b_var~") || cpBodyIsStatic("~b_var~")) 
+		) return;
+		";
+}
+
+static cpVect
+relative_velocity(cpBody *a, cpBody *b, cpVect r1, cpVect r2){
+	cpVect v1_sum = cpvadd(a.v, cpvmult(cpvperp(r1), a.w));
+	cpVect v2_sum = cpvadd(b.v, cpvmult(cpvperp(r2), b.w));
+	
+	return cpvsub(v2_sum, v1_sum);
+}
+
+static cpFloat
+normal_relative_velocity(cpBody *a, cpBody *b, cpVect r1, cpVect r2, cpVect n){
+	return cpvdot(relative_velocity(a, b, r1, r2), n);
+}
+
+static void
+apply_impulses(cpBody *a , cpBody *b, cpVect r1, cpVect r2, cpVect j)
+{
+	cpBodyApplyImpulse(a, cpvneg(j), r1);
+	cpBodyApplyImpulse(b, j, r2);
+}
+
+static void
+apply_bias_impulse(cpBody *_body, cpVect j, cpVect r)
+{
+	_body.v_bias = cpvadd(_body.v_bias, cpvmult(j, _body.m_inv));
+	_body.w_bias += _body.i_inv*cpvcross(r, j);
+}
+
+static void
+apply_bias_impulses(cpBody *a , cpBody *b, cpVect r1, cpVect r2, cpVect j)
+{
+	apply_bias_impulse(a, cpvneg(j), r1);
+	apply_bias_impulse(b, j, r2);
+}
+
+static cpVect
+clamp_vect(cpVect v, cpFloat len)
+{
+	return cpvclamp(v, len);
+//	return (cpvdot(v,v) > len*len) ? cpvmult(cpvnormalize(v), len) : v;
+}
+
+static cpFloat
+k_scalar(cpBody *a, cpBody *b, cpVect r1, cpVect r2, cpVect n)
+{
+	cpFloat mass_sum = a.m_inv + b.m_inv;
+	cpFloat r1cn = cpvcross(r1, n);
+	cpFloat r2cn = cpvcross(r2, n);
+	
+	cpFloat value = mass_sum + a.i_inv*r1cn*r1cn + b.i_inv*r2cn*r2cn;
+	assert(value != 0.0, "Unsolvable collision or constraint.");
+	
+	return value;
+}
+
+static void
+k_tensor(cpBody *a, cpBody *b, cpVect r1, cpVect r2, cpVect *k1, cpVect *k2)
+{
+	// calculate mass matrix
+	// If I wasn't lazy and wrote a proper matrix class, this wouldn't be so gross...
+	cpFloat k11, k12, k21, k22;
+	cpFloat m_sum = a.m_inv + b.m_inv;
+	
+	// start with I*m_sum
+	k11 = m_sum; k12 = 0.0f;
+	k21 = 0.0f;  k22 = m_sum;
+	
+	// add the influence from r1
+	cpFloat a_i_inv = a.i_inv;
+	cpFloat r1xsq =  r1.x * r1.x * a_i_inv;
+	cpFloat r1ysq =  r1.y * r1.y * a_i_inv;
+	cpFloat r1nxy = -r1.x * r1.y * a_i_inv;
+	k11 += r1ysq; k12 += r1nxy;
+	k21 += r1nxy; k22 += r1xsq;
+	
+	// add the influnce from r2
+	cpFloat b_i_inv = b.i_inv;
+	cpFloat r2xsq =  r2.x * r2.x * b_i_inv;
+	cpFloat r2ysq =  r2.y * r2.y * b_i_inv;
+	cpFloat r2nxy = -r2.x * r2.y * b_i_inv;
+	k11 += r2ysq; k12 += r2nxy;
+	k21 += r2nxy; k22 += r2xsq;
+	
+	// invert
+	cpFloat determinant = k11*k22 - k12*k21;
+	assert(determinant != 0.0, "Unsolvable constraint.");
+	
+	cpFloat det_inv = 1.0f/determinant;
+	*k1 = cpv( k22*det_inv, -k12*det_inv);
+	*k2 = cpv(-k21*det_inv,  k11*det_inv);
+}
+
+static cpVect
+mult_k(cpVect vr, cpVect k1, cpVect k2)
+{
+	return cpv(cpvdot(vr, k1), cpvdot(vr, k2));
+}

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/trunk/chipmunkd/cpArbiter.d	Thu Dec 02 02:11:26 2010 +0100
@@ -0,0 +1,394 @@
+
+// written in the D programming language
+
+module chipmunkd.cpArbiter;
+
+import chipmunkd.cpSpace;
+import chipmunkd.cpBody;
+import chipmunkd.cpShape;
+import chipmunkd.chipmunk;
+import chipmunkd.chipmunk_types_h;
+import chipmunkd.cpVect,chipmunkd.cpVect_h;
+import chipmunkd.constraints.util;
+
+// Determines how fast penetrations resolve themselves.
+//extern cpFloat cp_bias_coef;
+// Amount of allowed penetration. Used to reduce vibrating contacts.
+//extern cpFloat cp_collision_slop;
+
+// Data structure for contact points.
+struct cpContact {
+	// Contact point and normal.
+	cpVect p, n;
+	// Penetration distance.
+	cpFloat dist;
+	
+	// Calculated by cpArbiterPreStep().
+	cpVect r1, r2;
+	cpFloat nMass, tMass, bounce;
+
+	// Persistant contact information.
+	cpFloat jnAcc, jtAcc, jBias;
+	cpFloat bias;
+	
+	// Hash value used to (mostly) uniquely identify a contact.
+	cpHashValue hash;
+}
+
+//// Contacts are always allocated in groups.
+//cpContact* cpContactInit(cpContact *con, cpVect p, cpVect n, cpFloat dist, cpHashValue hash);
+//
+//// Sum the contact impulses. (Can be used after cpSpaceStep() returns)
+//cpVect cpContactsSumImpulses(cpContact *contacts, int numContacts);
+//cpVect cpContactsSumImpulsesWithFriction(cpContact *contacts, int numContacts);
+
+enum cpArbiterState {
+	cpArbiterStateNormal,
+	cpArbiterStateFirstColl,
+	cpArbiterStateIgnore,
+	cpArbiterStateSleep,
+	cpArbiterStateCached,
+}
+
+// Data structure for tracking collisions between shapes.
+struct cpArbiter {
+	// Information on the contact points between the objects.
+	int numContacts;
+	cpContact *contacts;
+	
+	// The two shapes and bodies involved in the collision.
+	// These variables are NOT in the order defined by the collision handler.
+	// Using CP_ARBITER_GET_SHAPES and CP_ARBITER_GET_BODIES will save you from
+	// many headaches
+	cpShape* private_a;
+	cpShape* private_b;
+	
+	// Calculated before calling the pre-solve collision handler
+	// Override them with custom values if you want specialized behavior
+	cpFloat e;
+	cpFloat u;
+	 // Used for surface_v calculations, implementation may change
+	cpVect surface_vr;
+	
+	// Time stamp of the arbiter. (from cpSpace)
+	cpTimestamp stamp;
+	
+	cpCollisionHandler *handler;
+	
+	// Are the shapes swapped in relation to the collision handler?
+	cpBool swappedColl;
+	cpArbiterState state;
+}
+
+//// Arbiters are allocated in large buffers by the space and don't require a destroy function
+//cpArbiter* cpArbiterInit(cpArbiter *arb, cpShape *a, cpShape *b);
+//
+//// These functions are all intended to be used internally.
+//// Inject new contact points into the arbiter while preserving contact history.
+//void cpArbiterUpdate(cpArbiter *arb, cpContact *contacts, int numContacts, struct cpCollisionHandler *handler, cpShape *a, cpShape *b);
+//// Precalculate values used by the solver.
+//void cpArbiterPreStep(cpArbiter *arb, cpFloat dt_inv);
+//void cpArbiterApplyCachedImpulse(cpArbiter *arb);
+//// Run an iteration of the solver on the arbiter.
+//void cpArbiterApplyImpulse(cpArbiter *arb, cpFloat eCoef);
+//
+//// Arbiter Helper Functions
+//cpVect cpArbiterTotalImpulse(cpArbiter *arb);
+//cpVect cpArbiterTotalImpulseWithFriction(cpArbiter *arb);
+//void cpArbiterIgnore(cpArbiter *arb);
+
+
+static void
+cpArbiterGetShapes(/+const+/ cpArbiter *arb, cpShape **a, cpShape **b)
+{
+	if(arb.swappedColl){
+		(*a) = arb.private_b, (*b) = arb.private_a;
+	} else {
+		(*a) = arb.private_a, (*b) = arb.private_b;
+	}
+}
+//#define CP_ARBITER_GET_SHAPES(arb, a, b) cpShape *a, *b; cpArbiterGetShapes(arb, &a, &b);
+template CP_ARBITER_GET_SHAPES(string arb,string a,string b)
+{
+	enum CP_ARBITER_GET_SHAPES = "cpShape* "~a~", "~b~";"~
+		"cpArbiterGetShapes("~arb~", &"~a~", &"~b~");";
+}
+
+static void
+cpArbiterGetBodies(/+const+/ cpArbiter *arb, cpBody **a, cpBody **b)
+{
+	//CP_ARBITER_GET_SHAPES(arb, shape_a, shape_b);
+	cpShape *shape_a, shape_b; cpArbiterGetShapes(arb, &shape_a, &shape_b);
+	(*a) = shape_a._body;
+	(*b) = shape_b._body;
+}
+//#define CP_ARBITER_GET_BODIES(arb, a, b) cpBody *a, *b; cpArbiterGetBodies(arb, &a, &b);
+
+static cpBool
+cpArbiterIsFirstContact(const cpArbiter *arb)
+{
+	return arb.state == cpArbiterState.cpArbiterStateFirstColl;
+}
+
+static cpVect
+cpArbiterGetNormal(const cpArbiter *arb, int i)
+{
+	cpVect n = arb.contacts[i].n;
+	return arb.swappedColl ? cpvneg(n) : n;
+}
+
+static cpVect
+cpArbiterGetPoint(const cpArbiter *arb, int i)
+{
+	return arb.contacts[i].p;
+}
+
+cpFloat cp_bias_coef = 0.1f;
+cpFloat cp_collision_slop = 0.1f;
+
+cpContact*
+cpContactInit(cpContact *con, cpVect p, cpVect n, cpFloat dist, cpHashValue hash)
+{
+	con.p = p;
+	con.n = n;
+	con.dist = dist;
+	
+	con.jnAcc = 0.0f;
+	con.jtAcc = 0.0f;
+	con.jBias = 0.0f;
+	
+	con.hash = hash;
+		
+	return con;
+}
+
+cpVect
+cpArbiterTotalImpulse(cpArbiter *arb)
+{
+	cpContact *contacts = arb.contacts;
+	cpVect sum = cpvzero;
+	
+	for(int i=0, count=arb.numContacts; i<count; i++){
+		cpContact *con = &contacts[i];
+		sum = cpvadd(sum, cpvmult(con.n, con.jnAcc));
+	}
+		
+	return sum;
+}
+
+cpVect
+cpArbiterTotalImpulseWithFriction(cpArbiter *arb)
+{
+	cpContact *contacts = arb.contacts;
+	cpVect sum = cpvzero;
+	
+	for(int i=0, count=arb.numContacts; i<count; i++){
+		cpContact *con = &contacts[i];
+		sum = cpvadd(sum, cpvrotate(con.n, cpv(con.jnAcc, con.jtAcc)));
+	}
+		
+	return sum;
+}
+
+cpFloat
+cpContactsEstimateCrushingImpulse(cpContact *contacts, int numContacts)
+{
+	cpFloat fsum = 0.0f;
+	cpVect vsum = cpvzero;
+	
+	for(int i=0; i<numContacts; i++){
+		cpContact *con = &contacts[i];
+		cpVect j = cpvrotate(con.n, cpv(con.jnAcc, con.jtAcc));
+		
+		fsum += cpvlength(j);
+		vsum = cpvadd(vsum, j);
+	}
+	
+	cpFloat vmag = cpvlength(vsum);
+	return (1.0f - vmag/fsum);
+}
+
+void
+cpArbiterIgnore(cpArbiter *arb)
+{
+	arb.state = cpArbiterState.cpArbiterStateIgnore;
+}
+
+cpArbiter*
+cpArbiterAlloc()
+{
+	return cast(cpArbiter *)cpcalloc(1, cpArbiter.sizeof);
+}
+
+cpArbiter*
+cpArbiterInit(cpArbiter *arb, cpShape *a, cpShape *b)
+{
+	arb.numContacts = 0;
+	arb.contacts = null;
+	
+	arb.private_a = a;
+	arb.private_b = b;
+	
+	arb.stamp = 0;
+	arb.state = cpArbiterState.cpArbiterStateFirstColl;
+	
+	return arb;
+}
+
+cpArbiter*
+cpArbiterNew(cpShape *a, cpShape *b)
+{
+	return cpArbiterInit(cpArbiterAlloc(), a, b);
+}
+
+void
+cpArbiterDestroy(cpArbiter *arb)
+{
+//	if(arb.contacts) cpfree(arb.contacts);
+}
+
+void
+cpArbiterFree(cpArbiter *arb)
+{
+	if(arb){
+		cpArbiterDestroy(arb);
+		cpfree(arb);
+	}
+}
+
+void
+cpArbiterUpdate(cpArbiter *arb, cpContact *contacts, int numContacts, cpCollisionHandler *handler, cpShape *a, cpShape *b)
+{
+	// Arbiters without contact data may exist if a collision function rejected the collision.
+	if(arb.contacts){
+		// Iterate over the possible pairs to look for hash value matches.
+		for(int i=0; i<arb.numContacts; i++){
+			cpContact *old = &arb.contacts[i];
+			
+			for(int j=0; j<numContacts; j++){
+				cpContact *new_contact = &contacts[j];
+				
+				// This could trigger false positives, but is fairly unlikely nor serious if it does.
+				if(new_contact.hash == old.hash){
+					// Copy the persistant contact information.
+					new_contact.jnAcc = old.jnAcc;
+					new_contact.jtAcc = old.jtAcc;
+				}
+			}
+		}
+
+//		cpfree(arb.contacts);
+	}
+	
+	arb.contacts = contacts;
+	arb.numContacts = numContacts;
+	
+	arb.handler = handler;
+	arb.swappedColl = (a.collision_type != handler.a);
+	
+	arb.e = a.e * b.e;
+	arb.u = a.u * b.u;
+	arb.surface_vr = cpvsub(a.surface_v, b.surface_v);
+	
+	// For collisions between two similar primitive types, the order could have been swapped.
+	arb.private_a = a;
+	arb.private_b = b;
+	
+	// mark it as new if it's been cached
+	if(arb.state == cpArbiterState.cpArbiterStateCached) arb.state = cpArbiterState.cpArbiterStateFirstColl;
+}
+
+void
+cpArbiterPreStep(cpArbiter *arb, cpFloat dt_inv)
+{
+	cpBody *a = arb.private_a._body;
+	cpBody *b = arb.private_b._body;
+	
+	for(int i=0; i<arb.numContacts; i++){
+		cpContact *con = &arb.contacts[i];
+		
+		// Calculate the offsets.
+		con.r1 = cpvsub(con.p, a.p);
+		con.r2 = cpvsub(con.p, b.p);
+		
+		// Calculate the mass normal and mass tangent.
+		con.nMass = 1.0f/k_scalar(a, b, con.r1, con.r2, con.n);
+		con.tMass = 1.0f/k_scalar(a, b, con.r1, con.r2, cpvperp(con.n));
+				
+		// Calculate the target bias velocity.
+		con.bias = -cp_bias_coef*dt_inv*cpfmin(0.0f, con.dist + cp_collision_slop);
+		con.jBias = 0.0f;
+		
+		// Calculate the target bounce velocity.
+		con.bounce = normal_relative_velocity(a, b, con.r1, con.r2, con.n)*arb.e;//cpvdot(con.n, cpvsub(v2, v1))*e;
+	}
+}
+
+void
+cpArbiterApplyCachedImpulse(cpArbiter *arb)
+{
+	cpShape *shapea = arb.private_a;
+	cpShape *shapeb = arb.private_b;
+		
+	arb.u = shapea.u * shapeb.u;
+	arb.surface_vr = cpvsub(shapeb.surface_v, shapea.surface_v);
+
+	cpBody *a = shapea._body;
+	cpBody *b = shapeb._body;
+	
+	for(int i=0; i<arb.numContacts; i++){
+		cpContact *con = &arb.contacts[i];
+		apply_impulses(a, b, con.r1, con.r2, cpvrotate(con.n, cpv(con.jnAcc, con.jtAcc)));
+	}
+}
+
+void
+cpArbiterApplyImpulse(cpArbiter *arb, cpFloat eCoef)
+{
+	cpBody *a = arb.private_a._body;
+	cpBody *b = arb.private_b._body;
+
+	for(int i=0; i<arb.numContacts; i++){
+		cpContact *con = &arb.contacts[i];
+		cpVect n = con.n;
+		cpVect r1 = con.r1;
+		cpVect r2 = con.r2;
+		
+		// Calculate the relative bias velocities.
+		cpVect vb1 = cpvadd(a.v_bias, cpvmult(cpvperp(r1), a.w_bias));
+		cpVect vb2 = cpvadd(b.v_bias, cpvmult(cpvperp(r2), b.w_bias));
+		cpFloat vbn = cpvdot(cpvsub(vb2, vb1), n);
+		
+		// Calculate and clamp the bias impulse.
+		cpFloat jbn = (con.bias - vbn)*con.nMass;
+		cpFloat jbnOld = con.jBias;
+		con.jBias = cpfmax(jbnOld + jbn, 0.0f);
+		jbn = con.jBias - jbnOld;
+		
+		// Apply the bias impulse.
+		apply_bias_impulses(a, b, r1, r2, cpvmult(n, jbn));
+
+		// Calculate the relative velocity.
+		cpVect vr = relative_velocity(a, b, r1, r2);
+		cpFloat vrn = cpvdot(vr, n);
+		
+		// Calculate and clamp the normal impulse.
+		cpFloat jn = -(con.bounce*eCoef + vrn)*con.nMass;
+		cpFloat jnOld = con.jnAcc;
+		con.jnAcc = cpfmax(jnOld + jn, 0.0f);
+		jn = con.jnAcc - jnOld;
+		
+		// Calculate the relative tangent velocity.
+		cpFloat vrt = cpvdot(cpvadd(vr, arb.surface_vr), cpvperp(n));
+		
+		// Calculate and clamp the friction impulse.
+		cpFloat jtMax = arb.u*con.jnAcc;
+		cpFloat jt = -vrt*con.tMass;
+		cpFloat jtOld = con.jtAcc;
+		con.jtAcc = cpfclamp(jtOld + jt, -jtMax, jtMax);
+		jt = con.jtAcc - jtOld;
+		
+		// Apply the final impulse.
+		apply_impulses(a, b, r1, r2, cpvrotate(n, cpv(jn, jt)));
+	}
+}

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/trunk/chipmunkd/cpArray.d	Thu Dec 02 02:11:26 2010 +0100
@@ -0,0 +1,133 @@
+
+// written in the D programming language
+
+module chipmunkd.cpArray;
+
+import chipmunkd.chipmunk;
+import chipmunkd.chipmunk_types_h;
+
+import core.stdc.string:memcpy;
+
+struct cpArray{
+	int num, max;
+	void **arr;
+}
+
+alias void function(void *ptr, void *data) cpArrayIter;
+
+//#define CP_ARRAY_INCREMENT 10
+
+// NOTE: cpArray is rarely used and will probably go away.
+
+cpArray*
+cpArrayAlloc()
+{
+	return cast(cpArray *)cpcalloc(1, cpArray.sizeof);
+}
+
+cpArray*
+cpArrayInit(cpArray *arr, int size)
+{
+	arr.num = 0;
+	
+	size = (size ? size : 4);
+	arr.max = size;
+	arr.arr = cast(void **)cpmalloc(size*(void**).sizeof);
+	
+	return arr;
+}
+
+cpArray*
+cpArrayNew(int size)
+{
+	return cpArrayInit(cpArrayAlloc(), size);
+}
+
+void
+cpArrayDestroy(cpArray *arr)
+{
+	cpfree(arr.arr);
+	arr.arr = null;
+}
+
+void
+cpArrayFree(cpArray *arr)
+{
+	if(arr){
+		cpArrayDestroy(arr);
+		cpfree(arr);
+	}
+}
+
+void
+cpArrayPush(cpArray *arr, void *object)
+{
+	if(arr.num == arr.max){
+		arr.max *= 2;
+		arr.arr = cast(void **)cprealloc(arr.arr, arr.max*(void**).sizeof);
+	}
+	
+	arr.arr[arr.num] = object;
+	arr.num++;
+}
+
+void *
+cpArrayPop(cpArray *arr)
+{
+	arr.num--;
+	
+	void *value = arr.arr[arr.num];
+	arr.arr[arr.num] = null;
+	
+	return value;
+}
+
+void
+cpArrayDeleteIndex(cpArray *arr, int idx)
+{
+	arr.num--;
+	
+	arr.arr[idx] = arr.arr[arr.num];
+	arr.arr[arr.num] = null;
+}
+
+void
+cpArrayDeleteObj(cpArray *arr, void *obj)
+{
+	for(int i=0; i<arr.num; i++){
+		if(arr.arr[i] == obj){
+			cpArrayDeleteIndex(arr, i);
+			return;
+		}
+	}
+}
+
+void
+cpArrayAppend(cpArray *arr, cpArray *other)
+{
+	void *tail = &arr.arr[arr.num];
+	
+	arr.num += other.num;
+	if(arr.num >= arr.max){
+		arr.max = arr.num;
+		arr.arr = cast(void **)cprealloc(arr.arr, arr.max*(void**).sizeof);
+	}
+	
+	memcpy(tail, other.arr, other.num*(void**).sizeof);
+}
+
+void
+cpArrayEach(cpArray *arr, cpArrayIter iterFunc, void *data)
+{
+	for(int i=0; i<arr.num; i++)
+		iterFunc(arr.arr[i], data);
+}
+
+cpBool
+cpArrayContains(cpArray *arr, void *ptr)
+{
+	for(int i=0; i<arr.num; i++)
+		if(arr.arr[i] == ptr) return cpTrue;
+	
+	return cpFalse;
+}

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/trunk/chipmunkd/cpBB.d	Thu Dec 02 02:11:26 2010 +0100
@@ -0,0 +1,79 @@
+
+// written in the D programming language
+
+module chipmunkd.cpBB;
+
+import chipmunkd.cpVect_h;
+import chipmunkd.chipmunk_types_h;
+
+struct cpBB{
+	cpFloat l, b, r ,t;
+}
+
+static cpBB
+cpBBNew(const cpFloat l, const cpFloat b,
+		const cpFloat r, const cpFloat t)
+{
+	cpBB bb = {l, b, r, t};
+	return bb;
+}
+
+static cpBool
+cpBBintersects(const cpBB a, const cpBB b)
+{
+	return (a.l<=b.r && b.l<=a.r && a.b<=b.t && b.b<=a.t);
+}
+
+static cpBool
+cpBBcontainsBB(const cpBB bb, const cpBB other)
+{
+	return (bb.l < other.l && bb.r > other.r && bb.b < other.b && bb.t > other.t);
+}
+
+static cpBool
+cpBBcontainsVect(const cpBB bb, const cpVect v)
+{
+	return (bb.l < v.x && bb.r > v.x && bb.b < v.y && bb.t > v.y);
+}
+
+static cpBB
+cpBBmerge(const cpBB a, const cpBB b){
+	return cpBBNew(
+		cpfmin(a.l, b.l),
+		cpfmin(a.b, b.b),
+		cpfmax(a.r, b.r),
+		cpfmax(a.t, b.t)
+	);
+}
+
+static cpBB
+cpBBexpand(const cpBB bb, const cpVect v){
+	return cpBBNew(
+		cpfmin(bb.l, v.x),
+		cpfmin(bb.b, v.y),
+		cpfmax(bb.r, v.x),
+		cpfmax(bb.t, v.y)
+	);
+}
+
+cpVect
+cpBBClampVect(const cpBB bb, const cpVect v)
+{
+	cpFloat x = cpfmin(cpfmax(bb.l, v.x), bb.r);
+	cpFloat y = cpfmin(cpfmax(bb.b, v.y), bb.t);
+	return cpv(x, y);
+}
+
+cpVect
+cpBBWrapVect(const cpBB bb, const cpVect v)
+{
+	cpFloat ix = cpfabs(bb.r - bb.l);
+	cpFloat modx = cpfmod(v.x - bb.l, ix);
+	cpFloat x = (modx > 0.0f) ? modx : modx + ix;
+	
+	cpFloat iy = cpfabs(bb.t - bb.b);
+	cpFloat mody = cpfmod(v.y - bb.b, iy);
+	cpFloat y = (mody > 0.0f) ? mody : mody + iy;
+	
+	return cpv(x + bb.l, y + bb.b);
+}

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/trunk/chipmunkd/cpBody.d	Thu Dec 02 02:11:26 2010 +0100
@@ -0,0 +1,367 @@
+
+// written in the D programming language
+
+module chipmunkd.cpBody;
+
+import chipmunkd.chipmunk;
+import chipmunkd.chipmunk_types_h;
+import chipmunkd.cpVect,chipmunkd.cpVect_h;
+import chipmunkd.cpShape;
+
+alias void function(cpBody *_body, cpVect gravity, cpFloat damping, cpFloat dt)cpBodyVelocityFunc;
+alias void function(cpBody *_body, cpFloat dt)cpBodyPositionFunc;
+
+//extern cpBodyVelocityFunc cpBodyUpdateVelocityDefault;
+//extern cpBodyPositionFunc cpBodyUpdatePositionDefault;
+
+// Structure to hold information about the contact graph components
+// when putting groups of objects to sleep.
+// No interesting user accessible fields.
+struct cpComponentNode {
+	cpBody *parent;
+	cpBody *next;
+	int rank;
+	cpFloat idleTime;
+}
+
+struct cpBody{
+	// *** Integration Functions.
+
+	// Function that is called to integrate the body's velocity. (Defaults to cpBodyUpdateVelocity)
+	cpBodyVelocityFunc velocity_func;
+	
+	// Function that is called to integrate the body's position. (Defaults to cpBodyUpdatePosition)
+	cpBodyPositionFunc position_func;
+	
+	// *** Mass Properties
+	
+	// Mass and it's inverse.
+	// Always use cpBodySetMass() whenever changing the mass as these values must agree.
+	cpFloat m, m_inv;
+	
+	// Moment of inertia and it's inverse.
+	// Always use cpBodySetMoment() whenever changing the moment as these values must agree.
+	cpFloat i, i_inv;
+	
+	// *** Positional Properties
+	
+	// Linear components of motion (position, velocity, and force)
+	cpVect p, v, f;
+	
+	// Angular components of motion (angle, angular velocity, and torque)
+	// Always use cpBodySetAngle() to set the angle of the body as a and rot must agree.
+	cpFloat a, w, t;
+	
+	// Cached unit length vector representing the angle of the body.
+	// Used for fast vector rotation using cpvrotate().
+	cpVect rot;
+	
+	// *** User Definable Fields
+	
+	// User defined data pointer.
+	cpDataPointer data;
+	
+	// *** Other Fields
+	
+	// Maximum velocities this body can move at after integrating velocity
+	cpFloat v_limit, w_limit;
+	
+	// *** Internally Used Fields
+	
+	// Velocity bias values used when solving penetrations and correcting constraints.
+	cpVect v_bias;
+	cpFloat w_bias;
+	
+	// Space this body has been added to
+	cpSpace* space;
+	
+	// Pointer to the shape list.
+	// Shapes form a linked list using cpShape.next when added to a space.
+	cpShape *shapesList;
+	
+	// Used by cpSpaceStep() to store contact graph information.
+	cpComponentNode node;
+}
+
+//// Basic allocation/destruction functions
+//cpBody *cpBodyAlloc(void);
+//cpBody *cpBodyInit(cpBody *body, cpFloat m, cpFloat i);
+//cpBody *cpBodyNew(cpFloat m, cpFloat i);
+//
+//void cpBodyDestroy(cpBody *body);
+//void cpBodyFree(cpBody *body);
+//
+//// Wake up a sleeping or idle body. (defined in cpSpace.c)
+//void cpBodyActivate(cpBody *body);
+//
+//// Force a body to sleep;
+//void cpBodySleep(cpBody *body);
+////void cpBodySleepGroup(cpBody *body, ...);
+
+static cpBool
+cpBodyIsSleeping(const cpBody *_body)
+{
+	return (_body.node.next !is null);
+}
+
+//cpBool cpBodyIsStatic(const cpBody *body);
+
+static cpBool
+cpBodyIsRogue(const cpBody* _body)
+{
+	return (_body.space is null);
+}
+
+
+//#define CP_DefineBodyGetter(type, member, name) \
+//static inline type cpBodyGet##name(const cpBody *body){return body.member;}
+//
+//#define CP_DefineBodySetter(type, member, name) \
+//static inline void \
+//cpBodySet##name(cpBody *body, const type value){ \
+//	cpBodyActivate(body); \
+//	body.member = value; \
+//} \
+
+//#define CP_DefineBodyProperty(type, member, name) \
+//CP_DefineBodyGetter(type, member, name) \
+//CP_DefineBodySetter(type, member, name)
+
+
+//// Accessors for cpBody struct members
+//CP_DefineBodyGetter(cpFloat, m, Mass);
+//void cpBodySetMass(cpBody *body, cpFloat m);
+//
+//CP_DefineBodyGetter(cpFloat, i, Moment);
+//void cpBodySetMoment(cpBody *body, cpFloat i);
+//
+//
+//CP_DefineBodyProperty(cpVect, p, Pos);
+//CP_DefineBodyProperty(cpVect, v, Vel);
+//CP_DefineBodyProperty(cpVect, f, Force);
+static cpFloat cpBodyGetAngle(const cpBody *_body){return _body.a;}
+static cpFloat cpBodySetAngle(cpBody *_body, const cpFloat value){cpBodyActivate(_body); return _body.a = value;}
+//CP_DefineBodyProperty(cpFloat, w, AngVel);
+//CP_DefineBodyProperty(cpFloat, t, Torque);
+//CP_DefineBodyGetter(cpVect, rot, Rot);
+//CP_DefineBodyProperty(cpFloat, v_limit, VelLimit);
+//CP_DefineBodyProperty(cpFloat, w_limit, AngVelLimit);
+
+////  Modify the velocity of the body so that it will move to the specified absolute coordinates in the next timestep.
+//// Intended for objects that are moved manually with a custom velocity integration function.
+//void cpBodySlew(cpBody *body, cpVect pos, cpFloat dt);
+//
+//// Default Integration functions.
+//void cpBodyUpdateVelocity(cpBody *body, cpVect gravity, cpFloat damping, cpFloat dt);
+//void cpBodyUpdatePosition(cpBody *body, cpFloat dt);
+
+// Convert body local to world coordinates
+static cpVect
+cpBodyLocal2World(const cpBody *_body, const cpVect v)
+{
+	return cpvadd(_body.p, cpvrotate(v, _body.rot));
+}
+
+// Convert world to body local coordinates
+static cpVect
+cpBodyWorld2Local(const cpBody *_body, const cpVect v)
+{
+	return cpvunrotate(cpvsub(v, _body.p), _body.rot);
+}
+
+// Apply an impulse (in world coordinates) to the body at a point relative to the center of gravity (also in world coordinates).
+static void
+cpBodyApplyImpulse(cpBody *_body, const cpVect j, const cpVect r)
+{
+	_body.v = cpvadd(_body.v, cpvmult(j, _body.m_inv));
+	_body.w += _body.i_inv*cpvcross(r, j);
+}
+
+//// Zero the forces on a body.
+//void cpBodyResetForces(cpBody *body);
+//// Apply a force (in world coordinates) to a body at a point relative to the center of gravity (also in world coordinates).
+//void cpBodyApplyForce(cpBody *body, const cpVect f, const cpVect r);
+
+static cpFloat
+cpBodyKineticEnergy(const cpBody *_body)
+{
+	// Need to do some fudging to avoid NaNs
+	cpFloat vsq = cpvdot(_body.v, _body.v);
+	cpFloat wsq = _body.w*_body.w;
+	return (vsq ? vsq*_body.m : 0.0f) + (wsq ? wsq*_body.i : 0.0f);
+}
+
+//// Apply a damped spring force between two bodies.
+//// Warning: Large damping values can be unstable. Use a cpDampedSpring constraint for this instead.
+//void cpApplyDampedSpring(cpBody *a, cpBody *b, cpVect anchr1, cpVect anchr2, cpFloat rlen, cpFloat k, cpFloat dmp, cpFloat dt);
+
+// initialized in cpInitChipmunk()
+cpBody cpStaticBodySingleton;
+
+cpBody*
+cpBodyAlloc()
+{
+	return cast(cpBody *)cpmalloc(cpBody.sizeof);
+}
+
+cpBodyVelocityFunc cpBodyUpdateVelocityDefault = &cpBodyUpdateVelocity;
+cpBodyPositionFunc cpBodyUpdatePositionDefault = &cpBodyUpdatePosition;
+
+cpBody*
+cpBodyInit(cpBody *_body, cpFloat m, cpFloat i)
+{
+	_body.velocity_func = cpBodyUpdateVelocityDefault;
+	_body.position_func = cpBodyUpdatePositionDefault;
+	
+	cpBodySetMass(_body, m);
+	cpBodySetMoment(_body, i);
+
+	_body.p = cpvzero;
+	_body.v = cpvzero;
+	_body.f = cpvzero;
+	
+	cpBodySetAngle(_body, 0.0f);
+	_body.w = 0.0f;
+	_body.t = 0.0f;
+	
+	_body.v_bias = cpvzero;
+	_body.w_bias = 0.0f;
+	
+	_body.data = null;
+	_body.v_limit = INFINITY;
+	_body.w_limit = INFINITY;
+	
+	_body.space = null;
+	_body.shapesList = null;
+	
+	cpComponentNode node = {null, null, 0, 0.0f};
+	_body.node = node;
+	
+	return _body;
+}
+
+cpBody*
+cpBodyNew(cpFloat m, cpFloat i)
+{
+	return cpBodyInit(cpBodyAlloc(), m, i);
+}
+
+void cpBodyDestroy(cpBody *_body){}
+
+void
+cpBodyFree(cpBody *_body)
+{
+	if(_body){
+		cpBodyDestroy(_body);
+		cpfree(_body);
+	}
+}
+
+void
+cpBodySetMass(cpBody *_body, cpFloat mass)
+{
+	_body.m = mass;
+	_body.m_inv = 1.0f/mass;
+}
+
+void
+cpBodySetMoment(cpBody *_body, cpFloat moment)
+{
+	_body.i = moment;
+	_body.i_inv = 1.0f/moment;
+}
+
+void
+cpBodySetAngle(cpBody *_body, cpFloat angle)
+{
+	_body.a = angle;//fmod(a, (cpFloat)M_PI*2.0f);
+	_body.rot = cpvforangle(angle);
+}
+
+void
+cpBodySlew(cpBody *_body, cpVect pos, cpFloat dt)
+{
+	cpVect delta = cpvsub(pos, _body.p);
+	_body.v = cpvmult(delta, 1.0f/dt);
+}
+
+void
+cpBodyUpdateVelocity(cpBody *_body, cpVect gravity, cpFloat damping, cpFloat dt)
+{
+	_body.v = cpvclamp(
+		cpvadd(cpvmult(_body.v, damping), cpvmult(cpvadd(gravity, cpvmult(_body.f, _body.m_inv)), dt)), 
+		_body.v_limit);
+	
+	cpFloat w_limit = _body.w_limit;
+	_body.w = cpfclamp(_body.w*damping + _body.t*_body.i_inv*dt, -w_limit, w_limit);
+}
+
+void
+cpBodyUpdatePosition(cpBody *_body, cpFloat dt)
+{
+	_body.p = cpvadd(_body.p, cpvmult(cpvadd(_body.v, _body.v_bias), dt));
+	cpBodySetAngle(_body, _body.a + (_body.w + _body.w_bias)*dt);
+	
+	_body.v_bias = cpvzero;
+	_body.w_bias = 0.0f;
+}
+
+void
+cpBodyResetForces(cpBody *_body)
+{
+	_body.f = cpvzero;
+	_body.t = 0.0f;
+}
+
+void
+cpBodyApplyForce(cpBody *_body, cpVect force, cpVect r)
+{
+	_body.f = cpvadd(_body.f, force);
+	_body.t += cpvcross(r, force);
+}
+
+void
+cpApplyDampedSpring(cpBody *a, cpBody *b, cpVect anchr1, cpVect anchr2, cpFloat rlen, cpFloat k, cpFloat dmp, cpFloat dt)
+{
+	// Calculate the world space anchor coordinates.
+	cpVect r1 = cpvrotate(anchr1, a.rot);
+	cpVect r2 = cpvrotate(anchr2, b.rot);
+	
+	cpVect delta = cpvsub(cpvadd(b.p, r2), cpvadd(a.p, r1));
+	cpFloat dist = cpvlength(delta);
+	cpVect n = dist ? cpvmult(delta, 1.0f/dist) : cpvzero;
+	
+	cpFloat f_spring = (dist - rlen)*k;
+
+	// Calculate the world relative velocities of the anchor points.
+	cpVect v1 = cpvadd(a.v, cpvmult(cpvperp(r1), a.w));
+	cpVect v2 = cpvadd(b.v, cpvmult(cpvperp(r2), b.w));
+	
+	// Calculate the damping force.
+	// This really should be in the impulse solver and can produce problems when using large damping values.
+	cpFloat vrn = cpvdot(cpvsub(v2, v1), n);
+	cpFloat f_damp = vrn*cpfmin(dmp, 1.0f/(dt*(a.m_inv + b.m_inv)));
+	
+	// Apply!
+	cpVect f = cpvmult(n, f_spring + f_damp);
+	cpBodyApplyForce(a, f, r1);
+	cpBodyApplyForce(b, cpvneg(f), r2);
+}
+
+cpBool
+cpBodyIsStatic(/+const+/ cpBody *_body)
+{
+	cpSpace *space = _body.space;
+	return ( (space !is null) && (_body is &space.staticBody) );
+}
+
+//void cpSpaceSleepBody(cpSpace *space, cpBody *_body);
+
+void
+cpBodySleep(cpBody *_body)
+{
+	if(cpBodyIsSleeping(_body)) return;
+	
+	assert(!cpBodyIsStatic(_body) && !cpBodyIsRogue(_body), "Rogue and static bodies cannot be put to sleep.");
+	cpSpaceSleepBody(_body.space, _body); //TODO
+}

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/trunk/chipmunkd/cpCollision.d	Thu Dec 02 02:11:26 2010 +0100
@@ -0,0 +1,380 @@
+
+// written in the D programming language
+
+module chipmunkd.cpCollision;
+
+import chipmunkd.chipmunk_types_h;
+import chipmunkd.chipmunk;
+import chipmunkd.cpShape;
+
+alias int function(const cpShape *, const cpShape *, cpContact *) collisionFunc;
+
+
+// Add contact points for circle to circle collisions.
+// Used by several collision tests.
+static int
+circle2circleQuery(const cpVect p1, const cpVect p2, const cpFloat r1, const cpFloat r2, cpContact *con)
+{
+	cpFloat mindist = r1 + r2;
+	cpVect delta = cpvsub(p2, p1);
+	cpFloat distsq = cpvlengthsq(delta);
+	if(distsq >= mindist*mindist) return 0;
+	
+	cpFloat dist = cpfsqrt(distsq);
+	
+	// Allocate and initialize the contact.
+	cpContactInit(
+		con,
+		cpvadd(p1, cpvmult(delta, 0.5f + (r1 - 0.5f*mindist)/(dist ? dist : INFINITY))),
+		(dist ? cpvmult(delta, 1.0f/dist) : cpv(1.0f, 0.0f)),
+		dist - mindist,
+		0
+	);
+	
+	return 1;
+}
+
+// Collide circle shapes.
+static int
+circle2circle(const cpShape *shape1, const cpShape *shape2, cpContact *arr)
+{
+	cpCircleShape *circ1 = cast(cpCircleShape*)shape1;
+	cpCircleShape *circ2 = cast(cpCircleShape*)shape2;
+	
+	return circle2circleQuery(circ1.tc, circ2.tc, circ1.r, circ2.r, arr);
+}
+
+// Collide circles to segment shapes.
+static int
+circle2segment(const cpShape *circleShape, const cpShape *segmentShape, cpContact *con)
+{
+	cpCircleShape *circ = cast(cpCircleShape *)circleShape;
+	cpSegmentShape *seg = cast(cpSegmentShape *)segmentShape;
+	
+	// Radius sum
+	cpFloat rsum = circ.r + seg.r;
+	
+	// Calculate normal distance from segment.
+	cpFloat dn = cpvdot(seg.tn, circ.tc) - cpvdot(seg.ta, seg.tn);
+	cpFloat dist = cpfabs(dn) - rsum;
+	if(dist > 0.0f) return 0;
+	
+	// Calculate tangential distance along segment.
+	cpFloat dt = -cpvcross(seg.tn, circ.tc);
+	cpFloat dtMin = -cpvcross(seg.tn, seg.ta);
+	cpFloat dtMax = -cpvcross(seg.tn, seg.tb);
+	
+	// Decision tree to decide which feature of the segment to collide with.
+	if(dt < dtMin){
+		if(dt < (dtMin - rsum)){
+			return 0;
+		} else {
+			return circle2circleQuery(circ.tc, seg.ta, circ.r, seg.r, con);
+		}
+	} else {
+		if(dt < dtMax){
+			cpVect n = (dn < 0.0f) ? seg.tn : cpvneg(seg.tn);
+			cpContactInit(
+				con,
+				cpvadd(circ.tc, cpvmult(n, circ.r + dist*0.5f)),
+				n,
+				dist,
+				0				 
+			);
+			return 1;
+		} else {
+			if(dt < (dtMax + rsum)) {
+				return circle2circleQuery(circ.tc, seg.tb, circ.r, seg.r, con);
+			} else {
+				return 0;
+			}
+		}
+	}
+	
+	return 1;
+}
+
+// Helper function for working with contact buffers
+// This used to malloc/realloc memory on the fly but was repurposed.
+static cpContact *
+nextContactPoint(cpContact *arr, int *numPtr)
+{
+	int num = *numPtr;
+	
+	if(num < CP_MAX_CONTACTS_PER_ARBITER)
+		(*numPtr) = num + 1;
+	
+	return &arr[num];
+}
+
+// Find the minimum separating axis for the give poly and axis list.
+static int
+findMSA(const cpPolyShape *poly, const cpPolyShapeAxis *axes, const int num, cpFloat *min_out)
+{
+	int min_index = 0;
+	cpFloat min = cpPolyShapeValueOnAxis(poly, axes.n, axes.d);
+	if(min > 0.0f) return -1;
+	
+	for(int i=1; i<num; i++){
+		cpFloat dist = cpPolyShapeValueOnAxis(poly, axes[i].n, axes[i].d);
+		if(dist > 0.0f) {
+			return -1;
+		} else if(dist > min){
+			min = dist;
+			min_index = i;
+		}
+	}
+	
+	(*min_out) = min;
+	return min_index;
+}
+
+// Add contacts for probably penetrating vertexes.
+// This handles the degenerate case where an overlap was detected, but no vertexes fall inside
+// the opposing polygon. (like a star of david)
+static int
+findVertsFallback(cpContact *arr, const cpPolyShape *poly1, const cpPolyShape *poly2, const cpVect n, const cpFloat dist)
+{
+	int num = 0;
+	
+	for(int i=0; i<poly1.numVerts; i++){
+		cpVect v = poly1.tVerts[i];
+		if(cpPolyShapeContainsVertPartial(poly2, v, cpvneg(n)))
+			cpContactInit(nextContactPoint(arr, &num), v, n, dist, CP_HASH_PAIR(poly1.shape.hashid, cast(const cpHashValue)i));
+	}
+	
+	for(int i=0; i<poly2.numVerts; i++){
+		cpVect v = poly2.tVerts[i];
+		if(cpPolyShapeContainsVertPartial(poly1, v, n))
+			cpContactInit(nextContactPoint(arr, &num), v, n, dist, CP_HASH_PAIR(poly2.shape.hashid, cast(const cpHashValue)i));
+	}
+	
+	return num;
+}
+
+// Add contacts for penetrating vertexes.
+static int
+findVerts(cpContact *arr, const cpPolyShape *poly1, const cpPolyShape *poly2, const cpVect n, const cpFloat dist)
+{
+	int num = 0;
+	
+	for(int i=0; i<poly1.numVerts; i++){
+		cpVect v = poly1.tVerts[i];
+		if(cpPolyShapeContainsVert(poly2, v))
+			cpContactInit(nextContactPoint(arr, &num), v, n, dist, CP_HASH_PAIR(poly1.shape.hashid, cast(const cpHashValue)i));
+	}
+	
+	for(int i=0; i<poly2.numVerts; i++){
+		cpVect v = poly2.tVerts[i];
+		if(cpPolyShapeContainsVert(poly1, v))
+			cpContactInit(nextContactPoint(arr, &num), v, n, dist, CP_HASH_PAIR(poly2.shape.hashid, cast(const cpHashValue)i));
+	}
+	
+	return (num ? num : findVertsFallback(arr, poly1, poly2, n, dist));
+}
+
+// Collide poly shapes together.
+static int
+poly2poly(const cpShape *shape1, const cpShape *shape2, cpContact *arr)
+{
+	cpPolyShape *poly1 = cast(cpPolyShape *)shape1;
+	cpPolyShape *poly2 = cast(cpPolyShape *)shape2;
+	
+	cpFloat min1;
+	int mini1 = findMSA(poly2, poly1.tAxes, poly1.numVerts, &min1);
+	if(mini1 == -1) return 0;
+	
+	cpFloat min2;
+	int mini2 = findMSA(poly1, poly2.tAxes, poly2.numVerts, &min2);
+	if(mini2 == -1) return 0;
+	
+	// There is overlap, find the penetrating verts
+	if(min1 > min2)
+		return findVerts(arr, poly1, poly2, poly1.tAxes[mini1].n, min1);
+	else
+		return findVerts(arr, poly1, poly2, cpvneg(poly2.tAxes[mini2].n), min2);
+}
+
+// Like cpPolyValueOnAxis(), but for segments.
+static cpFloat
+segValueOnAxis(const cpSegmentShape *seg, const cpVect n, const cpFloat d)
+{
+	cpFloat a = cpvdot(n, seg.ta) - seg.r;
+	cpFloat b = cpvdot(n, seg.tb) - seg.r;
+	return cpfmin(a, b) - d;
+}
+
+// Identify vertexes that have penetrated the segment.
+static void
+findPointsBehindSeg(cpContact *arr, int *num, const cpSegmentShape *seg, const cpPolyShape *poly, const cpFloat pDist, const cpFloat coef) 
+{
+	cpFloat dta = cpvcross(seg.tn, seg.ta);
+	cpFloat dtb = cpvcross(seg.tn, seg.tb);
+	cpVect n = cpvmult(seg.tn, coef);
+	
+	for(int i=0; i<poly.numVerts; i++){
+		cpVect v = poly.tVerts[i];
+		if(cpvdot(v, n) < cpvdot(seg.tn, seg.ta)*coef + seg.r){
+			cpFloat dt = cpvcross(seg.tn, v);
+			if(dta >= dt && dt >= dtb){
+				cpContactInit(nextContactPoint(arr, num), v, n, pDist, CP_HASH_PAIR(poly.shape.hashid, cast(const cpHashValue)i));
+			}
+		}
+	}
+}
+
+// This one is complicated and gross. Just don't go there...
+// TODO: Comment me!
+static int
+seg2poly(const cpShape *shape1, const cpShape *shape2, cpContact *arr)
+{
+	cpSegmentShape *seg = cast(cpSegmentShape *)shape1;
+	cpPolyShape *poly = cast(cpPolyShape *)shape2;
+	cpPolyShapeAxis *axes = poly.tAxes;
+	
+	cpFloat segD = cpvdot(seg.tn, seg.ta);
+	cpFloat minNorm = cpPolyShapeValueOnAxis(poly, seg.tn, segD) - seg.r;
+	cpFloat minNeg = cpPolyShapeValueOnAxis(poly, cpvneg(seg.tn), -segD) - seg.r;
+	if(minNeg > 0.0f || minNorm > 0.0f) return 0;
+	
+	int mini = 0;
+	cpFloat poly_min = segValueOnAxis(seg, axes.n, axes.d);
+	if(poly_min > 0.0f) return 0;
+	for(int i=0; i<poly.numVerts; i++){
+		cpFloat dist = segValueOnAxis(seg, axes[i].n, axes[i].d);
+		if(dist > 0.0f){
+			return 0;
+		} else if(dist > poly_min){
+			poly_min = dist;
+			mini = i;
+		}
+	}
+	
+	int num = 0;
+	
+	cpVect poly_n = cpvneg(axes[mini].n);
+	
+	cpVect va = cpvadd(seg.ta, cpvmult(poly_n, seg.r));
+	cpVect vb = cpvadd(seg.tb, cpvmult(poly_n, seg.r));
+	if(cpPolyShapeContainsVert(poly, va))
+		cpContactInit(nextContactPoint(arr, &num), va, poly_n, poly_min, CP_HASH_PAIR(seg.shape.hashid, cast(cpHashValue)0));
+	if(cpPolyShapeContainsVert(poly, vb))
+		cpContactInit(nextContactPoint(arr, &num), vb, poly_n, poly_min, CP_HASH_PAIR(seg.shape.hashid, cast(cpHashValue)1));
+
+	// Floating point precision problems here.
+	// This will have to do for now.
+	poly_min -= cp_collision_slop;
+	if(minNorm >= poly_min || minNeg >= poly_min) {
+		if(minNorm > minNeg)
+			findPointsBehindSeg(arr, &num, seg, poly, minNorm, 1.0f);
+		else
+			findPointsBehindSeg(arr, &num, seg, poly, minNeg, -1.0f);
+	}
+	
+	// If no other collision points are found, try colliding endpoints.
+	if(num == 0){
+		cpVect poly_a = poly.tVerts[mini];
+		cpVect poly_b = poly.tVerts[(mini + 1)%poly.numVerts];
+		
+		if(circle2circleQuery(seg.ta, poly_a, seg.r, 0.0f, arr))
+			return 1;
+			
+		if(circle2circleQuery(seg.tb, poly_a, seg.r, 0.0f, arr))
+			return 1;
+			
+		if(circle2circleQuery(seg.ta, poly_b, seg.r, 0.0f, arr))
+			return 1;
+			
+		if(circle2circleQuery(seg.tb, poly_b, seg.r, 0.0f, arr))
+			return 1;
+	}
+
+	return num;
+}
+
+// This one is less gross, but still gross.
+// TODO: Comment me!
+static int
+circle2poly(const cpShape *shape1, const cpShape *shape2, cpContact *con)
+{
+	cpCircleShape *circ = cast(cpCircleShape *)shape1;
+	cpPolyShape *poly = cast(cpPolyShape *)shape2;
+	cpPolyShapeAxis *axes = poly.tAxes;
+	
+	int mini = 0;
+	cpFloat min = cpvdot(axes.n, circ.tc) - axes.d - circ.r;
+	for(int i=0; i<poly.numVerts; i++){
+		cpFloat dist = cpvdot(axes[i].n, circ.tc) - axes[i].d - circ.r;
+		if(dist > 0.0f){
+			return 0;
+		} else if(dist > min) {
+			min = dist;
+			mini = i;
+		}
+	}
+	
+	cpVect n = axes[mini].n;
+	cpVect a = poly.tVerts[mini];
+	cpVect b = poly.tVerts[(mini + 1)%poly.numVerts];
+	cpFloat dta = cpvcross(n, a);
+	cpFloat dtb = cpvcross(n, b);
+	cpFloat dt = cpvcross(n, circ.tc);
+		
+	if(dt < dtb){
+		return circle2circleQuery(circ.tc, b, circ.r, 0.0f, con);
+	} else if(dt < dta) {
+		cpContactInit(
+			con,
+			cpvsub(circ.tc, cpvmult(n, circ.r + min/2.0f)),
+			cpvneg(n),
+			min,
+			0				 
+		);
+	
+		return 1;
+	} else {
+		return circle2circleQuery(circ.tc, a, circ.r, 0.0f, con);
+	}
+}
+
+static const collisionFunc builtinCollisionFuncs[9] = [
+	&circle2circle,
+	null,
+	null,
+	&circle2segment,
+	null,
+	null,
+	&circle2poly,
+	&seg2poly,
+	&poly2poly,
+];
+
+static collisionFunc[cpShapeType.CP_NUM_SHAPES * cpShapeType.CP_NUM_SHAPES] colfuncs = builtinCollisionFuncs;
+
+static void
+addColFunc(const cpShapeType a, const cpShapeType b, collisionFunc func)
+{
+	colfuncs[a + b*cpShapeType.CP_NUM_SHAPES] = func;
+}
+
+// Initializes the array of collision functions.
+// Called by cpInitChipmunk().
+void cpInitCollisionFuncs()
+{	
+	addColFunc(cpShapeType.CP_CIRCLE_SHAPE,  cpShapeType.CP_CIRCLE_SHAPE, &circle2circle);
+	addColFunc(cpShapeType.CP_CIRCLE_SHAPE,  cpShapeType.CP_SEGMENT_SHAPE, &circle2segment);
+	addColFunc(cpShapeType.CP_SEGMENT_SHAPE, cpShapeType.CP_POLY_SHAPE,    &seg2poly);
+	addColFunc(cpShapeType.CP_CIRCLE_SHAPE,  cpShapeType.CP_POLY_SHAPE,    &circle2poly);
+	addColFunc(cpShapeType.CP_POLY_SHAPE,    cpShapeType.CP_POLY_SHAPE,    &poly2poly);
+}	
+
+
+int
+cpCollideShapes(const cpShape *a, const cpShape *b, cpContact *arr)
+{
+	// Their shape types must be in order.
+	assert(a.klass.type <= b.klass.type, "Collision shapes passed to cpCollideShapes() are not sorted.");
+	
+	collisionFunc cfunc = colfuncs[a.klass.type + b.klass.type*cpShapeType.CP_NUM_SHAPES];
+	return (cfunc) ? cfunc(a, b, arr) : 0;
+}

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/trunk/chipmunkd/cpHashSet.d	Thu Dec 02 02:11:26 2010 +0100
@@ -0,0 +1,277 @@
+
+// written in the D programming language
+
+module chipmunkd.cpHashSet;
+
+import chipmunkd.chipmunk;
+import chipmunkd.chipmunk_types_h;
+import chipmunkd.cpArray;
+import chipmunkd.prime;
+
+// cpHashSet uses a chained hashtable implementation.
+// Other than the transformation functions, there is nothing fancy going on.
+
+// cpHashSetBin's form the linked lists in the chained hash table.
+struct cpHashSetBin {
+	// Pointer to the element.
+	void *elt;
+	// Hash value of the element.
+	cpHashValue hash;
+	// Next element in the chain.
+	cpHashSetBin *next;
+}
+
+// Equality function. Returns true if ptr is equal to elt.
+alias cpBool function(void *ptr, void *elt) cpHashSetEqlFunc;
+// Used by cpHashSetInsert(). Called to transform the ptr into an element.
+alias void* function(void *ptr, void *data) cpHashSetTransFunc;
+
+struct cpHashSet {
+	// Number of elements stored in the table.
+	int entries;
+	// Number of cells in the table.
+	int size;
+	
+	cpHashSetEqlFunc eql;
+	cpHashSetTransFunc trans;
+	
+	// Default value returned by cpHashSetFind() when no element is found.
+	// Defaults to null.
+	void *default_value;
+	
+	// The table and recycled bins
+	cpHashSetBin **table;
+	cpHashSetBin *pooledBins;
+	
+	cpArray *allocatedBuffers;
+}
+
+alias void function(void *elt, void *data) cpHashSetIterFunc;
+alias cpBool function(void *elt, void *data) cpHashSetFilterFunc;
+
+static void freeWrap(void *ptr, void *unused){cpfree(ptr);}
+
+void
+cpHashSetDestroy(cpHashSet *set)
+{
+	// Free the table.
+	cpfree(set.table);
+	
+	cpArrayEach(set.allocatedBuffers, &freeWrap, null);
+	cpArrayFree(set.allocatedBuffers);
+}
+
+void
+cpHashSetFree(cpHashSet *set)
+{
+	if(set){
+		cpHashSetDestroy(set);
+		cpfree(set);
+	}
+}
+
+cpHashSet *
+cpHashSetAlloc()
+{
+	return cast(cpHashSet *)cpcalloc(1, cpHashSet.sizeof);
+}
+
+cpHashSet *
+cpHashSetInit(cpHashSet *set, int size, cpHashSetEqlFunc eqlFunc, cpHashSetTransFunc trans)
+{
+	set.size = next_prime(size);
+	set.entries = 0;
+	
+	set.eql = eqlFunc;
+	set.trans = trans;
+	
+	set.default_value = null;
+	
+	set.table = cast(cpHashSetBin **)cpcalloc(set.size, (cpHashSetBin *).sizeof);
+	set.pooledBins = null;
+	
+	set.allocatedBuffers = cpArrayNew(0);
+	
+	return set;
+}
+
+cpHashSet *
+cpHashSetNew(int size, cpHashSetEqlFunc eqlFunc, cpHashSetTransFunc trans)
+{
+	return cpHashSetInit(cpHashSetAlloc(), size, eqlFunc, trans);
+}
+
+static int
+setIsFull(cpHashSet *set)
+{
+	return (set.entries >= set.size);
+}
+
+static void
+cpHashSetResize(cpHashSet *set)
+{
+	// Get the next approximate doubled prime.
+	int newSize = next_prime(set.size + 1);
+	// Allocate a new table.
+	cpHashSetBin **newTable = cast(cpHashSetBin **)cpcalloc(newSize, (cpHashSetBin *).sizeof);
+	
+	// Iterate over the chains.
+	for(int i=0; i<set.size; i++){
+		// Rehash the bins into the new table.
+		cpHashSetBin *bin = set.table[i];
+		while(bin){
+			cpHashSetBin *next = bin.next;
+			
+			int idx = bin.hash%newSize;
+			bin.next = newTable[idx];
+			newTable[idx] = bin;
+			
+			bin = next;
+		}
+	}
+	
+	cpfree(set.table);
+	
+	set.table = newTable;
+	set.size = newSize;
+}
+
+static void
+recycleBin(cpHashSet *set, cpHashSetBin *bin)
+{
+	bin.next = set.pooledBins;
+	set.pooledBins = bin;
+	bin.elt = null;
+}
+
+static cpHashSetBin *
+getUnusedBin(cpHashSet *set)
+{
+	cpHashSetBin *bin = set.pooledBins;
+	
+	if(bin){
+		set.pooledBins = bin.next;
+		return bin;
+	} else {
+		// Pool is exhausted, make more
+		int count = CP_BUFFER_BYTES/cpHashSetBin.sizeof;
+		assert(count!=0, "Buffer size is too small.");
+		
+		cpHashSetBin *buffer = cast(cpHashSetBin *)cpmalloc(CP_BUFFER_BYTES);
+		cpArrayPush(set.allocatedBuffers, buffer);
+		
+		// push all but the first one, return the first instead
+		for(int i=1; i<count; i++) recycleBin(set, buffer + i);
+		return buffer;
+	}
+}
+
+void *
+cpHashSetInsert(cpHashSet *set, cpHashValue hash, void *ptr, void *data)
+{
+	int idx = hash%set.size;
+	
+	// Find the bin with the matching element.
+	cpHashSetBin *bin = set.table[idx];
+	while(bin && !set.eql(ptr, bin.elt))
+		bin = bin.next;
+	
+	// Create it necessary.
+	if(!bin){
+		bin = getUnusedBin(set);
+		bin.hash = hash;
+		bin.elt = set.trans(ptr, data); // Transform the pointer.
+		
+		bin.next = set.table[idx];
+		set.table[idx] = bin;
+		
+		set.entries++;
+		
+		// Resize the set if it's full.
+		if(setIsFull(set))
+			cpHashSetResize(set);
+	}
+	
+	return bin.elt;
+}
+
+void *
+cpHashSetRemove(cpHashSet *set, cpHashValue hash, void *ptr)
+{
+	int idx = hash%set.size;
+	
+	// Pointer to the previous bin pointer.
+	cpHashSetBin **prev_ptr = &set.table[idx];
+	// Pointer the the current bin.
+	cpHashSetBin *bin = set.table[idx];
+	
+	// Find the bin
+	while(bin && !set.eql(ptr, bin.elt)){
+		prev_ptr = &bin.next;
+		bin = bin.next;
+	}
+	
+	// Remove it if it exists.
+	if(bin){
+		// Update the previous bin pointer to point to the next bin.
+		(*prev_ptr) = bin.next;
+		set.entries--;
+		
+		void *return_value = bin.elt;
+		
+		recycleBin(set, bin);
+		
+		return return_value;
+	}
+	
+	return null;
+}
+
+void *
+cpHashSetFind(cpHashSet *set, cpHashValue hash, void *ptr)
+{	
+	int idx = hash%set.size;
+	cpHashSetBin *bin = set.table[idx];
+	while(bin && !set.eql(ptr, bin.elt))
+		bin = bin.next;
+		
+	return (bin ? bin.elt : set.default_value);
+}
+
+void
+cpHashSetEach(cpHashSet *set, cpHashSetIterFunc func, void *data)
+{
+	for(int i=0; i<set.size; i++){
+		cpHashSetBin *bin = set.table[i];
+		while(bin){
+			cpHashSetBin *next = bin.next;
+			func(bin.elt, data);
+			bin = next;
+		}
+	}
+}
+
+void
+cpHashSetFilter(cpHashSet *set, cpHashSetFilterFunc func, void *data)
+{
+	// Iterate over all the chains.
+	for(int i=0; i<set.size; i++){
+		// The rest works similarly to cpHashSetRemove() above.
+		cpHashSetBin **prev_ptr = &set.table[i];
+		cpHashSetBin *bin = set.table[i];
+		while(bin){
+			cpHashSetBin *next = bin.next;
+			
+			if(func(bin.elt, data)){
+				prev_ptr = &bin.next;
+			} else {
+				(*prev_ptr) = next;
+
+				set.entries--;
+				recycleBin(set, bin);
+			}
+			
+			bin = next;
+		}
+	}
+}

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/trunk/chipmunkd/cpPolyShape.d	Thu Dec 02 02:11:26 2010 +0100
@@ -0,0 +1,315 @@
+
+// written in the D programming language
+
+module chipmunkd.cpPolyShape;
+
+import chipmunkd.cpShape;
+import chipmunkd.cpBody;
+import chipmunkd.chipmunk;
+import chipmunkd.chipmunk_types_h;
+import chipmunkd.cpVect_h;
+import chipmunkd.cpBB;
+
+// Axis structure used by cpPolyShape.
+struct cpPolyShapeAxis{
+	// normal
+	cpVect n;
+	// distance from origin
+	cpFloat d;
+}
+
+// Convex polygon shape structure.
+struct cpPolyShape{
+	cpShape shape;
+	
+	// Vertex and axis lists.
+	int numVerts;
+	cpVect *verts;
+	cpPolyShapeAxis *axes;
+
+	// Transformed vertex and axis lists.
+	cpVect *tVerts;
+	cpPolyShapeAxis *tAxes;
+}
+
+//// Basic allocation functions.
+//cpPolyShape *cpPolyShapeAlloc(void);
+//cpPolyShape *cpPolyShapeInit(cpPolyShape *poly, cpBody *body, int numVerts, cpVect *verts, cpVect offset);
+//cpShape *cpPolyShapeNew(cpBody *body, int numVerts, cpVect *verts, cpVect offset);
+//
+//cpPolyShape *cpBoxShapeInit(cpPolyShape *poly, cpBody *body, cpFloat width, cpFloat height);
+//cpShape *cpBoxShapeNew(cpBody *body, cpFloat width, cpFloat height);
+//
+//// Check that a set of vertexes has a correct winding and that they are convex
+//cpBool cpPolyValidate(cpVect *verts, int numVerts);
+//
+//int cpPolyShapeGetNumVerts(cpShape *shape);
+//cpVect cpPolyShapeGetVert(cpShape *shape, int idx);
+
+// *** inlined utility functions
+
+// Returns the minimum distance of the polygon to the axis.
+static cpFloat
+cpPolyShapeValueOnAxis(const cpPolyShape *poly, const cpVect n, const cpFloat d)
+{
+	const cpVect *verts = poly.tVerts;
+	cpFloat min = cpvdot(n, verts[0]);
+	
+	int i;
+	for(i=1; i<poly.numVerts; i++)
+		min = cpfmin(min, cpvdot(n, verts[i]));
+	
+	return min - d;
+}
+
+// Returns true if the polygon contains the vertex.
+static cpBool
+cpPolyShapeContainsVert(const cpPolyShape *poly, const cpVect v)
+{
+	const cpPolyShapeAxis *axes = poly.tAxes;
+	
+	int i;
+	for(i=0; i<poly.numVerts; i++){
+		cpFloat dist = cpvdot(axes[i].n, v) - axes[i].d;
+		if(dist > 0.0f) return cpFalse;
+	}
+	
+	return cpTrue;
+}
+
+// Same as cpPolyShapeContainsVert() but ignores faces pointing away from the normal.
+static cpBool
+cpPolyShapeContainsVertPartial(const cpPolyShape *poly, const cpVect v, const cpVect n)
+{
+	const cpPolyShapeAxis *axes = poly.tAxes;
+	
+	int i;
+	for(i=0; i<poly.numVerts; i++){
+		if(cpvdot(axes[i].n, n) < 0.0f) continue;
+		cpFloat dist = cpvdot(axes[i].n, v) - axes[i].d;
+		if(dist > 0.0f) return cpFalse;
+	}
+	
+	return cpTrue;
+}
+
+// cpPolyShape.c ---------------------------------
+
+
+cpPolyShape *
+cpPolyShapeAlloc()
+{
+	return cast(cpPolyShape *)cpcalloc(1, cpPolyShape.sizeof);
+}
+
+static void
+cpPolyShapeTransformVerts(cpPolyShape *poly, cpVect p, cpVect rot)
+{
+	cpVect *src = poly.verts;
+	cpVect *dst = poly.tVerts;
+	
+	for(int i=0; i<poly.numVerts; i++)
+		dst[i] = cpvadd(p, cpvrotate(src[i], rot));
+}
+
+static void
+cpPolyShapeTransformAxes(cpPolyShape *poly, cpVect p, cpVect rot)
+{
+	cpPolyShapeAxis *src = poly.axes;
+	cpPolyShapeAxis *dst = poly.tAxes;
+	
+	for(int i=0; i<poly.numVerts; i++){
+		cpVect n = cpvrotate(src[i].n, rot);
+		dst[i].n = n;
+		dst[i].d = cpvdot(p, n) + src[i].d;
+	}
+}
+
+static cpBB
+cpPolyShapeCacheData(cpShape *shape, cpVect p, cpVect rot)
+{
+	cpPolyShape *poly = cast(cpPolyShape *)shape;
+	
+	cpFloat l, b, r, t;
+	
+	cpPolyShapeTransformAxes(poly, p, rot);
+	cpPolyShapeTransformVerts(poly, p, rot);
+	
+	cpVect *verts = poly.tVerts;
+	l = r = verts[0].x;
+	b = t = verts[0].y;
+	
+	// TODO do as part of cpPolyShapeTransformVerts?
+	for(int i=1; i<poly.numVerts; i++){
+		cpVect v = verts[i];
+		
+		l = cpfmin(l, v.x);
+		r = cpfmax(r, v.x);
+		
+		b = cpfmin(b, v.y);
+		t = cpfmax(t, v.y);
+	}
+	
+	return cpBBNew(l, b, r, t);
+}
+
+static void
+cpPolyShapeDestroy(cpShape *shape)
+{
+	cpPolyShape *poly = cast(cpPolyShape *)shape;
+	
+	cpfree(poly.verts);
+	cpfree(poly.tVerts);
+	
+	cpfree(poly.axes);
+	cpfree(poly.tAxes);
+}
+
+static cpBool
+cpPolyShapePointQuery(cpShape *shape, cpVect p){
+	return cpBBcontainsVect(shape.bb, p) && cpPolyShapeContainsVert(cast(cpPolyShape *)shape, p);
+}
+
+static void
+cpPolyShapeSegmentQuery(cpShape *shape, cpVect a, cpVect b, cpSegmentQueryInfo *info)
+{
+	cpPolyShape *poly = cast(cpPolyShape *)shape;
+	cpPolyShapeAxis *axes = poly.tAxes;
+	cpVect *verts = poly.tVerts;
+	int numVerts = poly.numVerts;
+	
+	for(int i=0; i<numVerts; i++){
+		cpVect n = axes[i].n;
+		cpFloat an = cpvdot(a, n);
+		if(axes[i].d > an) continue;
+		
+		cpFloat bn = cpvdot(b, n);
+		cpFloat t = (axes[i].d - an)/(bn - an);
+		if(t < 0.0f || 1.0f < t) continue;
+		
+		cpVect point = cpvlerp(a, b, t);
+		cpFloat dt = -cpvcross(n, point);
+		cpFloat dtMin = -cpvcross(n, verts[i]);
+		cpFloat dtMax = -cpvcross(n, verts[(i+1)%numVerts]);
+		
+		if(dtMin <= dt && dt <= dtMax){
+			info.shape = shape;
+			info.t = t;
+			info.n = n;
+		}
+	}
+}
+
+static /+const+/ cpShapeClass polyClass = {
+	cpShapeType.CP_POLY_SHAPE,
+	&cpPolyShapeCacheData,
+	&cpPolyShapeDestroy,
+	&cpPolyShapePointQuery,
+	&cpPolyShapeSegmentQuery,
+};
+
+cpBool
+cpPolyValidate(cpVect *verts, int numVerts)
+{
+	for(int i=0; i<numVerts; i++){
+		cpVect a = verts[i];
+		cpVect b = verts[(i+1)%numVerts];
+		cpVect c = verts[(i+2)%numVerts];
+		
+		if(cpvcross(cpvsub(b, a), cpvsub(c, b)) > 0.0f)
+			return cpFalse;
+	}
+	
+	return cpTrue;
+}
+
+int
+cpPolyShapeGetNumVerts(cpShape *shape)
+{
+	assert(shape.klass == &polyClass, "Shape is not a poly shape.");
+	return (cast(cpPolyShape *)shape).numVerts;
+}
+
+cpVect
+cpPolyShapeGetVert(cpShape *shape, int idx)
+{
+	assert(shape.klass == &polyClass, "Shape is not a poly shape.");
+	assert(0 <= idx && idx < cpPolyShapeGetNumVerts(shape), "Index out of range.");
+	
+	return (cast(cpPolyShape *)shape).verts[idx];
+}
+
+
+static void
+setUpVerts(cpPolyShape *poly, int numVerts, cpVect *verts, cpVect offset)
+{
+	poly.numVerts = numVerts;
+
+	poly.verts	= cast(cpVect *)cpcalloc(numVerts, cpVect.sizeof);
+	poly.tVerts = cast(cpVect *)cpcalloc(numVerts, cpVect.sizeof);
+	poly.axes	= cast(cpPolyShapeAxis *)cpcalloc(numVerts, (cpPolyShapeAxis).sizeof);
+	poly.tAxes	= cast(cpPolyShapeAxis *)cpcalloc(numVerts, (cpPolyShapeAxis).sizeof);
+	
+	for(int i=0; i<numVerts; i++){
+		cpVect a = cpvadd(offset, verts[i]);
+		cpVect b = cpvadd(offset, verts[(i+1)%numVerts]);
+		cpVect n = cpvnormalize(cpvperp(cpvsub(b, a)));
+
+		poly.verts[i] = a;
+		poly.axes[i].n = n;
+		poly.axes[i].d = cpvdot(n, a);
+	}
+}
+
+cpPolyShape *
+cpPolyShapeInit(cpPolyShape *poly, cpBody *_body, int numVerts, cpVect *verts, cpVect offset)
+{
+	// Fail if the user attempts to pass a concave poly, or a bad winding.
+	assert(cpPolyValidate(verts, numVerts), "Polygon is concave or has a reversed winding.");
+	
+	setUpVerts(poly, numVerts, verts, offset);
+	cpShapeInit(cast(cpShape *)poly, &polyClass, _body);
+
+	return poly;
+}
+
+cpShape *
+cpPolyShapeNew(cpBody *_body, int numVerts, cpVect *verts, cpVect offset)
+{
+	return cast(cpShape *)cpPolyShapeInit(cpPolyShapeAlloc(), _body, numVerts, verts, offset);
+}
+
+cpPolyShape *
+cpBoxShapeInit(cpPolyShape *poly, cpBody *_body, cpFloat width, cpFloat height)
+{
+	cpFloat hw = width/2.0f;
+	cpFloat hh = height/2.0f;
+	
+	cpVect verts[] = [
+		cpv(-hw,-hh),
+		cpv(-hw, hh),
+		cpv( hw, hh),
+		cpv( hw,-hh),
+	];
+	
+	return cpPolyShapeInit(poly, _body, 4, verts.ptr, cpvzero);
+}
+
+cpShape *
+cpBoxShapeNew(cpBody *_body, cpFloat width, cpFloat height)
+{
+	return cast(cpShape *)cpBoxShapeInit(cpPolyShapeAlloc(), _body, width, height);
+}
+
+// Unsafe API (chipmunk_unsafe.h)
+
+void
+cpPolyShapeSetVerts(cpShape *shape, int numVerts, cpVect *verts, cpVect offset)
+{
+	assert(shape.klass == &polyClass, "Shape is not a poly shape.");
+	cpPolyShapeDestroy(shape);
+	setUpVerts(cast(cpPolyShape *)shape, numVerts, verts, offset);
+}
+
+
+

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/trunk/chipmunkd/cpShape.d	Thu Dec 02 02:11:26 2010 +0100
@@ -0,0 +1,533 @@
+
+// written in the D programming language
+
+module chipmunkd.cpShape;
+
+import chipmunkd.cpBody;
+import chipmunkd.chipmunk;
+import chipmunkd.chipmunk_types_h;
+import chipmunkd.cpVect_h;
+import chipmunkd.cpBB;
+
+import std.stdio;
+
+struct cpSegmentQueryInfo {
+	cpShape *shape; // shape that was hit, null if no collision
+	cpFloat t; // Distance along query segment, will always be in the range [0, 1].
+	cpVect n; // normal of hit surface
+}
+
+// Enumeration of shape types.
+enum cpShapeType{
+	CP_CIRCLE_SHAPE,
+	CP_SEGMENT_SHAPE,
+	CP_POLY_SHAPE,
+	CP_NUM_SHAPES
+}
+
+// Shape class. Holds function pointers and type data.
+struct cpShapeClass {
+	cpShapeType type;
+	
+	// Called by cpShapeCacheBB().
+	cpBB function(cpShape *shape, cpVect p, cpVect rot) cacheData;
+	// Called to by cpShapeDestroy().
+	void function(cpShape *shape) destroy;
+	
+	// called by cpShapePointQuery().
+	cpBool function(cpShape *shape, cpVect p)pointQuery;
+	
+	// called by cpShapeSegmentQuery()
+	 void function(cpShape *shape, cpVect a, cpVect b, cpSegmentQueryInfo *info) segmentQuery;
+}
+
+// Basic shape struct that the others inherit from.
+struct cpShape{
+	// The "class" of a shape as defined above 
+	/+const+/ cpShapeClass *klass;
+	
+	// cpBody that the shape is attached to.
+	cpBody* _body;
+	
+	// Cached BBox for the shape.
+	cpBB bb;
+	
+	// Sensors invoke callbacks, but do not generate collisions
+	cpBool sensor;
+	
+	// *** Surface properties.
+	
+	// Coefficient of restitution. (elasticity)
+	cpFloat e;
+	// Coefficient of friction.
+	cpFloat u;
+	// Surface velocity used when solving for friction.
+	cpVect surface_v;
+	
+	// *** User Definable Fields
+	
+	// User defined data pointer for the shape.
+	cpDataPointer data;
+	
+	// User defined collision type for the shape.
+	cpCollisionType collision_type;
+	// User defined collision group for the shape.
+	cpGroup group;
+	// User defined layer bitmask for the shape.
+	cpLayers layers;
+	
+	// *** Internally Used Fields
+	
+	// Shapes form a linked list when added to space on a non-null body
+	cpShape* next;
+	
+	// Unique id used as the hash value.
+	cpHashValue hashid;
+}
+
+//
+//// Low level shape initialization func.
+//cpShape* cpShapeInit(cpShape *shape, const struct cpShapeClass *klass, cpBody *body);
+//
+//// Basic destructor functions. (allocation functions are not shared)
+//void cpShapeDestroy(cpShape *shape);
+//void cpShapeFree(cpShape *shape);
+//
+//// Cache the BBox of the shape.
+//cpBB cpShapeCacheBB(cpShape *shape);
+//
+//// Test if a point lies within a shape.
+//cpBool cpShapePointQuery(cpShape *shape, cpVect p);
+//
+//#define CP_DeclareShapeGetter(struct, type, name) type struct##Get##name(cpShape *shape)
+//
+
+// Circle shape structure.
+struct cpCircleShape{
+	cpShape shape;
+	
+	// Center in body space coordinates
+	cpVect c;
+	// Radius.
+	cpFloat r;
+	
+	// Transformed center. (world space coordinates)
+	cpVect tc;
+}
+
+//// Basic allocation functions for cpCircleShape.
+//cpCircleShape *cpCircleShapeAlloc(void);
+//cpCircleShape *cpCircleShapeInit(cpCircleShape *circle, cpBody *body, cpFloat radius, cpVect offset);
+//cpShape *cpCircleShapeNew(cpBody *body, cpFloat radius, cpVect offset);
+//
+//CP_DeclareShapeGetter(cpCircleShape, cpVect, Offset);
+//CP_DeclareShapeGetter(cpCircleShape, cpFloat, Radius);
+
+// Segment shape structure.
+struct cpSegmentShape{
+	cpShape shape;
+	
+	// Endpoints and normal of the segment. (body space coordinates)
+	cpVect a, b, n;
+	// Radius of the segment. (Thickness)
+	cpFloat r;
+
+	// Transformed endpoints and normal. (world space coordinates)
+	cpVect ta, tb, tn;
+}
+//
+//// Basic allocation functions for cpSegmentShape.
+//cpSegmentShape* cpSegmentShapeAlloc(void);
+//cpSegmentShape* cpSegmentShapeInit(cpSegmentShape *seg, cpBody *body, cpVect a, cpVect b, cpFloat radius);
+//cpShape* cpSegmentShapeNew(cpBody *body, cpVect a, cpVect b, cpFloat radius);
+//
+//CP_DeclareShapeGetter(cpSegmentShape, cpVect, A);
+//CP_DeclareShapeGetter(cpSegmentShape, cpVect, B);
+//CP_DeclareShapeGetter(cpSegmentShape, cpVect, Normal);
+//CP_DeclareShapeGetter(cpSegmentShape, cpFloat, Radius);
+//
+//// For determinism, you can reset the shape id counter.
+//void cpResetShapeIdCounter(void);
+//
+//// Directed segment queries against individual shapes.
+//void cpSegmentQueryInfoPrint(cpSegmentQueryInfo *info);
+//
+//cpBool cpShapeSegmentQuery(cpShape *shape, cpVect a, cpVect b, cpSegmentQueryInfo *info);
+//
+static cpVect
+cpSegmentQueryHitPoint(const cpVect start, const cpVect end, const cpSegmentQueryInfo info)
+{
+	return cpvlerp(start, end, info.t);
+}
+
+static cpFloat
+cpSegmentQueryHitDist(const cpVect start, const cpVect end, const cpSegmentQueryInfo info)
+{
+	return cpvdist(start, end)*info.t;
+}
+
+cpHashValue SHAPE_ID_COUNTER = 0;
+
+void
+cpResetShapeIdCounter()
+{
+	SHAPE_ID_COUNTER = 0;
+}
+
+cpShape*
+cpShapeInit(cpShape *shape, /+const+/ cpShapeClass *klass, cpBody *_body)
+{
+	shape.klass = klass;
+	
+	shape.hashid = SHAPE_ID_COUNTER;
+	SHAPE_ID_COUNTER++;
+	
+	shape._body = _body;
+	shape.sensor = 0;
+	
+	shape.e = 0.0f;
+	shape.u = 0.0f;
+	shape.surface_v = cpvzero;
+	
+	shape.collision_type = 0;
+	shape.group = CP_NO_GROUP;
+	shape.layers = CP_ALL_LAYERS;
+	
+	shape.data = null;
+	shape.next = null;
+	
+//	cpShapeCacheBB(shape);
+	
+	return shape;
+}
+
+void
+cpShapeDestroy(cpShape *shape)
+{
+	if(shape.klass.destroy) shape.klass.destroy(shape);
+}
+
+void
+cpShapeFree(cpShape *shape)
+{
+	if(shape){
+		cpShapeDestroy(shape);
+		cpfree(shape);
+	}
+}
+
+cpBB
+cpShapeCacheBB(cpShape *shape)
+{
+	cpBody *_body = shape._body;
+	
+	shape.bb = shape.klass.cacheData(shape, _body.p, _body.rot);
+	return shape.bb;
+}
+
+cpBool
+cpShapePointQuery(cpShape *shape, cpVect p){
+	return shape.klass.pointQuery(shape, p);
+}
+
+cpBool
+cpShapeSegmentQuery(cpShape *shape, cpVect a, cpVect b, cpSegmentQueryInfo *info){
+	cpSegmentQueryInfo blank = {null, 0.0f, cpvzero};
+	(*info) = blank;
+	
+	shape.klass.segmentQuery(shape, a, b, info);
+	return (info.shape !is null);
+}
+
+void
+cpSegmentQueryInfoPrint(cpSegmentQueryInfo *info)
+{
+	writefln("Segment Query:\n");
+	writefln("\tt: %s\n", info.t);
+//	writefln("\tdist: %f\n", info.dist);
+//	writefln("\tpoint: %s\n", cpvstr(info.point));
+	//writefln("\tn: %s\n", cpvstr(info.n)); //TODO:
+}
+
+cpCircleShape *
+cpCircleShapeAlloc()
+{
+	return cast(cpCircleShape *)cpcalloc(1, cpCircleShape.sizeof);
+}
+
+static cpBB
+bbFromCircle(const cpVect c, const cpFloat r)
+{
+	return cpBBNew(c.x-r, c.y-r, c.x+r, c.y+r);
+}
+
+static cpBB
+cpCircleShapeCacheData(cpShape *shape, cpVect p, cpVect rot)
+{
+	cpCircleShape *circle = cast(cpCircleShape *)shape;
+	
+	circle.tc = cpvadd(p, cpvrotate(circle.c, rot));
+	return bbFromCircle(circle.tc, circle.r);
+}
+
+static cpBool
+cpCircleShapePointQuery(cpShape *shape, cpVect p){
+	cpCircleShape *circle = cast(cpCircleShape *)shape;
+	return cpvnear(circle.tc, p, circle.r);
+}
+
+static void
+circleSegmentQuery(cpShape *shape, cpVect center, cpFloat r, cpVect a, cpVect b, cpSegmentQueryInfo *info)
+{
+	// offset the line to be relative to the circle
+	a = cpvsub(a, center);
+	b = cpvsub(b, center);
+	
+	cpFloat qa = cpvdot(a, a) - 2.0f*cpvdot(a, b) + cpvdot(b, b);
+	cpFloat qb = -2.0f*cpvdot(a, a) + 2.0f*cpvdot(a, b);
+	cpFloat qc = cpvdot(a, a) - r*r;
+	
+	cpFloat det = qb*qb - 4.0f*qa*qc;
+	
+	if(det >= 0.0f){
+		cpFloat t = (-qb - cpfsqrt(det))/(2.0f*qa);
+		if(0.0f<= t && t <= 1.0f){
+			info.shape = shape;
+			info.t = t;
+			info.n = cpvnormalize(cpvlerp(a, b, t));
+		}
+	}
+}
+
+static void
+cpCircleShapeSegmentQuery(cpShape *shape, cpVect a, cpVect b, cpSegmentQueryInfo *info)
+{
+	cpCircleShape *circle = cast(cpCircleShape *)shape;
+	circleSegmentQuery(shape, circle.tc, circle.r, a, b, info);
+}
+
+static /+const+/ cpShapeClass cpCircleShapeClass = {
+	cpShapeType.CP_CIRCLE_SHAPE,
+	&cpCircleShapeCacheData,
+	null,
+	&cpCircleShapePointQuery,
+	&cpCircleShapeSegmentQuery,
+};
+
+cpCircleShape *
+cpCircleShapeInit(cpCircleShape *circle, cpBody *_body, cpFloat radius, cpVect offset)
+{
+	circle.c = offset;
+	circle.r = radius;
+	
+	cpShapeInit(cast(cpShape *)circle, &cpCircleShapeClass, _body);
+	
+	return circle;
+}
+
+cpShape *
+cpCircleShapeNew(cpBody *_body, cpFloat radius, cpVect offset)
+{
+	return cast(cpShape *)cpCircleShapeInit(cpCircleShapeAlloc(), _body, radius, offset);
+}
+//TODO:
+//CP_DefineShapeGetter(cpCircleShape, cpVect, c, Offset)
+//CP_DefineShapeGetter(cpCircleShape, cpFloat, r, Radius)
+
+cpSegmentShape *
+cpSegmentShapeAlloc()
+{
+	return cast(cpSegmentShape *)cpcalloc(1, cpSegmentShape.sizeof);
+}
+
+static cpBB
+cpSegmentShapeCacheData(cpShape *shape, cpVect p, cpVect rot)
+{
+	cpSegmentShape *seg = cast(cpSegmentShape *)shape;
+	
+	seg.ta = cpvadd(p, cpvrotate(seg.a, rot));
+	seg.tb = cpvadd(p, cpvrotate(seg.b, rot));
+	seg.tn = cpvrotate(seg.n, rot);
+	
+	cpFloat l,r,s,t;
+	
+	if(seg.ta.x < seg.tb.x){
+		l = seg.ta.x;
+		r = seg.tb.x;
+	} else {
+		l = seg.tb.x;
+		r = seg.ta.x;
+	}
+	
+	if(seg.ta.y < seg.tb.y){
+		s = seg.ta.y;
+		t = seg.tb.y;
+	} else {
+		s = seg.tb.y;
+		t = seg.ta.y;
+	}
+	
+	cpFloat rad = seg.r;
+	return cpBBNew(l - rad, s - rad, r + rad, t + rad);
+}
+
+static cpBool
+cpSegmentShapePointQuery(cpShape *shape, cpVect p){
+	if(!cpBBcontainsVect(shape.bb, p)) return cpFalse;
+	
+	cpSegmentShape *seg = cast(cpSegmentShape *)shape;
+	
+	// Calculate normal distance from segment.
+	cpFloat dn = cpvdot(seg.tn, p) - cpvdot(seg.ta, seg.tn);
+	cpFloat dist = cpfabs(dn) - seg.r;
+	if(dist > 0.0f) return cpFalse;
+	
+	// Calculate tangential distance along segment.
+	cpFloat dt = -cpvcross(seg.tn, p);
+	cpFloat dtMin = -cpvcross(seg.tn, seg.ta);
+	cpFloat dtMax = -cpvcross(seg.tn, seg.tb);
+	
+	// Decision tree to decide which feature of the segment to collide with.
+	if(dt <= dtMin){
+		if(dt < (dtMin - seg.r)){
+			return cpFalse;
+		} else {
+			return cpvlengthsq(cpvsub(seg.ta, p)) < (seg.r*seg.r);
+		}
+	} else {
+		if(dt < dtMax){
+			return cpTrue;
+		} else {
+			if(dt < (dtMax + seg.r)) {
+				return cpvlengthsq(cpvsub(seg.tb, p)) < (seg.r*seg.r);
+			} else {
+				return cpFalse;
+			}
+		}
+	}
+	
+	return cpTrue;	
+}
+
+static cpBool inUnitRange(cpFloat t){return (0.0f < t && t < 1.0f);}
+
+static void
+cpSegmentShapeSegmentQuery(cpShape *shape, cpVect a, cpVect b, cpSegmentQueryInfo *info)
+{
+	// TODO this function could be optimized better.
+	
+	cpSegmentShape *seg = cast(cpSegmentShape *)shape;
+	cpVect n = seg.tn;
+	// flip n if a is behind the axis
+	if(cpvdot(a, n) < cpvdot(seg.ta, n))
+		n = cpvneg(n);
+	
+	cpFloat an = cpvdot(a, n);
+	cpFloat bn = cpvdot(b, n);
+	
+	if(an != bn){
+		cpFloat d = cpvdot(seg.ta, n) + seg.r;
+		cpFloat t = (d - an)/(bn - an);
+		
+		if(0.0f < t && t < 1.0f){
+			cpVect point = cpvlerp(a, b, t);
+			cpFloat dt = -cpvcross(seg.tn, point);
+			cpFloat dtMin = -cpvcross(seg.tn, seg.ta);
+			cpFloat dtMax = -cpvcross(seg.tn, seg.tb);
+			
+			if(dtMin < dt && dt < dtMax){
+				info.shape = shape;
+				info.t = t;
+				info.n = n;
+				
+				return; // don't continue on and check endcaps
+			}
+		}
+	}
+	
+	if(seg.r) {
+		cpSegmentQueryInfo info1 = {null, 1.0f, cpvzero};
+		cpSegmentQueryInfo info2 = {null, 1.0f, cpvzero};
+		circleSegmentQuery(shape, seg.ta, seg.r, a, b, &info1);
+		circleSegmentQuery(shape, seg.tb, seg.r, a, b, &info2);
+		
+		if(info1.t < info2.t){
+			(*info) = info1;
+		} else {
+			(*info) = info2;
+		}
+	}
+}
+
+static /+const+/ cpShapeClass cpSegmentShapeClass = {
+	cpShapeType.CP_SEGMENT_SHAPE,
+	&cpSegmentShapeCacheData,
+	null,
+	&cpSegmentShapePointQuery,
+	&cpSegmentShapeSegmentQuery,
+};
+
+cpSegmentShape *
+cpSegmentShapeInit(cpSegmentShape *seg, cpBody *_body, cpVect a, cpVect b, cpFloat r)
+{
+	seg.a = a;
+	seg.b = b;
+	seg.n = cpvperp(cpvnormalize(cpvsub(b, a)));
+	
+	seg.r = r;
+	
+	cpShapeInit(cast(cpShape *)seg, &cpSegmentShapeClass, _body);
+	
+	return seg;
+}
+
+cpShape*
+cpSegmentShapeNew(cpBody *_body, cpVect a, cpVect b, cpFloat r)
+{
+	return cast(cpShape *)cpSegmentShapeInit(cpSegmentShapeAlloc(), _body, a, b, r);
+}
+//TODO:
+//CP_DefineShapeGetter(cpSegmentShape, cpVect, a, A)
+//CP_DefineShapeGetter(cpSegmentShape, cpVect, b, B)
+//CP_DefineShapeGetter(cpSegmentShape, cpVect, n, Normal)
+//CP_DefineShapeGetter(cpSegmentShape, cpFloat, r, Radius)
+
+// Unsafe API (chipmunk_unsafe.h)
+
+void
+cpCircleShapeSetRadius(cpShape *shape, cpFloat radius)
+{
+	assert(shape.klass is &cpCircleShapeClass, "Shape is not a circle shape.");
+	cpCircleShape *circle = cast(cpCircleShape *)shape;
+	
+	circle.r = radius;
+}
+
+void
+cpCircleShapeSetOffset(cpShape *shape, cpVect offset)
+{
+	assert(shape.klass is &cpCircleShapeClass, "Shape is not a circle shape.");
+	cpCircleShape *circle = cast(cpCircleShape *)shape;
+	
+	circle.c = offset;
+}
+
+void
+cpSegmentShapeSetEndpoints(cpShape *shape, cpVect a, cpVect b)
+{
+	assert(shape.klass is &cpSegmentShapeClass, "Shape is not a segment shape.");
+	cpSegmentShape *seg = cast(cpSegmentShape *)shape;
+	
+	seg.a = a;
+	seg.b = b;
+	seg.n = cpvperp(cpvnormalize(cpvsub(b, a)));
+}
+
+void
+cpSegmentShapeSetRadius(cpShape *shape, cpFloat radius)
+{
+	assert(shape.klass is &cpSegmentShapeClass, "Shape is not a segment shape.");
+	cpSegmentShape *seg = cast(cpSegmentShape *)shape;
+	
+	seg.r = radius;
+}

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/trunk/chipmunkd/cpSpace.d	Thu Dec 02 02:11:26 2010 +0100
@@ -0,0 +1,642 @@
+
+// written in the D programming language
+
+module chipmunkd.cpSpace;
+
+import chipmunkd.chipmunk;
+import chipmunkd.chipmunk_types_h;
+import chipmunkd.cpVect,chipmunkd.cpVect_h;
+import chipmunkd.cpHashSet;
+import chipmunkd.cpCollision;
+import chipmunkd.cpBody;
+import chipmunkd.cpArray;
+import chipmunkd.cpShape;
+import chipmunkd.cpBB;
+import chipmunkd.cpArbiter;
+import chipmunkd.cpSpaceHash;
+import chipmunkd.constraints.cpConstraint;
+import chipmunkd.cpSpaceQuery;
+
+// Number of frames that contact information should persist.
+//extern cpTimestamp cp_contact_persistence;
+
+// User collision handler function types.
+alias cpBool	function(cpArbiter *arb, cpSpace *space, void *data)cpCollisionBeginFunc;
+alias cpBool	function(cpArbiter *arb, cpSpace *space, void *data)cpCollisionPreSolveFunc;
+alias void		function(cpArbiter *arb, cpSpace *space, void *data)cpCollisionPostSolveFunc;
+alias void		function(cpArbiter *arb, cpSpace *space, void *data)cpCollisionSeparateFunc;
+
+// Structure for holding collision pair function information.
+// Used internally.
+struct cpCollisionHandler {
+	cpCollisionType a;
+	cpCollisionType b;
+	cpCollisionBeginFunc begin;
+	cpCollisionPreSolveFunc preSolve;
+	cpCollisionPostSolveFunc postSolve;
+	cpCollisionSeparateFunc separate;
+	void *data;
+}
+
+enum CP_MAX_CONTACTS_PER_ARBITER = 6;
+struct cpContactBufferHeader {
+	cpTimestamp stamp;
+	cpContactBufferHeader *next;
+	uint numContacts;
+}
+
+struct cpSpace{
+	// *** User definable fields
+	
+	// Number of iterations to use in the impulse solver to solve contacts.
+	int iterations;
+	
+	// Number of iterations to use in the impulse solver to solve elastic collisions.
+	int elasticIterations;
+	
+	// Default gravity to supply when integrating rigid body motions.
+	cpVect gravity;
+	
+	// Default damping to supply when integrating rigid body motions.
+	cpFloat damping;
+	
+	// Speed threshold for a body to be considered idle.
+	// The default value of 0 means to let the space guess a good threshold based on gravity.
+	cpFloat idleSpeedThreshold;
+	
+	// Time a group of bodies must remain idle in order to fall asleep
+	// The default value of INFINITY disables the sleeping algorithm.
+	cpFloat sleepTimeThreshold;
+	
+	// *** Internally Used Fields
+	
+	// When the space is locked, you should not add or remove objects;
+	cpBool locked;
+	
+	// Time stamp. Is incremented on every call to cpSpaceStep().
+	cpTimestamp stamp;
+
+	// The static and active shape spatial hashes.
+	cpSpaceHash* staticShapes;
+	cpSpaceHash* activeShapes;
+	
+	// List of bodies in the system.
+	cpArray *bodies;
+	
+	// List of groups of sleeping bodies.
+	cpArray *sleepingComponents;
+	
+	// List of active arbiters for the impulse solver.
+	cpArray* arbiters, pooledArbiters;
+	
+	// Linked list ring of contact buffers.
+	// Head is the newest buffer, and each buffer points to a newer buffer.
+	// Head wraps around and points to the oldest (tail) buffer.
+	cpContactBufferHeader* contactBuffersHead, _contactBuffersTail;
+	
+	// List of buffers to be free()ed when destroying the space.
+	cpArray *allocatedBuffers;
+	
+	// Persistant contact set.
+	cpHashSet *contactSet;
+	
+	// List of constraints in the system.
+	cpArray *constraints;
+	
+	// Set of collisionpair functions.
+	cpHashSet *collFuncSet;
+	// Default collision handler.
+	cpCollisionHandler defaultHandler;
+	
+	cpHashSet *postStepCallbacks;
+	
+	cpBody staticBody;
+}
+
+//// Basic allocation/destruction functions.
+//cpSpace* cpSpaceAlloc(void);
+//cpSpace* cpSpaceInit(cpSpace *space);
+//cpSpace* cpSpaceNew(void);
+//
+//void cpSpaceDestroy(cpSpace *space);
+//void cpSpaceFree(cpSpace *space);
+//
+//// Convenience function. Frees all referenced entities. (bodies, shapes and constraints)
+//void cpSpaceFreeChildren(cpSpace *space);
+//
+//// Collision handler management functions.
+//void cpSpaceSetDefaultCollisionHandler(
+//	cpSpace *space,
+//	cpCollisionBeginFunc begin,
+//	cpCollisionPreSolveFunc preSolve,
+//	cpCollisionPostSolveFunc postSolve,
+//	cpCollisionSeparateFunc separate,
+//	void *data
+//);
+//void cpSpaceAddCollisionHandler(
+//	cpSpace *space,
+//	cpCollisionType a, cpCollisionType b,
+//	cpCollisionBeginFunc begin,
+//	cpCollisionPreSolveFunc preSolve,
+//	cpCollisionPostSolveFunc postSolve,
+//	cpCollisionSeparateFunc separate,
+//	void *data
+//);
+//void cpSpaceRemoveCollisionHandler(cpSpace *space, cpCollisionType a, cpCollisionType b);
+//
+//// Add and remove entities from the system.
+//cpShape *cpSpaceAddShape(cpSpace *space, cpShape *shape);
+//cpShape *cpSpaceAddStaticShape(cpSpace *space, cpShape *shape);
+//cpBody *cpSpaceAddBody(cpSpace *space, cpBody *body);
+//cpConstraint *cpSpaceAddConstraint(cpSpace *space, cpConstraint *constraint);
+//
+//void cpSpaceRemoveShape(cpSpace *space, cpShape *shape);
+//void cpSpaceRemoveStaticShape(cpSpace *space, cpShape *shape);
+//void cpSpaceRemoveBody(cpSpace *space, cpBody *body);
+//void cpSpaceRemoveConstraint(cpSpace *space, cpConstraint *constraint);
+//
+//// Post Step function definition
+alias void function(cpSpace *space, void *obj, void *data) cpPostStepFunc;
+//// Register a post step function to be called after cpSpaceStep() has finished.
+//// obj is used a key, you can only register one callback per unique value for obj
+//void cpSpaceAddPostStepCallback(cpSpace *space, cpPostStepFunc func, void *obj, void *data);
+//
+//// Point query callback function
+alias void function(cpShape *shape, void *data) cpSpacePointQueryFunc;
+//void cpSpacePointQuery(cpSpace *space, cpVect point, cpLayers layers, cpGroup group, cpSpacePointQueryFunc func, void *data);
+//cpShape *cpSpacePointQueryFirst(cpSpace *space, cpVect point, cpLayers layers, cpGroup group);
+//
+//// Segment query callback function
+alias void function(cpShape *shape, cpFloat t, cpVect n, void *data)cpSpaceSegmentQueryFunc;
+//void cpSpaceSegmentQuery(cpSpace *space, cpVect start, cpVect end, cpLayers layers, cpGroup group, cpSpaceSegmentQueryFunc func, void *data);
+//cpShape *cpSpaceSegmentQueryFirst(cpSpace *space, cpVect start, cpVect end, cpLayers layers, cpGroup group, cpSegmentQueryInfo *out);
+//
+//// BB query callback function
+alias void function(cpShape *shape, void *data)cpSpaceBBQueryFunc;
+//void cpSpaceBBQuery(cpSpace *space, cpBB bb, cpLayers layers, cpGroup group, cpSpaceBBQueryFunc func, void *data);
+//
+//
+//// Iterator function for iterating the bodies in a space.
+alias void function(cpBody *_body, void *data)cpSpaceBodyIterator;
+//void cpSpaceEachBody(cpSpace *space, cpSpaceBodyIterator func, void *data);
+//
+//// Spatial hash management functions.
+//void cpSpaceResizeStaticHash(cpSpace *space, cpFloat dim, int count);
+//void cpSpaceResizeActiveHash(cpSpace *space, cpFloat dim, int count);
+//void cpSpaceRehashStatic(cpSpace *space);
+//
+//void cpSpaceRehashShape(cpSpace *space, cpShape *shape);
+//
+//// Update the space.
+//void cpSpaceStep(cpSpace *space, cpFloat dt);
+
+
+cpTimestamp cp_contact_persistence = 3;
+
+// Equal function for contactSet.
+static cpBool
+contactSetEql(cpShape **shapes, cpArbiter *arb)
+{
+	cpShape *a = shapes[0];
+	cpShape *b = shapes[1];
+	
+	return ((a == arb.private_a && b == arb.private_b) || (b == arb.private_a && a == arb.private_b));
+}
+
+// Transformation function for contactSet.
+static void *
+contactSetTrans(cpShape **shapes, cpSpace *space)
+{
+	if(space.pooledArbiters.num == 0){
+		// arbiter pool is exhausted, make more
+		int count = CP_BUFFER_BYTES/cpArbiter.sizeof;
+		assert(count, "Buffer size too small.");
+		
+		cpArbiter *buffer = cast(cpArbiter *)cpmalloc(CP_BUFFER_BYTES);
+		cpArrayPush(space.allocatedBuffers, buffer);
+		
+		for(int i=0; i<count; i++) cpArrayPush(space.pooledArbiters, buffer + i);
+	}
+	
+	return cpArbiterInit(cast(cpArbiter *) cpArrayPop(space.pooledArbiters), shapes[0], shapes[1]);
+}
+
+// Equals function for collFuncSet.
+static cpBool
+collFuncSetEql(cpCollisionHandler *check, cpCollisionHandler *pair)
+{
+	return ((check.a == pair.a && check.b == pair.b) || (check.b == pair.a && check.a == pair.b));
+}
+
+// Transformation function for collFuncSet.
+static void *
+collFuncSetTrans(cpCollisionHandler *handler, void *unused)
+{
+	cpCollisionHandler *copy = cast(cpCollisionHandler *)cpmalloc(cpCollisionHandler.sizeof);
+	(*copy) = (*handler);
+	
+	return copy;
+}
+
+// Default collision functions.
+static cpBool alwaysCollide(cpArbiter *arb, cpSpace *space, void *data){return 1;}
+static void nothing(cpArbiter *arb, cpSpace *space, void *data){}
+
+// BBfunc callback for the spatial hash.
+static cpBB shapeBBFunc(cpShape *shape){return shape.bb;}
+
+// Iterator functions for destructors.
+static void             freeWrap(void         *ptr, void *unused){            cpfree(ptr);}
+static void        shapeFreeWrap(cpShape      *ptr, void *unused){     cpShapeFree(ptr);}
+static void         bodyFreeWrap(cpBody       *ptr, void *unused){      cpBodyFree(ptr);}
+static void   constraintFreeWrap(cpConstraint *ptr, void *unused){cpConstraintFree(ptr);}
+
+cpSpace *
+cpSpaceAlloc()
+{
+	return cast(cpSpace *)cpcalloc(1, cpSpace.sizeof);
+}
+
+enum DEFAULT_DIM_SIZE				= 100.0f;
+enum DEFAULT_COUNT					= 1000;
+enum DEFAULT_ITERATIONS				= 10;
+enum DEFAULT_ELASTIC_ITERATIONS		= 0;
+
+cpCollisionHandler defaultHandler = {0, 0, &alwaysCollide, &alwaysCollide, &nothing, &nothing, null};
+
+cpSpace*
+cpSpaceInit(cpSpace *space)
+{
+	space.iterations = DEFAULT_ITERATIONS;
+	space.elasticIterations = DEFAULT_ELASTIC_ITERATIONS;
+//	space.sleepTicks = 300;
+	
+	space.gravity = cpvzero;
+	space.damping = 1.0f;
+	
+	space.locked = 0;
+	space.stamp = 0;
+
+	space.staticShapes = cpSpaceHashNew(DEFAULT_DIM_SIZE, DEFAULT_COUNT, cast(cpSpaceHashBBFunc)&shapeBBFunc);
+	space.activeShapes = cpSpaceHashNew(DEFAULT_DIM_SIZE, DEFAULT_COUNT, cast(cpSpaceHashBBFunc)&shapeBBFunc);
+	
+	space.allocatedBuffers = cpArrayNew(0);
+	
+	space.bodies = cpArrayNew(0);
+	space.sleepingComponents = cpArrayNew(0);
+	space.sleepTimeThreshold = INFINITY;
+	space.idleSpeedThreshold = 0.0f;
+	
+	space.arbiters = cpArrayNew(0);
+	space.pooledArbiters = cpArrayNew(0);
+	
+	space.contactBuffersHead = null;
+	space.contactSet = cpHashSetNew(0, cast(cpHashSetEqlFunc)&contactSetEql, cast(cpHashSetTransFunc)&contactSetTrans);
+	
+	space.constraints = cpArrayNew(0);
+	
+	space.defaultHandler = defaultHandler;
+	space.collFuncSet = cpHashSetNew(0, cast(cpHashSetEqlFunc)&collFuncSetEql, cast(cpHashSetTransFunc)&collFuncSetTrans);
+	space.collFuncSet.default_value = &space.defaultHandler;
+	
+	space.postStepCallbacks = null;
+	
+	cpBodyInit(&space.staticBody, INFINITY, INFINITY);
+	space.staticBody.space = space;
+	
+	return space;
+}
+
+cpSpace*
+cpSpaceNew()
+{
+	return cpSpaceInit(cpSpaceAlloc());
+}
+
+void
+cpSpaceDestroy(cpSpace *space)
+{
+	cpSpaceHashFree(space.staticShapes);
+	cpSpaceHashFree(space.activeShapes);
+	
+	cpArrayFree(space.bodies);
+	cpArrayFree(space.sleepingComponents);
+	
+	cpArrayFree(space.constraints);
+	
+	cpHashSetFree(space.contactSet);
+	
+	cpArrayFree(space.arbiters);
+	cpArrayFree(space.pooledArbiters);
+	
+	if(space.allocatedBuffers){
+		cpArrayEach(space.allocatedBuffers, &freeWrap, null);
+		cpArrayFree(space.allocatedBuffers);
+	}
+	
+	if(space.postStepCallbacks){
+		cpHashSetEach(space.postStepCallbacks, &freeWrap, null);
+		cpHashSetFree(space.postStepCallbacks);
+	}
+	
+	if(space.collFuncSet){
+		cpHashSetEach(space.collFuncSet, &freeWrap, null);
+		cpHashSetFree(space.collFuncSet);
+	}
+}
+
+void
+cpSpaceFree(cpSpace *space)
+{
+	if(space){
+		cpSpaceDestroy(space);
+		cpfree(space);
+	}
+}
+
+void
+cpSpaceFreeChildren(cpSpace *space)
+{
+	cpArray *components = space.sleepingComponents;
+	while(components.num) cpBodyActivate(cast(cpBody *)components.arr[0]);
+	
+	cpSpaceHashEach(space.staticShapes, cast(cpSpaceHashIterator)&shapeFreeWrap, null);
+	cpSpaceHashEach(space.activeShapes, cast(cpSpaceHashIterator)&shapeFreeWrap, null);
+	cpArrayEach(space.bodies,           cast(cpArrayIter)&bodyFreeWrap,          null);
+	cpArrayEach(space.constraints,      cast(cpArrayIter)&constraintFreeWrap,    null);
+}
+
+void
+cpSpaceAddCollisionHandler(
+	cpSpace *space,
+	cpCollisionType a, cpCollisionType b,
+	cpCollisionBeginFunc begin,
+	cpCollisionPreSolveFunc preSolve,
+	cpCollisionPostSolveFunc postSolve,
+	cpCollisionSeparateFunc separate,
+	void *data
+){
+	// Remove any old function so the new one will get added.
+	cpSpaceRemoveCollisionHandler(space, a, b);
+	
+	cpCollisionHandler handler = {
+		a, b,
+		begin ? begin : &alwaysCollide,
+		preSolve ? preSolve : &alwaysCollide,
+		postSolve ? postSolve : &nothing,
+		separate ? separate : &nothing,
+		data
+	};
+	
+	cpHashSetInsert(space.collFuncSet, CP_HASH_PAIR(a, b), &handler, null);
+}
+
+void
+cpSpaceRemoveCollisionHandler(cpSpace *space, cpCollisionType a, cpCollisionType b)
+{
+	struct tmp{cpCollisionType a, b;} tmp ids = {a, b};
+	cpCollisionHandler *old_handler = cast(cpCollisionHandler *) cpHashSetRemove(space.collFuncSet, CP_HASH_PAIR(a, b), &ids);
+	cpfree(old_handler);
+}
+
+void
+cpSpaceSetDefaultCollisionHandler(
+	cpSpace *space,
+	cpCollisionBeginFunc begin,
+	cpCollisionPreSolveFunc preSolve,
+	cpCollisionPostSolveFunc postSolve,
+	cpCollisionSeparateFunc separate,
+	void *data
+){
+	cpCollisionHandler handler = {
+		0, 0,
+		begin ? begin : &alwaysCollide,
+		preSolve ? preSolve : &alwaysCollide,
+		postSolve ? postSolve : &nothing,
+		separate ? separate : &nothing,
+		data
+	};
+	
+	space.defaultHandler = handler;
+}
+
+//#pragma mark Body, Shape, and Joint Management
+
+void cpAssertSpaceUnlocked(cpSpace* _space){
+	assert(!_space.locked,
+		"This addition/removal cannot be done safely during a call to cpSpaceStep(). "
+		"Put these calls into a Post Step Callback."
+	);}
+
+static void
+cpBodyAddShape(cpBody *_body, cpShape *shape)
+{
+	shape.next = shape._body.shapesList;
+	shape._body.shapesList = shape;
+}
+
+static void
+cpBodyRemoveShape(cpBody *_body, cpShape *shape)
+{
+	cpShape **prev_ptr = &_body.shapesList;
+	cpShape *node = _body.shapesList;
+	
+	while(node && node != shape){
+		prev_ptr = &node.next;
+		node = node.next;
+	}
+	
+	assert(node, "Attempted to remove a shape from a body it was never attached to.");
+	(*prev_ptr) = node.next;
+}
+
+cpShape *
+cpSpaceAddShape(cpSpace *space, cpShape *shape)
+{
+	cpBody *_body = shape._body;
+	if(!_body || _body == &space.staticBody) return cpSpaceAddStaticShape(space, shape);
+	
+	assert(!cpHashSetFind(space.activeShapes.handleSet, shape.hashid, shape),
+		"Cannot add the same shape more than once.");
+	cpAssertSpaceUnlocked(space);
+	
+	cpBodyActivate(_body);
+	cpBodyAddShape(_body, shape);
+	
+	cpShapeCacheBB(shape);
+	cpSpaceHashInsert(space.activeShapes, shape, shape.hashid, shape.bb);
+		
+	return shape;
+}
+
+static void
+activateShapesTouchingShapeHelper(cpShape *shape, void *unused)
+{
+	cpBodyActivate(shape._body);
+}
+
+static void
+activateShapesTouchingShape(cpSpace *space, cpShape *shape)
+{
+	// TODO this query should be more precise
+	// Use shape queries once they are written
+	cpSpaceBBQuery(space, shape.bb, shape.layers, shape.group, &activateShapesTouchingShapeHelper, null);
+}
+
+cpShape *
+cpSpaceAddStaticShape(cpSpace *space, cpShape *shape)
+{
+	assert(!cpHashSetFind(space.staticShapes.handleSet, shape.hashid, shape),
+		"Cannot add the same static shape more than once.");
+	cpAssertSpaceUnlocked(space);
+	
+	if(!shape._body) shape._body = &space.staticBody;
+	
+	cpShapeCacheBB(shape);
+	activateShapesTouchingShape(space, shape);
+	cpSpaceHashInsert(space.staticShapes, shape, shape.hashid, shape.bb);
+	
+	return shape;
+}
+
+cpBody *
+cpSpaceAddBody(cpSpace *space, cpBody *_body)
+{
+	cpAssertWarn(_body.m != INFINITY, "Did you really mean to add an infinite mass body to the space?");
+	assert(!_body.space, "Cannot add a body to a more than one space or to the same space twice.");
+//	cpAssertSpaceUnlocked(space); This should be safe as long as it's not from an integration callback
+	
+	cpArrayPush(space.bodies, _body);
+	_body.space = space;
+	
+	return _body;
+}
+
+cpConstraint *
+cpSpaceAddConstraint(cpSpace *space, cpConstraint *constraint)
+{
+	assert(!cpArrayContains(space.constraints, constraint), "Cannot add the same constraint more than once.");
+//	cpAssertSpaceUnlocked(space); This should be safe as long as its not from a constraint callback.
+	
+	if(!constraint.a) constraint.a = &space.staticBody;
+	if(!constraint.b) constraint.b = &space.staticBody;
+	
+	cpBodyActivate(constraint.a);
+	cpBodyActivate(constraint.b);
+	cpArrayPush(space.constraints, constraint);
+	
+	return constraint;
+}
+
+struct removalContext {
+	cpSpace *space;
+	cpShape *shape;
+}
+
+// Hashset filter func to throw away old arbiters.
+static cpBool
+contactSetFilterRemovedShape(cpArbiter *arb, removalContext *context)
+{
+	if(context.shape == arb.private_a || context.shape == arb.private_b){
+		arb.handler.separate(arb, context.space, arb.handler.data);
+		cpArrayPush(context.space.pooledArbiters, arb);
+		return cpFalse;
+	}
+	
+	return cpTrue;
+}
+
+void
+cpSpaceRemoveShape(cpSpace *space, cpShape *shape)
+{
+	cpBody *_body = shape._body;
+	if(cpBodyIsStatic(_body)){
+		cpSpaceRemoveStaticShape(space, shape);
+		return;
+	}
+
+	cpBodyActivate(_body);
+	
+	cpAssertSpaceUnlocked(space);
+	cpAssertWarn(cpHashSetFind(space.activeShapes.handleSet, shape.hashid, shape) !is null,
+		"Cannot remove a shape that was never added to the space. (Removed twice maybe?)");
+	
+	cpBodyRemoveShape(_body, shape);
+	
+	removalContext context = {space, shape};
+	cpHashSetFilter(space.contactSet, cast(cpHashSetFilterFunc)&contactSetFilterRemovedShape, &context);
+	cpSpaceHashRemove(space.activeShapes, shape, shape.hashid);
+}
+
+void
+cpSpaceRemoveStaticShape(cpSpace *space, cpShape *shape)
+{
+	cpAssertWarn(cpHashSetFind(space.staticShapes.handleSet, shape.hashid, shape) !is null,
+		"Cannot remove a static or sleeping shape that was never added to the space. (Removed twice maybe?)");
+	cpAssertSpaceUnlocked(space);
+	
+	removalContext context = {space, shape};
+	cpHashSetFilter(space.contactSet, cast(cpHashSetFilterFunc)&contactSetFilterRemovedShape, &context);
+	cpSpaceHashRemove(space.staticShapes, shape, shape.hashid);
+	
+	activateShapesTouchingShape(space, shape);
+}
+
+void
+cpSpaceRemoveBody(cpSpace *space, cpBody *_body)
+{
+	cpAssertWarn(_body.space == space,
+		"Cannot remove a body that was never added to the space. (Removed twice maybe?)");
+	cpAssertSpaceUnlocked(space);
+	
+	cpBodyActivate(_body);
+	cpArrayDeleteObj(space.bodies, _body);
+	_body.space = null;
+}
+
+void
+cpSpaceRemoveConstraint(cpSpace *space, cpConstraint *constraint)
+{
+	cpAssertWarn(cpArrayContains(space.constraints, constraint),
+		"Cannot remove a constraint that was never added to the space. (Removed twice maybe?)");
+//	cpAssertSpaceUnlocked(space); Should be safe as long as its not from a constraint callback.
+	
+	cpBodyActivate(constraint.a);
+	cpBodyActivate(constraint.b);
+	cpArrayDeleteObj(space.constraints, constraint);
+}
+
+//#pragma mark Spatial Hash Management
+
+static void updateBBCache(cpShape *shape, void *unused){cpShapeCacheBB(shape);}
+
+void
+cpSpaceResizeStaticHash(cpSpace *space, cpFloat dim, int count)
+{
+	cpSpaceHashResize(space.staticShapes, dim, count);
+	cpSpaceHashRehash(space.staticShapes);
+}
+
+void
+cpSpaceResizeActiveHash(cpSpace *space, cpFloat dim, int count)
+{
+	cpSpaceHashResize(space.activeShapes, dim, count);
+}
+
+void 
+cpSpaceRehashStatic(cpSpace *space)
+{
+	cpSpaceHashEach(space.staticShapes, cast(cpSpaceHashIterator)&updateBBCache, null);
+	cpSpaceHashRehash(space.staticShapes);
+}
+
+void
+cpSpaceRehashShape(cpSpace *space, cpShape *shape)
+{
+	cpShapeCacheBB(shape);
+	
+	// attempt to rehash the shape in both hashes
+	cpSpaceHashRehashObject(space.activeShapes, shape, shape.hashid);
+	cpSpaceHashRehashObject(space.staticShapes, shape, shape.hashid);
+}
+