chipmunk 5.3.4.5 → 6.1.3.0.rc1

data/ext/chipmunk/cpSpace.c

@@ -19,20 +19,16 @@
  * SOFTWARE.
  */
  
-#include <stdlib.h>
-//#include <stdio.h>
+#include <stdio.h>
 #include <string.h>
-#include <math.h>
 
 #include "chipmunk_private.h"
 
-cpTimestamp cp_contact_persistence = 3;
+//MARK: Contact Set Helpers
 
-#pragma mark Contact Set Helpers
-
-// Equal function for contactSet.
+// Equal function for arbiterSet.
 static cpBool
-contactSetEql(cpShape **shapes, cpArbiter *arb)
+arbiterSetEql(cpShape **shapes, cpArbiter *arb)
 {
 	cpShape *a = shapes[0];
 	cpShape *b = shapes[1];
@@ -40,59 +36,37 @@ contactSetEql(cpShape **shapes, cpArbiter *arb)
 	return ((a == arb->a && b == arb->b) || (b == arb->a && a == arb->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/sizeof(cpArbiter);
-		cpAssert(count, "Buffer size too small.");
-		
-		cpArbiter *buffer = (cpArbiter *)cpmalloc(CP_BUFFER_BYTES);
-		cpArrayPush(space->allocatedBuffers, buffer);
-		
-		for(int i=0; i<count; i++) cpArrayPush(space->pooledArbiters, buffer + i);
-	}
-	
-	return cpArbiterInit((cpArbiter *) cpArrayPop(space->pooledArbiters), shapes[0], shapes[1]);
-}
-
-#pragma mark Collision Pair Function Helpers
+//MARK: Collision Handler Set HelperFunctions
 
-// Equals function for collFuncSet.
+// Equals function for collisionHandlers.
 static cpBool
-collFuncSetEql(cpCollisionHandler *check, cpCollisionHandler *pair)
+handlerSetEql(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.
+// Transformation function for collisionHandlers.
 static void *
-collFuncSetTrans(cpCollisionHandler *handler, void *unused)
+handlerSetTrans(cpCollisionHandler *handler, void *unused)
 {
-	cpCollisionHandler *copy = (cpCollisionHandler *)cpmalloc(sizeof(cpCollisionHandler));
+	cpCollisionHandler *copy = (cpCollisionHandler *)cpcalloc(1, sizeof(cpCollisionHandler));
 	(*copy) = (*handler);
 	
 	return copy;
 }
 
-#pragma mark Misc Helper Funcs
+//MARK: Misc Helper Funcs
 
 // 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;}
+// function to get the estimated velocity of a shape for the cpBBTree.
+static cpVect shapeVelocityFunc(cpShape *shape){return shape->body->v;}
 
-// 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);}
+static void freeWrap(void *ptr, void *unused){cpfree(ptr);}
 
-#pragma mark Memory Management Functions
+//MARK: Memory Management Functions
 
 cpSpace *
 cpSpaceAlloc(void)
@@ -100,28 +74,35 @@ cpSpaceAlloc(void)
 	return (cpSpace *)cpcalloc(1, sizeof(cpSpace));
 }
 
-#define DEFAULT_DIM_SIZE 100.0f
-#define DEFAULT_COUNT 1000
-#define DEFAULT_ITERATIONS 10
-#define DEFAULT_ELASTIC_ITERATIONS 0
-
-cpCollisionHandler defaultHandler = {0, 0, alwaysCollide, alwaysCollide, nothing, nothing, NULL};
+cpCollisionHandler cpDefaultCollisionHandler = {0, 0, alwaysCollide, alwaysCollide, nothing, nothing, NULL};
 
 cpSpace*
 cpSpaceInit(cpSpace *space)
 {
-	space->iterations = DEFAULT_ITERATIONS;
-	space->elasticIterations = DEFAULT_ELASTIC_ITERATIONS;
-//	space->sleepTicks = 300;
+#ifndef NDEBUG
+	static cpBool done = cpFalse;
+	if(!done){
+		printf("Initializing cpSpace - Chipmunk v%s (Debug Enabled)\n", cpVersionString);
+		printf("Compile with -DNDEBUG defined to disable debug mode and runtime assertion checks\n");
+		done = cpTrue;
+	}
+#endif
+
+	space->iterations = 10;
 	
 	space->gravity = cpvzero;
 	space->damping = 1.0f;
 	
+	space->collisionSlop = 0.1f;
+	space->collisionBias = cpfpow(1.0f - 0.1f, 60.0f);
+	space->collisionPersistence = 3;
+	
 	space->locked = 0;
 	space->stamp = 0;
 
-	space->staticShapes = cpSpaceHashNew(DEFAULT_DIM_SIZE, DEFAULT_COUNT, (cpSpaceHashBBFunc)shapeBBFunc);
-	space->activeShapes = cpSpaceHashNew(DEFAULT_DIM_SIZE, DEFAULT_COUNT, (cpSpaceHashBBFunc)shapeBBFunc);
+	space->staticShapes = cpBBTreeNew((cpSpatialIndexBBFunc)cpShapeGetBB, NULL);
+	space->activeShapes = cpBBTreeNew((cpSpatialIndexBBFunc)cpShapeGetBB, space->staticShapes);
+	cpBBTreeSetVelocityFunc(space->activeShapes, (cpBBTreeVelocityFunc)shapeVelocityFunc);
 	
 	space->allocatedBuffers = cpArrayNew(0);
 	
@@ -131,22 +112,25 @@ cpSpaceInit(cpSpace *space)
 	
 	space->sleepTimeThreshold = INFINITY;
 	space->idleSpeedThreshold = 0.0f;
+	space->enableContactGraph = cpFalse;
 	
 	space->arbiters = cpArrayNew(0);
 	space->pooledArbiters = cpArrayNew(0);
 	
 	space->contactBuffersHead = NULL;
-	space->contactSet = cpHashSetNew(0, (cpHashSetEqlFunc)contactSetEql, (cpHashSetTransFunc)contactSetTrans);
+	space->cachedArbiters = cpHashSetNew(0, (cpHashSetEqlFunc)arbiterSetEql);
 	
 	space->constraints = cpArrayNew(0);
 	
-	space->defaultHandler = defaultHandler;
-	space->collFuncSet = cpHashSetNew(0, (cpHashSetEqlFunc)collFuncSetEql, (cpHashSetTransFunc)collFuncSetTrans);
-	space->collFuncSet->default_value = &space->defaultHandler;
+	space->defaultHandler = cpDefaultCollisionHandler;
+	space->collisionHandlers = cpHashSetNew(0, (cpHashSetEqlFunc)handlerSetEql);
+	cpHashSetSetDefaultValue(space->collisionHandlers, &cpDefaultCollisionHandler);
 	
-	space->postStepCallbacks = NULL;
+	space->postStepCallbacks = cpArrayNew(0);
+	space->skipPostStep = cpFalse;
 	
-	cpBodyInitStatic(&space->staticBody);
+	cpBodyInitStatic(&space->_staticBody);
+	space->staticBody = &space->_staticBody;
 	
 	return space;
 }
@@ -160,8 +144,10 @@ cpSpaceNew(void)
 void
 cpSpaceDestroy(cpSpace *space)
 {
-	cpSpaceHashFree(space->staticShapes);
-	cpSpaceHashFree(space->activeShapes);
+	cpSpaceEachBody(space, (cpSpaceBodyIteratorFunc)cpBodyActivate, NULL);
+	
+	cpSpatialIndexFree(space->staticShapes);
+	cpSpatialIndexFree(space->activeShapes);
 	
 	cpArrayFree(space->bodies);
 	cpArrayFree(space->sleepingComponents);
@@ -169,25 +155,23 @@ cpSpaceDestroy(cpSpace *space)
 	
 	cpArrayFree(space->constraints);
 	
-	cpHashSetFree(space->contactSet);
+	cpHashSetFree(space->cachedArbiters);
 	
 	cpArrayFree(space->arbiters);
 	cpArrayFree(space->pooledArbiters);
 	
 	if(space->allocatedBuffers){
-		cpArrayEach(space->allocatedBuffers, freeWrap, NULL);
+		cpArrayFreeEach(space->allocatedBuffers, cpfree);
 		cpArrayFree(space->allocatedBuffers);
 	}
 	
 	if(space->postStepCallbacks){
-		cpHashSetEach(space->postStepCallbacks, freeWrap, NULL);
-		cpHashSetFree(space->postStepCallbacks);
+		cpArrayFreeEach(space->postStepCallbacks, cpfree);
+		cpArrayFree(space->postStepCallbacks);
 	}
 	
-	if(space->collFuncSet){
-		cpHashSetEach(space->collFuncSet, freeWrap, NULL);
-		cpHashSetFree(space->collFuncSet);
-	}
+	if(space->collisionHandlers) cpHashSetEach(space->collisionHandlers, freeWrap, NULL);
+	cpHashSetFree(space->collisionHandlers);
 }
 
 void
@@ -199,19 +183,13 @@ cpSpaceFree(cpSpace *space)
 	}
 }
 
-void
-cpSpaceFreeChildren(cpSpace *space)
-{
-	cpArray *components = space->sleepingComponents;
-	while(components->num) cpBodyActivate((cpBody *)components->arr[0]);
-	
-	cpSpaceHashEach(space->staticShapes, (cpSpaceHashIterator)&shapeFreeWrap, NULL);
-	cpSpaceHashEach(space->activeShapes, (cpSpaceHashIterator)&shapeFreeWrap, NULL);
-	cpArrayEach(space->bodies,           (cpArrayIter)&bodyFreeWrap,          NULL);
-	cpArrayEach(space->constraints,      (cpArrayIter)&constraintFreeWrap,    NULL);
-}
+#define cpAssertSpaceUnlocked(space) \
+	cpAssertHard(!space->locked, \
+		"This addition/removal cannot be done safely during a call to cpSpaceStep() or during a query. " \
+		"Put these calls into a post-step callback." \
+	);
 
-#pragma mark Collision Handler Function Management
+//MARK: Collision Handler Function Management
 
 void
 cpSpaceAddCollisionHandler(
@@ -223,6 +201,8 @@ cpSpaceAddCollisionHandler(
 	cpCollisionSeparateFunc separate,
 	void *data
 ){
+	cpAssertSpaceUnlocked(space);
+	
 	// Remove any old function so the new one will get added.
 	cpSpaceRemoveCollisionHandler(space, a, b);
 	
@@ -235,14 +215,16 @@ cpSpaceAddCollisionHandler(
 		data
 	};
 	
-	cpHashSetInsert(space->collFuncSet, CP_HASH_PAIR(a, b), &handler, NULL);
+	cpHashSetInsert(space->collisionHandlers, CP_HASH_PAIR(a, b), &handler, NULL, (cpHashSetTransFunc)handlerSetTrans);
 }
 
 void
 cpSpaceRemoveCollisionHandler(cpSpace *space, cpCollisionType a, cpCollisionType b)
 {
-	struct{cpCollisionType a, b;} ids = {a, b};
-	cpCollisionHandler *old_handler = (cpCollisionHandler *) cpHashSetRemove(space->collFuncSet, CP_HASH_PAIR(a, b), &ids);
+	cpAssertSpaceUnlocked(space);
+	
+	struct { cpCollisionType a, b; } ids = {a, b};
+	cpCollisionHandler *old_handler = (cpCollisionHandler *) cpHashSetRemove(space->collisionHandlers, CP_HASH_PAIR(a, b), &ids);
 	cpfree(old_handler);
 }
 
@@ -255,6 +237,8 @@ cpSpaceSetDefaultCollisionHandler(
 	cpCollisionSeparateFunc separate,
 	void *data
 ){
+	cpAssertSpaceUnlocked(space);
+	
 	cpCollisionHandler handler = {
 		0, 0,
 		begin ? begin : alwaysCollide,
@@ -265,53 +249,25 @@ cpSpaceSetDefaultCollisionHandler(
 	};
 	
 	space->defaultHandler = handler;
+	cpHashSetSetDefaultValue(space->collisionHandlers, &space->defaultHandler);
 }
 
-#pragma mark Body, Shape, and Joint Management
-
-#define cpAssertSpaceUnlocked(space) \
-	cpAssert(!space->locked, \
-		"This addition/removal cannot be done safely during a call to cpSpaceStep() or during a query. " \
-		"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;
-	}
-	
-	cpAssert(node, "Attempted to remove a shape from a body it was never attached to.");
-	(*prev_ptr) = node->next;
-}
-
+//MARK: Body, Shape, and Joint Management
 cpShape *
 cpSpaceAddShape(cpSpace *space, cpShape *shape)
 {
 	cpBody *body = shape->body;
-	if(!body || cpBodyIsStatic(body)) return cpSpaceAddStaticShape(space, shape);
+	if(cpBodyIsStatic(body)) return cpSpaceAddStaticShape(space, shape);
 	
-	cpAssert(!cpHashSetFind(space->activeShapes->handleSet, shape->hashid, shape),
-		"Cannot add the same shape more than once.");
+	cpAssertHard(!shape->space, "This shape is already added to a space and cannot be added to another.");
 	cpAssertSpaceUnlocked(space);
 	
 	cpBodyActivate(body);
 	cpBodyAddShape(body, shape);
 	
-	cpShapeCacheBB(shape);
-	cpSpaceHashInsert(space->activeShapes, shape, shape->hashid, shape->bb);
+	cpShapeUpdate(shape, body->p, body->rot);
+	cpSpatialIndexInsert(space->activeShapes, shape, shape->hashid);
+	shape->space = space;
 		
 	return shape;
 }
@@ -319,15 +275,14 @@ cpSpaceAddShape(cpSpace *space, cpShape *shape)
 cpShape *
 cpSpaceAddStaticShape(cpSpace *space, cpShape *shape)
 {
-	cpAssert(!cpHashSetFind(space->staticShapes->handleSet, shape->hashid, shape),
-		"Cannot add the same static shape more than once.");
+	cpAssertHard(!shape->space, "This shape is already added to a space and cannot be added to another.");
 	cpAssertSpaceUnlocked(space);
 	
-	if(!shape->body) shape->body = &space->staticBody;
-	
-	cpShapeCacheBB(shape);
-	cpSpaceActivateShapesTouchingShape(space, shape);
-	cpSpaceHashInsert(space->staticShapes, shape, shape->hashid, shape->bb);
+	cpBody *body = shape->body;
+	cpBodyAddShape(body, shape);
+	cpShapeUpdate(shape, body->p, body->rot);
+	cpSpatialIndexInsert(space->staticShapes, shape, shape->hashid);
+	shape->space = space;
 	
 	return shape;
 }
@@ -335,9 +290,9 @@ cpSpaceAddStaticShape(cpSpace *space, cpShape *shape)
 cpBody *
 cpSpaceAddBody(cpSpace *space, cpBody *body)
 {
-	cpAssertWarn(!cpBodyIsStatic(body), "Static bodies cannot be added to a space as they are not meant to be simulated.");
-	cpAssert(!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
+	cpAssertHard(!cpBodyIsStatic(body), "Static bodies cannot be added to a space as they are not meant to be simulated.");
+	cpAssertHard(!body->space, "This body is already added to a space and cannot be added to another.");
+	cpAssertSpaceUnlocked(space);
 	
 	cpArrayPush(space->bodies, body);
 	body->space = space;
@@ -348,84 +303,102 @@ cpSpaceAddBody(cpSpace *space, cpBody *body)
 cpConstraint *
 cpSpaceAddConstraint(cpSpace *space, cpConstraint *constraint)
 {
-	cpAssert(!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;
+	cpAssertHard(!constraint->space, "This shape is already added to a space and cannot be added to another.");
+	cpAssertSpaceUnlocked(space);
 	
 	cpBodyActivate(constraint->a);
 	cpBodyActivate(constraint->b);
 	cpArrayPush(space->constraints, constraint);
 	
+	// Push onto the heads of the bodies' constraint lists
+	cpBody *a = constraint->a, *b = constraint->b;
+	constraint->next_a = a->constraintList; a->constraintList = constraint;
+	constraint->next_b = b->constraintList; b->constraintList = constraint;
+	constraint->space = space;
+	
 	return constraint;
 }
 
-typedef struct removalContext {
+struct arbiterFilterContext {
 	cpSpace *space;
+	cpBody *body;
 	cpShape *shape;
-} removalContext;
+};
 
-// Hashset filter func to throw away old arbiters.
 static cpBool
-contactSetFilterRemovedShape(cpArbiter *arb, removalContext *context)
+cachedArbitersFilter(cpArbiter *arb, struct arbiterFilterContext *context)
 {
-	if(context->shape == arb->a || context->shape == arb->b){
-		if(arb->state != cpArbiterStateCached){
-			arb->handler->separate(arb, context->space, arb->handler->data);
-		}
+	cpShape *shape = context->shape;
+	cpBody *body = context->body;
+	
+	
+	// Match on the filter shape, or if it's NULL the filter body
+	if(
+		(body == arb->body_a && (shape == arb->a || shape == NULL)) ||
+		(body == arb->body_b && (shape == arb->b || shape == NULL))
+	){
+		// Call separate when removing shapes.
+		if(shape && arb->state != cpArbiterStateCached) cpArbiterCallSeparate(arb, context->space);
 		
+		cpArbiterUnthread(arb);
+		cpArrayDeleteObj(context->space->arbiters, arb);
 		cpArrayPush(context->space->pooledArbiters, arb);
+		
 		return cpFalse;
 	}
 	
 	return cpTrue;
 }
 
+void
+cpSpaceFilterArbiters(cpSpace *space, cpBody *body, cpShape *filter)
+{
+	cpSpaceLock(space); {
+		struct arbiterFilterContext context = {space, body, filter};
+		cpHashSetFilter(space->cachedArbiters, (cpHashSetFilterFunc)cachedArbitersFilter, &context);
+	} cpSpaceUnlock(space, cpTrue);
+}
+
 void
 cpSpaceRemoveShape(cpSpace *space, cpShape *shape)
 {
 	cpBody *body = shape->body;
 	if(cpBodyIsStatic(body)){
 		cpSpaceRemoveStaticShape(space, shape);
-		return;
+	} else {
+		cpAssertHard(cpSpaceContainsShape(space, shape), "Cannot remove a shape that was not added to the space. (Removed twice maybe?)");
+		cpAssertSpaceUnlocked(space);
+		
+		cpBodyActivate(body);
+		cpBodyRemoveShape(body, shape);
+		cpSpaceFilterArbiters(space, body, shape);
+		cpSpatialIndexRemove(space->activeShapes, shape, shape->hashid);
+		shape->space = NULL;
 	}
-
-	cpBodyActivate(body);
-	
-	cpAssertSpaceUnlocked(space);
-	cpAssertWarn(cpHashSetFind(space->activeShapes->handleSet, shape->hashid, shape),
-		"Cannot remove a shape that was not added to the space. (Removed twice maybe?)");
-	
-	cpBodyRemoveShape(body, shape);
-	
-	removalContext context = {space, shape};
-	cpHashSetFilter(space->contactSet, (cpHashSetFilterFunc)contactSetFilterRemovedShape, &context);
-	cpSpaceHashRemove(space->activeShapes, shape, shape->hashid);
 }
 
 void
 cpSpaceRemoveStaticShape(cpSpace *space, cpShape *shape)
 {
-	cpAssertWarn(cpHashSetFind(space->staticShapes->handleSet, shape->hashid, shape),
-		"Cannot remove a static or sleeping shape that was not added to the space. (Removed twice maybe?)");
+	cpAssertHard(cpSpaceContainsShape(space, shape), "Cannot remove a static or sleeping shape that was not added to the space. (Removed twice maybe?)");
 	cpAssertSpaceUnlocked(space);
 	
-	removalContext context = {space, shape};
-	cpHashSetFilter(space->contactSet, (cpHashSetFilterFunc)contactSetFilterRemovedShape, &context);
-	cpSpaceHashRemove(space->staticShapes, shape, shape->hashid);
-	
-	cpSpaceActivateShapesTouchingShape(space, shape);
+	cpBody *body = shape->body;
+	if(cpBodyIsStatic(body)) cpBodyActivateStatic(body, shape);
+	cpBodyRemoveShape(body, shape);
+	cpSpaceFilterArbiters(space, body, shape);
+	cpSpatialIndexRemove(space->staticShapes, shape, shape->hashid);
+	shape->space = NULL;
 }
 
 void
 cpSpaceRemoveBody(cpSpace *space, cpBody *body)
 {
-	cpAssertWarn(body->space == space,
-		"Cannot remove a body that was not added to the space. (Removed twice maybe?)");
+	cpAssertHard(cpSpaceContainsBody(space, body), "Cannot remove a body that was not added to the space. (Removed twice maybe?)");
 	cpAssertSpaceUnlocked(space);
 	
 	cpBodyActivate(body);
+//	cpSpaceFilterArbiters(space, body, NULL);
 	cpArrayDeleteObj(space->bodies, body);
 	body->space = NULL;
 }
@@ -433,67 +406,149 @@ cpSpaceRemoveBody(cpSpace *space, cpBody *body)
 void
 cpSpaceRemoveConstraint(cpSpace *space, cpConstraint *constraint)
 {
-	cpAssertWarn(cpArrayContains(space->constraints, constraint),
-		"Cannot remove a constraint that was not added to the space. (Removed twice maybe?)");
-//	cpAssertSpaceUnlocked(space); Should be safe as long as its not from a constraint callback.
+	cpAssertHard(cpSpaceContainsConstraint(space, constraint), "Cannot remove a constraint that was not added to the space. (Removed twice maybe?)");
+	cpAssertSpaceUnlocked(space);
 	
 	cpBodyActivate(constraint->a);
 	cpBodyActivate(constraint->b);
 	cpArrayDeleteObj(space->constraints, constraint);
+	
+	cpBodyRemoveConstraint(constraint->a, constraint);
+	cpBodyRemoveConstraint(constraint->b, constraint);
+	constraint->space = NULL;
+}
+
+cpBool cpSpaceContainsShape(cpSpace *space, cpShape *shape)
+{
+	return (shape->space == space);
+}
+
+cpBool cpSpaceContainsBody(cpSpace *space, cpBody *body)
+{
+	return (body->space == space);
+}
+
+cpBool cpSpaceContainsConstraint(cpSpace *space, cpConstraint *constraint)
+{
+	return (constraint->space == space);
+}
+
+
+//MARK: Iteration
+
+void
+cpSpaceEachBody(cpSpace *space, cpSpaceBodyIteratorFunc func, void *data)
+{
+	cpSpaceLock(space); {
+		cpArray *bodies = space->bodies;
+		
+		for(int i=0; i<bodies->num; i++){
+			func((cpBody *)bodies->arr[i], data);
+		}
+		
+		cpArray *components = space->sleepingComponents;
+		for(int i=0; i<components->num; i++){
+			cpBody *root = (cpBody *)components->arr[i];
+			
+			cpBody *body = root;
+			while(body){
+				cpBody *next = body->node.next;
+				func(body, data);
+				body = next;
+			}
+		}
+	} cpSpaceUnlock(space, cpTrue);
 }
 
-#pragma mark Spatial Hash Management
+typedef struct spaceShapeContext {
+	cpSpaceShapeIteratorFunc func;
+	void *data;
+} spaceShapeContext;
 
-static void updateBBCache(cpShape *shape, void *unused){cpShapeCacheBB(shape);}
+static void
+spaceEachShapeIterator(cpShape *shape, spaceShapeContext *context)
+{
+	context->func(shape, context->data);
+}
 
 void
-cpSpaceResizeStaticHash(cpSpace *space, cpFloat dim, int count)
+cpSpaceEachShape(cpSpace *space, cpSpaceShapeIteratorFunc func, void *data)
 {
-	cpSpaceHashResize(space->staticShapes, dim, count);
-	cpSpaceHashRehash(space->staticShapes);
+	cpSpaceLock(space); {
+		spaceShapeContext context = {func, data};
+		cpSpatialIndexEach(space->activeShapes, (cpSpatialIndexIteratorFunc)spaceEachShapeIterator, &context);
+		cpSpatialIndexEach(space->staticShapes, (cpSpatialIndexIteratorFunc)spaceEachShapeIterator, &context);
+	} cpSpaceUnlock(space, cpTrue);
 }
 
 void
-cpSpaceResizeActiveHash(cpSpace *space, cpFloat dim, int count)
+cpSpaceEachConstraint(cpSpace *space, cpSpaceConstraintIteratorFunc func, void *data)
 {
-	cpSpaceHashResize(space->activeShapes, dim, count);
+	cpSpaceLock(space); {
+		cpArray *constraints = space->constraints;
+		
+		for(int i=0; i<constraints->num; i++){
+			func((cpConstraint *)constraints->arr[i], data);
+		}
+	} cpSpaceUnlock(space, cpTrue);
+}
+
+//MARK: Spatial Index Management
+
+static void
+updateBBCache(cpShape *shape, void *unused)
+{
+	cpBody *body = shape->body;
+	cpShapeUpdate(shape, body->p, body->rot);
 }
 
 void 
-cpSpaceRehashStatic(cpSpace *space)
+cpSpaceReindexStatic(cpSpace *space)
 {
-	cpSpaceHashEach(space->staticShapes, (cpSpaceHashIterator)&updateBBCache, NULL);
-	cpSpaceHashRehash(space->staticShapes);
+	cpAssertHard(!space->locked, "You cannot manually reindex objects while the space is locked. Wait until the current query or step is complete.");
+	
+	cpSpatialIndexEach(space->staticShapes, (cpSpatialIndexIteratorFunc)&updateBBCache, NULL);
+	cpSpatialIndexReindex(space->staticShapes);
 }
 
 void
-cpSpaceRehashShape(cpSpace *space, cpShape *shape)
+cpSpaceReindexShape(cpSpace *space, cpShape *shape)
 {
-	cpShapeCacheBB(shape);
+	cpAssertHard(!space->locked, "You cannot manually reindex objects while the space is locked. Wait until the current query or step is complete.");
+	
+	cpBody *body = shape->body;
+	cpShapeUpdate(shape, body->p, body->rot);
 	
 	// attempt to rehash the shape in both hashes
-	cpSpaceHashRehashObject(space->activeShapes, shape, shape->hashid);
-	cpSpaceHashRehashObject(space->staticShapes, shape, shape->hashid);
+	cpSpatialIndexReindexObject(space->activeShapes, shape, shape->hashid);
+	cpSpatialIndexReindexObject(space->staticShapes, shape, shape->hashid);
 }
 
 void
-cpSpaceEachBody(cpSpace *space, cpSpaceBodyIterator func, void *data)
+cpSpaceReindexShapesForBody(cpSpace *space, cpBody *body)
 {
-	cpArray *bodies = space->bodies;
-	
-	for(int i=0; i<bodies->num; i++){
-		func((cpBody *)bodies->arr[i], data);
-	}
-	
-	cpArray *components = space->sleepingComponents;
-	for(int i=0; i<components->num; i++){
-		cpBody *root = (cpBody *)components->arr[i];
-		cpBody *body = root, *next;
-		do {
-			next = body->node.next;
-			func(body, data);
-		} while((body = next) != root);
-	}
+	CP_BODY_FOREACH_SHAPE(body, shape) cpSpaceReindexShape(space, shape);
 }
 
 
+static void
+copyShapes(cpShape *shape, cpSpatialIndex *index)
+{
+	cpSpatialIndexInsert(index, shape, shape->hashid);
+}
+
+void
+cpSpaceUseSpatialHash(cpSpace *space, cpFloat dim, int count)
+{
+	cpSpatialIndex *staticShapes = cpSpaceHashNew(dim, count, (cpSpatialIndexBBFunc)cpShapeGetBB, NULL);
+	cpSpatialIndex *activeShapes = cpSpaceHashNew(dim, count, (cpSpatialIndexBBFunc)cpShapeGetBB, staticShapes);
+	
+	cpSpatialIndexEach(space->staticShapes, (cpSpatialIndexIteratorFunc)copyShapes, staticShapes);
+	cpSpatialIndexEach(space->activeShapes, (cpSpatialIndexIteratorFunc)copyShapes, activeShapes);
+	
+	cpSpatialIndexFree(space->staticShapes);
+	cpSpatialIndexFree(space->activeShapes);
+	
+	space->staticShapes = staticShapes;
+	space->activeShapes = activeShapes;
+}