--- a/trunk/chipmunkd/chipmunk.d	Thu Dec 09 22:25:04 2010 +0100
+++ b/trunk/chipmunkd/chipmunk.d	Fri Dec 10 02:10:27 2010 +0100
@@ -6,6 +6,7 @@
 import std.stdio;
 import std.string:format;
 import std.conv:to;
+import std.math:PI;
 
 void
 cpMessage(string message, string condition, string file, int line, bool isError)
@@ -92,8 +93,17 @@
 cpFloat
 cpMomentForCircle(cpFloat m, cpFloat r1, cpFloat r2, cpVect offset)
 {
-	return (1.0f/2.0f)*m*(r1*r1 + r2*r2) + m*cpvdot(offset, offset);
+	return m*(0.5f*(r1*r1 + r2*r2) + cpvlengthsq(offset));
 }
+
+/**
+	Calculate area of a hollow circle.
+*/
+cpFloat cpAreaForCircle(cpFloat r1, cpFloat r2)
+{
+	return 2.0f*cast(cpFloat)PI*cpfabs(r1*r1 - r2*r2);
+}
+
 /**
 	Calculate the moment of inertia for a line segment.
 	Beveling radius is not supported.
@@ -104,7 +114,16 @@
 	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);
+	return m*(length*length/12.0f + cpvlengthsq(offset));
+}
+
+/**
+	Calculate the area of a fattened (capsule shaped) line segment.
+*/
+cpFloat
+cpAreaForSegment(cpVect a, cpVect b, cpFloat r)
+{
+	return 2.0f*r*(cast(cpFloat)PI*r + cpvdist(a, b));
 }
 
 /**
@@ -135,6 +154,51 @@
 }
 
 /**
+	Calculate the signed area of a polygon.
+*/
+cpFloat cpAreaForPoly(const int numVerts, const cpVect *verts)
+{
+	cpFloat area = 0.0f;
+	for(int i=0; i<numVerts; i++){
+		area += cpvcross(verts[i], verts[(i+1)%numVerts]);
+	}
+	
+	return area/2.0f;
+}
+	
+/**
+	Calculate the natural centroid of a polygon.
+*/
+cpVect cpCentroidForPoly(const int numVerts, const cpVect *verts)
+{
+	cpFloat sum = 0.0f;
+	cpVect vsum = cpvzero;
+	
+	for(int i=0; i<numVerts; i++){
+		cpVect v1 = verts[i];
+		cpVect v2 = verts[(i+1)%numVerts];
+		cpFloat cross = cpvcross(v1, v2);
+		
+		sum += cross;
+		vsum = cpvadd(vsum, cpvmult(cpvadd(v1, v2), cross));
+	}
+	
+	return cpvmult(vsum, 1.0f/(3.0f*sum));
+}
+
+/**
+	Center the polygon on the origin. (Subtracts the centroid of the polygon from each vertex)
+*/
+void cpRecenterPoly(const int numVerts, cpVect *verts)
+{
+	cpVect centroid = cpCentroidForPoly(numVerts, verts);
+	
+	for(int i=0; i<numVerts; i++){
+		verts[i] = cpvsub(verts[i], centroid);
+	}
+}
+
+/**
 	Calculate the moment of inertia for a solid box.
 */
 cpFloat

--- a/trunk/chipmunkd/chipmunk_types.d	Thu Dec 09 22:25:04 2010 +0100
+++ b/trunk/chipmunkd/chipmunk_types.d	Fri Dec 10 02:10:27 2010 +0100
@@ -106,6 +106,8 @@
 	{
 		cpFloat x = 0; cpFloat y=0;
 		
+		//TODO: basic operators
+		
 		string toString() const
 		{
 			return .format("(%s,%s)",x,y);

--- a/trunk/chipmunkd/cpArbiter.d	Thu Dec 09 22:25:04 2010 +0100
+++ b/trunk/chipmunkd/cpArbiter.d	Fri Dec 10 02:10:27 2010 +0100
@@ -37,6 +37,8 @@
 //cpVect cpContactsSumImpulses(cpContact *contacts, int numContacts);
 //cpVect cpContactsSumImpulsesWithFriction(cpContact *contacts, int numContacts);
 
+enum CP_MAX_CONTACTS_PER_ARBITER = 6;
+
 enum cpArbiterState {
 	cpArbiterStateNormal,
 	cpArbiterStateFirstColl,
@@ -55,8 +57,8 @@
 	// 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;
+	cpShape* a;
+	cpShape* b;
 	
 	// Calculated before calling the pre-solve collision handler
 	// Override them with custom values if you want specialized behavior
@@ -97,9 +99,9 @@
 cpArbiterGetShapes(/+const+/ cpArbiter *arb, cpShape **a, cpShape **b)
 {
 	if(arb.swappedColl){
-		(*a) = arb.private_b, (*b) = arb.private_a;
+		(*a) = arb.b, (*b) = arb.a;
 	} else {
-		(*a) = arb.private_a, (*b) = arb.private_b;
+		(*a) = arb.a, (*b) = arb.b;
 	}
 }
 
@@ -125,6 +127,12 @@
 	return arb.state == cpArbiterState.cpArbiterStateFirstColl;
 }
 
+static int
+cpArbiterGetCount(const cpArbiter *arb)
+{
+	return arb.numContacts;
+}
+
 static cpVect
 cpArbiterGetNormal(const cpArbiter *arb, int i)
 {
@@ -138,6 +146,43 @@
 	return arb.contacts[i].p;
 }
 
+
+static cpFloat 
+cpArbiteGetDepth(const cpArbiter *arb, int i)
+{
+	return arb.contacts[i].dist;
+}
+
+struct cpContactPointSet {
+	int count;
+	
+	struct TPoint
+	{
+		cpVect point, normal;
+		cpFloat dist = 0;
+	}
+	
+	TPoint[CP_MAX_CONTACTS_PER_ARBITER] points;
+}
+
+static cpContactPointSet
+cpArbiterGetContactPointSet(const cpArbiter *arb)
+{
+	cpContactPointSet set;
+	set.count = cpArbiterGetCount(arb);
+	
+	int i;
+	for(i=0; i<set.count; i++){
+		set.points[i].point = arb.contacts[i].p;
+		set.points[i].normal = arb.contacts[i].p;
+		set.points[i].dist = arb.contacts[i].dist;
+	}
+	
+	return set;
+}
+
+// cpArbiter.c --------------------------
+
 cpFloat cp_bias_coef = 0.1f;
 cpFloat cp_collision_slop = 0.1f;
 
@@ -218,11 +263,18 @@
 cpArbiter*
 cpArbiterInit(cpArbiter *arb, cpShape *a, cpShape *b)
 {
+	arb.handler = null;
+	arb.swappedColl = cpFalse;
+	   
+	arb.e = 0.0f;
+	arb.u = 0.0f;
+	arb.surface_vr = cpvzero;
+	
 	arb.numContacts = 0;
 	arb.contacts = null;
 	
-	arb.private_a = a;
-	arb.private_b = b;
+	arb.a = a;
+	arb.b = b;
 	
 	arb.stamp = 0;
 	arb.state = cpArbiterState.cpArbiterStateFirstColl;
@@ -271,8 +323,6 @@
 				}
 			}
 		}
-
-//		cpfree(arb.contacts);
 	}
 	
 	arb.contacts = contacts;
@@ -286,8 +336,8 @@
 	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;
+	arb.a = a;
+	arb.b = b;
 	
 	// mark it as new if it's been cached
 	if(arb.state == cpArbiterState.cpArbiterStateCached) arb.state = cpArbiterState.cpArbiterStateFirstColl;
@@ -296,8 +346,8 @@
 void
 cpArbiterPreStep(cpArbiter *arb, cpFloat dt_inv)
 {
-	cpBody *a = arb.private_a._body;
-	cpBody *b = arb.private_b._body;
+	cpBody *a = arb.a._body;
+	cpBody *b = arb.b._body;
 	
 	for(int i=0; i<arb.numContacts; i++){
 		cpContact *con = &arb.contacts[i];
@@ -322,8 +372,8 @@
 void
 cpArbiterApplyCachedImpulse(cpArbiter *arb)
 {
-	cpShape *shapea = arb.private_a;
-	cpShape *shapeb = arb.private_b;
+	cpShape *shapea = arb.a;
+	cpShape *shapeb = arb.b;
 		
 	arb.u = shapea.u * shapeb.u;
 	arb.surface_vr = cpvsub(shapeb.surface_v, shapea.surface_v);
@@ -340,8 +390,8 @@
 void
 cpArbiterApplyImpulse(cpArbiter *arb, cpFloat eCoef)
 {
-	cpBody *a = arb.private_a._body;
-	cpBody *b = arb.private_b._body;
+	cpBody *a = arb.a._body;
+	cpBody *b = arb.b._body;
 
 	for(int i=0; i<arb.numContacts; i++){
 		cpContact *con = &arb.contacts[i];

--- a/trunk/chipmunkd/cpBody.d	Thu Dec 09 22:25:04 2010 +0100
+++ b/trunk/chipmunkd/cpBody.d	Fri Dec 10 02:10:27 2010 +0100
@@ -101,7 +101,11 @@
 	return (_body.node.next !is null);
 }
 
-//cpBool cpBodyIsStatic(const cpBody *body);
+static cpBool
+cpBodyIsStatic(const cpBody *_body)
+{
+	return _body.node.idleTime == INFINITY;
+}
 
 static cpBool
 cpBodyIsRogue(const cpBody* _body)
@@ -266,6 +270,21 @@
 	return cpBodyInit(cpBodyAlloc(), m, i);
 }
 
+cpBody *
+cpBodyInitStatic(cpBody *_body)
+{
+	cpBodyInit(_body, cast(cpFloat)INFINITY, cast(cpFloat)INFINITY);
+	_body.node.idleTime = cast(cpFloat)INFINITY;
+	
+	return _body;
+}
+
+cpBody *
+cpBodyNewStatic()
+{
+	return cpBodyInitStatic(cpBodyAlloc());
+}
+
 void cpBodyDestroy(cpBody *_body){}
 
 void
@@ -366,22 +385,4 @@
 	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);
-}
+}
\ No newline at end of file

--- a/trunk/chipmunkd/cpCollision.d	Thu Dec 09 22:25:04 2010 +0100
+++ b/trunk/chipmunkd/cpCollision.d	Fri Dec 10 02:10:27 2010 +0100
@@ -99,12 +99,14 @@
 static cpContact *
 nextContactPoint(cpContact *arr, int *numPtr)
 {
-	int num = *numPtr;
+	int index = *numPtr;
 	
-	if(num < CP_MAX_CONTACTS_PER_ARBITER)
-		(*numPtr) = num + 1;
-	
-	return &arr[num];
+	if(index < CP_MAX_CONTACTS_PER_ARBITER){
+		(*numPtr) = index + 1;
+		return &arr[index];
+	} else {
+		return &arr[CP_MAX_CONTACTS_PER_ARBITER - 1];
+	}
 }
 
 // Find the minimum separating axis for the give poly and axis list.

--- a/trunk/chipmunkd/cpPolyShape.d	Thu Dec 09 22:25:04 2010 +0100
+++ b/trunk/chipmunkd/cpPolyShape.d	Fri Dec 10 02:10:27 2010 +0100
@@ -204,7 +204,7 @@
 };
 
 cpBool
-cpPolyValidate(cpVect *verts, int numVerts)
+cpPolyValidate(in cpVect *verts, in int numVerts)
 {
 	for(int i=0; i<numVerts; i++){
 		cpVect a = verts[i];

--- a/trunk/chipmunkd/cpShape.d	Thu Dec 09 22:25:04 2010 +0100
+++ b/trunk/chipmunkd/cpShape.d	Fri Dec 10 02:10:27 2010 +0100
@@ -218,6 +218,7 @@
 	}
 }
 
+// TODO this function should really take a position and rotation explicitly and be renamed
 cpBB
 cpShapeCacheBB(cpShape *shape)
 {

--- a/trunk/chipmunkd/cpSpace.d	Thu Dec 09 22:25:04 2010 +0100
+++ b/trunk/chipmunkd/cpSpace.d	Fri Dec 10 02:10:27 2010 +0100
@@ -26,7 +26,6 @@
 	void *data;
 }
 
-enum CP_MAX_CONTACTS_PER_ARBITER = 6;
 struct cpContactBufferHeader {
 	cpTimestamp stamp;
 	cpContactBufferHeader *next;
@@ -80,7 +79,8 @@
 	// 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;
+	cpContactBufferHeader* contactBuffersHead;
+	deprecated cpContactBufferHeader* _contactBuffersTail_Deprecated;
 	
 	// List of buffers to be free()ed when destroying the space.
 	cpArray *allocatedBuffers;
@@ -162,8 +162,14 @@
 //// 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);
-//
-//
+
+// Shape query callback function
+alias void function(cpShape *shape, cpContactPointSet *points, void *data)cpSpaceShapeQueryFunc;
+//cpBool cpSpaceShapeQuery(cpSpace *space, cpShape *shape, cpSpaceShapeQueryFunc func, void *data);
+
+
+//void cpSpaceActivateShapesTouchingShape(cpSpace *space, cpShape *shape);
+
 //// 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);
@@ -188,7 +194,7 @@
 	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));
+	return ((a == arb.a && b == arb.b) || (b == arb.a && a == arb.b));
 }
 
 // Transformation function for contactSet.
@@ -457,20 +463,6 @@
 	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)
 {
@@ -481,7 +473,7 @@
 	if(!shape._body) shape._body = &space.staticBody;
 	
 	cpShapeCacheBB(shape);
-	activateShapesTouchingShape(space, shape);
+	cpSpaceActivateShapesTouchingShape(space, shape);
 	cpSpaceHashInsert(space.staticShapes, shape, shape.hashid, shape.bb);
 	
 	return shape;
@@ -490,7 +482,7 @@
 cpBody *
 cpSpaceAddBody(cpSpace *space, cpBody *_body)
 {
-	mixin(cpAssertWarn!("_body.m != INFINITY", "Did you really mean to add an infinite mass body to the space?",__FILE__,__LINE__));
+	mixin(cpAssertWarn!("!cpBodyIsStatic(_body)", "Static bodies cannot be added to a space as they are not meant to be simulated.",__FILE__,__LINE__));
 	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
 	
@@ -525,8 +517,11 @@
 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);
+	if(context.shape == arb.a || context.shape == arb.b){
+		if(arb.state != cpArbiterState.cpArbiterStateCached){
+			arb.handler.separate(arb, context.space, arb.handler.data);
+		}
+		
 		cpArrayPush(context.space.pooledArbiters, arb);
 		return cpFalse;
 	}
@@ -567,7 +562,7 @@
 	cpHashSetFilter(space.contactSet, cast(cpHashSetFilterFunc)&contactSetFilterRemovedShape, &context);
 	cpSpaceHashRemove(space.staticShapes, shape, shape.hashid);
 	
-	activateShapesTouchingShape(space, shape);
+	cpSpaceActivateShapesTouchingShape(space, shape);
 }
 
 void

--- a/trunk/chipmunkd/cpSpaceComponent.d	Thu Dec 09 22:25:04 2010 +0100
+++ b/trunk/chipmunkd/cpSpaceComponent.d	Fri Dec 10 02:10:27 2010 +0100
@@ -42,15 +42,17 @@
 	
 	cpSpace *space = root.space;
 	assert(space, "Trying to activate a body that was never added to a space.");
+	assert(!space.locked, "Bodies can not be awakened during a query or a call to cpSpaceSte(). Put these calls into a post-step callback.");
 	
-	cpBody* _body = root;
-	cpBody* next;
+	cpBody *_body = root;
+	cpBody *next;
 	do {
 		next = _body.node.next;
 		
-		cpComponentNode node = {null, null, 0, 0.0f};
+		cpFloat idleTime = (cpBodyIsStatic(_body) ? cast(cpFloat)INFINITY : 0.0f);
+		cpComponentNode node = {null, null, 0, idleTime};
 		_body.node = node;
-		cpArrayPush(space.bodies, _body);
+		if(!cpBodyIsRogue(_body)) cpArrayPush(space.bodies, _body);
 		
 		for(cpShape *shape=_body.shapesList; shape; shape=shape.next){
 			cpSpaceHashRemove(space.staticShapes, shape, shape.hashid);
@@ -66,7 +68,7 @@
 {
 	// Reset the idle time even if it's not in a currently sleeping component
 	// Like a body resting on or jointed to a rogue body.
-	_body.node.idleTime = 0.0f;
+	if(!cpBodyIsStatic(_body)) _body.node.idleTime = 0.0f;
 	componentActivate(componentNodeRoot(_body));
 }
 
@@ -90,8 +92,8 @@
 	} 
 	
 	// Add any rogue bodies (bodies not added to the space)
-	if(!a.space) cpArrayPush(rogueBodies, a);
-	if(!b.space) cpArrayPush(rogueBodies, b);
+	if(cpBodyIsRogue(a)) cpArrayPush(rogueBodies, a);
+	if(cpBodyIsRogue(b)) cpArrayPush(rogueBodies, b);
 	
 	componentNodeMerge(a_root, b_root);
 }
@@ -103,7 +105,7 @@
 	cpBody *next;
 	do {
 		next = _body.node.next;
-		if(cpBodyIsRogue(_body) || _body.node.idleTime < threshold) return cpTrue;
+		if(_body.node.idleTime < threshold) return cpTrue;
 	} while((_body = next) != root);
 	
 	return cpFalse;
@@ -155,7 +157,7 @@
 	// iterate graph edges and build forests
 	for(int i=0; i<arbiters.num; i++){
 		cpArbiter *arb = cast(cpArbiter*)arbiters.arr[i];
-		mergeBodies(space, components, rogueBodies, arb.private_a._body, arb.private_b._body);
+		mergeBodies(space, components, rogueBodies, arb.a._body, arb.b._body);
 	}
 	for(int j=0; j<constraints.num; j++){
 		cpConstraint *constraint = cast(cpConstraint *)constraints.arr[j];
@@ -205,9 +207,21 @@
 }
 
 void
-cpSpaceSleepBody(cpSpace *space, cpBody *_body){
-	cpComponentNode node = {null, _body, 0, 0.0f};
-	_body.node = node;
+cpBodySleep(cpBody *_body)
+{
+	cpBodySleepWithGroup(_body, null);
+}
+
+void
+cpBodySleepWithGroup(cpBody *_body, cpBody *group){
+	assert(!cpBodyIsStatic(_body) && !cpBodyIsRogue(_body), "Rogue and static bodies cannot be put to sleep.");
+	
+	cpSpace *space = _body.space;
+	assert(space, "Cannot put a body to sleep that has not been added to a space.");
+	assert(!space.locked, "Bodies can not be put to sleep during a query or a call to cpSpaceSte(). Put these calls into a post-step callback.");
+	assert(!group || cpBodyIsSleeping(group), "Cannot use a non-sleeping body as a group identifier.");
+	
+	if(cpBodyIsSleeping(_body)) return;
 	
 	for(cpShape *shape = _body.shapesList; shape; shape = shape.next){
 		cpSpaceHashRemove(space.activeShapes, shape, shape.hashid);
@@ -216,6 +230,37 @@
 		cpSpaceHashInsert(space.staticShapes, shape, shape.hashid, shape.bb);
 	}
 	
-	cpArrayPush(space.sleepingComponents, _body);
+	if(group){
+		cpBody *root = componentNodeRoot(group);
+		
+		cpComponentNode node = {root, root.node.next, 0, 0.0f};
+		_body.node = node;
+		root.node.next = _body;
+	} else {
+		cpComponentNode node = {null, _body, 0, 0.0f};
+		_body.node = node;
+		
+		cpArrayPush(space.sleepingComponents, _body);
+	}
+	
 	cpArrayDeleteObj(space.bodies, _body);
 }
+
+static void
+activateTouchingHelper(cpShape *shape, cpContactPointSet *points, cpArray **bodies){
+	if(*bodies == null) (*bodies) = cpArrayNew(0);
+	cpArrayPush(*bodies, shape._body);
+}
+
+void
+cpSpaceActivateShapesTouchingShape(cpSpace *space, cpShape *shape){
+	cpArray *bodies = null;
+	cpSpaceShapeQuery(space, shape, cast(cpSpaceShapeQueryFunc)&activateTouchingHelper, &bodies);
+	
+	if(bodies){
+		for(int i=0; i<bodies.num; i++){
+			cpBody *_body = cast(cpBody *)bodies.arr[i];
+			cpBodyActivate(_body);
+		}
+	}
+}

--- a/trunk/chipmunkd/cpSpaceQuery.d	Thu Dec 09 22:25:04 2010 +0100
+++ b/trunk/chipmunkd/cpSpaceQuery.d	Fri Dec 10 02:10:27 2010 +0100
@@ -28,8 +28,11 @@
 cpSpacePointQuery(cpSpace *space, cpVect point, cpLayers layers, cpGroup group, cpSpacePointQueryFunc func, void *data)
 {
 	pointQueryContext context = {layers, group, func, data};
-	cpSpaceHashPointQuery(space.activeShapes, point, cast(cpSpaceHashQueryFunc)&pointQueryHelper, &context);
-	cpSpaceHashPointQuery(space.staticShapes, point, cast(cpSpaceHashQueryFunc)&pointQueryHelper, &context);
+	
+	cpBool locked = space.locked; space.locked = cpTrue; {
+		cpSpaceHashPointQuery(space.activeShapes, point, cast(cpSpaceHashQueryFunc)&pointQueryHelper, &context);
+		cpSpaceHashPointQuery(space.staticShapes, point, cast(cpSpaceHashQueryFunc)&pointQueryHelper, &context);
+	} space.locked = locked;
 }
 
 static void
@@ -89,8 +92,10 @@
 		func,
 	};
 	
-	cpSpaceHashSegmentQuery(space.staticShapes, &context, start, end, 1.0f, cast(cpSpaceHashSegmentQueryFunc)&segQueryFunc, data);
-	cpSpaceHashSegmentQuery(space.activeShapes, &context, start, end, 1.0f, cast(cpSpaceHashSegmentQueryFunc)&segQueryFunc, data);
+	cpBool locked = space.locked; space.locked = cpTrue; {
+		cpSpaceHashSegmentQuery(space.staticShapes, &context, start, end, 1.0f, cast(cpSpaceHashSegmentQueryFunc)&segQueryFunc, data);
+		cpSpaceHashSegmentQuery(space.activeShapes, &context, start, end, 1.0f, cast(cpSpaceHashSegmentQueryFunc)&segQueryFunc, data);
+	} space.locked = locked;
 }
 
 struct segQueryFirstContext {
@@ -162,6 +167,69 @@
 cpSpaceBBQuery(cpSpace *space, cpBB bb, cpLayers layers, cpGroup group, cpSpaceBBQueryFunc func, void *data)
 {
 	bbQueryContext context = {layers, group, func, data};
+	
+	cpBool locked = space.locked; space.locked = cpTrue; {
 	cpSpaceHashQuery(space.activeShapes, &bb, bb, cast(cpSpaceHashQueryFunc)&bbQueryHelper, &context);
 	cpSpaceHashQuery(space.staticShapes, &bb, bb, cast(cpSpaceHashQueryFunc)&bbQueryHelper, &context);
+	} space.locked = locked;
 }
+
+//#pragma mark Shape Query Functions
+
+struct shapeQueryContext {
+	cpSpaceShapeQueryFunc func;
+	void *data;
+	cpBool anyCollision;
+}
+
+// Callback from the spatial hash.
+static void
+shapeQueryHelper(cpShape *a, cpShape *b, shapeQueryContext *context)
+{
+	// Reject any of the simple cases
+	if(
+		(a.group && a.group == b.group) ||
+		!(a.layers & b.layers) ||
+		a.sensor || b.sensor
+	) return;
+	
+	cpContact contacts[CP_MAX_CONTACTS_PER_ARBITER];
+	int numContacts = 0;
+	
+	// Shape 'a' should have the lower shape type. (required by cpCollideShapes() )
+	if(a.klass.type <= b.klass.type){
+		numContacts = cpCollideShapes(a, b, contacts.ptr);
+	} else {
+		numContacts = cpCollideShapes(b, a, contacts.ptr);
+		for(int i=0; i<numContacts; i++) contacts[i].n = cpvneg(contacts[i].n);
+	}
+	
+	if(numContacts){
+		context.anyCollision = cpTrue;
+		
+		if(context.func){
+			cpContactPointSet set; set.count = numContacts;
+			for(int i=0; i<set.count; i++){
+				set.points[i].point = contacts[i].p;
+				set.points[i].normal = contacts[i].p;
+				set.points[i].dist = contacts[i].dist;
+			}
+			
+			context.func(b, &set, context.data);
+		}
+	}
+}
+
+cpBool
+cpSpaceShapeQuery(cpSpace *space, cpShape *shape, cpSpaceShapeQueryFunc func, void *data)
+{
+	cpBB bb = cpShapeCacheBB(shape);
+	shapeQueryContext context = {func, data, cpFalse};
+	
+	cpBool locked = space.locked; space.locked = cpTrue; {
+		cpSpaceHashQuery(space.activeShapes, shape, bb, cast(cpSpaceHashQueryFunc)&shapeQueryHelper, &context);
+		cpSpaceHashQuery(space.staticShapes, shape, bb, cast(cpSpaceHashQueryFunc)&shapeQueryHelper, &context);
+	} space.locked = locked;
+	
+	return context.anyCollision;
+}

--- a/trunk/chipmunkd/cpSpaceStep.d	Thu Dec 09 22:25:04 2010 +0100
+++ b/trunk/chipmunkd/cpSpaceStep.d	Fri Dec 10 02:10:27 2010 +0100
@@ -214,8 +214,8 @@
 contactSetFilter(cpArbiter *arb, cpSpace *space)
 {
 	if(space.sleepTimeThreshold != INFINITY){
-		cpBody *a = arb.private_a._body;
-		cpBody *b = arb.private_b._body;
+		cpBody *a = arb.a._body;
+		cpBody *b = arb.b._body;
 		
 		// both bodies are either static or sleeping
 		cpBool sleepingNow =
@@ -275,8 +275,6 @@
 	cpArray *bodies = space.bodies;
 	cpArray *constraints = space.constraints;
 	
-	space.locked = cpTrue;
-	
 	// Empty the arbiter list.
 	space.arbiters.num = 0;
 
@@ -289,12 +287,16 @@
 	// Pre-cache BBoxes and shape data.
 	cpSpaceHashEach(space.activeShapes, cast(cpSpaceHashIterator)&updateBBCache, null);
 	
+	space.locked = cpTrue;
+	
 	// Collide!
 	cpSpacePushFreshContactBuffer(space);
 	if(space.staticShapes.handleSet.entries)
 		cpSpaceHashEach(space.activeShapes, cast(cpSpaceHashIterator)&active2staticIter, space);
 	cpSpaceHashQueryRehash(space.activeShapes, cast(cpSpaceHashQueryFunc)&queryFunc, space);
 	
+	space.locked = cpFalse;
+	
 	// If body sleeping is enabled, do that now.
 	if(space.sleepTimeThreshold != INFINITY){
 		cpSpaceProcessComponents(space, dt);
@@ -349,7 +351,7 @@
 		}
 	}
 	
-	space.locked = cpFalse;
+	space.locked = cpTrue;
 	
 	// run the post solve callbacks
 	for(int i=0; i<arbiters.num; i++){
@@ -361,6 +363,8 @@
 		arb.state = cpArbiterState.cpArbiterStateNormal;
 	}
 	
+	space.locked = cpFalse;
+	
 	// Run the post step callbacks
 	// Loop because post step callbacks may create more post step callbacks
 	while(space.postStepCallbacks){