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