chipmunk 5.3.4.0-x86-mingw32 → 5.3.4.2-x86-mingw32

data/ext/chipmunk/vendor/chipmunk-5.3.4/src/cpShape.c

@@ -0,0 +1,405 @@
+/* Copyright (c) 2007 Scott Lembcke
+ * 
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ * 
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ * 
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+ 
+#include <stdlib.h>
+#include <stdio.h>
+#include <math.h>
+
+#include "chipmunk_private.h"
+#include "chipmunk_unsafe.h"
+
+#define CP_DefineShapeGetter(struct, type, member, name) \
+CP_DeclareShapeGetter(struct, type, name){ \
+	cpAssert(shape->klass == &struct##Class, "shape is not a "#struct); \
+	return ((struct *)shape)->member; \
+}
+cpHashValue SHAPE_ID_COUNTER = 0;
+
+void
+cpResetShapeIdCounter(void)
+{
+	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 && shape->klass->destroy) { 
+    shape->klass->destroy(shape);
+  } 
+}
+
+void
+cpShapeFree(cpShape *shape)
+{
+	if(shape){
+		cpShapeDestroy(shape);
+		cpfree(shape);
+	}
+}
+
+// TODO this function should really take a position and rotation explicitly and be renamed
+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 != NULL);
+}
+
+void
+cpSegmentQueryInfoPrint(cpSegmentQueryInfo *info)
+{
+	printf("Segment Query:\n");
+	printf("\tt: %f\n", info->t);
+//	printf("\tdist: %f\n", info->dist);
+//	printf("\tpoint: %s\n", cpvstr(info->point));
+	printf("\tn: %s\n", cpvstr(info->n));
+}
+
+
+
+
+cpCircleShape *
+cpCircleShapeAlloc(void)
+{
+	return (cpCircleShape *)cpcalloc(1, sizeof(cpCircleShape));
+}
+
+static inline 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 = (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 = (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 = (cpCircleShape *)shape;
+	circleSegmentQuery(shape, circle->tc, circle->r, a, b, info);
+}
+
+static const cpShapeClass cpCircleShapeClass = {
+	CP_CIRCLE_SHAPE,
+	cpCircleShapeCacheData,
+	NULL,
+	cpCircleShapePointQuery,
+	cpCircleShapeSegmentQuery,
+};
+
+cpCircleShape *
+cpCircleShapeInit(cpCircleShape *circle, cpBody *body, cpFloat radius, cpVect offset)
+{
+	circle->c = offset;
+	circle->r = radius;
+	
+	cpShapeInit((cpShape *)circle, &cpCircleShapeClass, body);
+	
+	return circle;
+}
+
+cpShape *
+cpCircleShapeNew(cpBody *body, cpFloat radius, cpVect offset)
+{
+	return (cpShape *)cpCircleShapeInit(cpCircleShapeAlloc(), body, radius, offset);
+}
+
+CP_DefineShapeGetter(cpCircleShape, cpVect, c, Offset)
+CP_DefineShapeGetter(cpCircleShape, cpFloat, r, Radius)
+
+cpSegmentShape *
+cpSegmentShapeAlloc(void)
+{
+	return (cpSegmentShape *)cpcalloc(1, sizeof(cpSegmentShape));
+}
+
+static cpBB
+cpSegmentShapeCacheData(cpShape *shape, cpVect p, cpVect rot)
+{
+	cpSegmentShape *seg = (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 = (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 inline 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 = (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 = {
+	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((cpShape *)seg, &cpSegmentShapeClass, body);
+	
+	return seg;
+}
+
+cpShape*
+cpSegmentShapeNew(cpBody *body, cpVect a, cpVect b, cpFloat r)
+{
+	return (cpShape *)cpSegmentShapeInit(cpSegmentShapeAlloc(), body, a, b, r);
+}
+
+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)
+{
+	cpAssert(shape->klass == &cpCircleShapeClass, "Shape is not a circle shape.");
+	cpCircleShape *circle = (cpCircleShape *)shape;
+	
+	circle->r = radius;
+}
+
+void
+cpCircleShapeSetOffset(cpShape *shape, cpVect offset)
+{
+	cpAssert(shape->klass == &cpCircleShapeClass, "Shape is not a circle shape.");
+	cpCircleShape *circle = (cpCircleShape *)shape;
+	
+	circle->c = offset;
+}
+
+void
+cpSegmentShapeSetEndpoints(cpShape *shape, cpVect a, cpVect b)
+{
+	cpAssert(shape->klass == &cpSegmentShapeClass, "Shape is not a segment shape.");
+	cpSegmentShape *seg = (cpSegmentShape *)shape;
+	
+	seg->a = a;
+	seg->b = b;
+	seg->n = cpvperp(cpvnormalize(cpvsub(b, a)));
+}
+
+void
+cpSegmentShapeSetRadius(cpShape *shape, cpFloat radius)
+{
+	cpAssert(shape->klass == &cpSegmentShapeClass, "Shape is not a segment shape.");
+	cpSegmentShape *seg = (cpSegmentShape *)shape;
+	
+	seg->r = radius;
+}

data/ext/chipmunk/vendor/chipmunk-5.3.4/src/cpSpace.c

@@ -0,0 +1,499 @@
+/* Copyright (c) 2007 Scott Lembcke
+ * 
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ * 
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ * 
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+ 
+#include <stdlib.h>
+//#include <stdio.h>
+#include <string.h>
+#include <math.h>
+
+#include "chipmunk_private.h"
+
+cpTimestamp cp_contact_persistence = 3;
+
+#pragma mark Contact Set Helpers
+
+// Equal function for contactSet.
+static cpBool
+contactSetEql(cpShape **shapes, cpArbiter *arb)
+{
+	cpShape *a = shapes[0];
+	cpShape *b = shapes[1];
+	
+	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
+
+// 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 = (cpCollisionHandler *)cpmalloc(sizeof(cpCollisionHandler));
+	(*copy) = (*handler);
+	
+	return copy;
+}
+
+#pragma 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;}
+
+// 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);}
+
+#pragma mark Memory Management Functions
+
+cpSpace *
+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};
+
+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, (cpSpaceHashBBFunc)shapeBBFunc);
+	space->activeShapes = cpSpaceHashNew(DEFAULT_DIM_SIZE, DEFAULT_COUNT, (cpSpaceHashBBFunc)shapeBBFunc);
+	
+	space->allocatedBuffers = cpArrayNew(0);
+	
+	space->bodies = cpArrayNew(0);
+	space->sleepingComponents = cpArrayNew(0);
+	space->rousedBodies = cpArrayNew(0);
+	
+	space->sleepTimeThreshold = INFINITY;
+	space->idleSpeedThreshold = 0.0f;
+	
+	space->arbiters = cpArrayNew(0);
+	space->pooledArbiters = cpArrayNew(0);
+	
+	space->contactBuffersHead = NULL;
+	space->contactSet = cpHashSetNew(0, (cpHashSetEqlFunc)contactSetEql, (cpHashSetTransFunc)contactSetTrans);
+	
+	space->constraints = cpArrayNew(0);
+	
+	space->defaultHandler = defaultHandler;
+	space->collFuncSet = cpHashSetNew(0, (cpHashSetEqlFunc)collFuncSetEql, (cpHashSetTransFunc)collFuncSetTrans);
+	space->collFuncSet->default_value = &space->defaultHandler;
+	
+	space->postStepCallbacks = NULL;
+	
+	cpBodyInitStatic(&space->staticBody);
+	
+	return space;
+}
+
+cpSpace*
+cpSpaceNew(void)
+{
+	return cpSpaceInit(cpSpaceAlloc());
+}
+
+void
+cpSpaceDestroy(cpSpace *space)
+{
+	cpSpaceHashFree(space->staticShapes);
+	cpSpaceHashFree(space->activeShapes);
+	
+	cpArrayFree(space->bodies);
+	cpArrayFree(space->sleepingComponents);
+	cpArrayFree(space->rousedBodies);
+	
+	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((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);
+}
+
+#pragma mark Collision Handler Function Management
+
+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{cpCollisionType a, b;} ids = {a, b};
+	cpCollisionHandler *old_handler = (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
+
+#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;
+}
+
+cpShape *
+cpSpaceAddShape(cpSpace *space, cpShape *shape)
+{
+	cpBody *body = shape->body;
+	if(!body || cpBodyIsStatic(body)) return cpSpaceAddStaticShape(space, shape);
+	
+	cpAssert(!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;
+}
+
+cpShape *
+cpSpaceAddStaticShape(cpSpace *space, cpShape *shape)
+{
+	cpAssert(!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);
+	cpSpaceActivateShapesTouchingShape(space, shape);
+	cpSpaceHashInsert(space->staticShapes, shape, shape->hashid, shape->bb);
+	
+	return 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
+	
+	cpArrayPush(space->bodies, body);
+	body->space = space;
+	
+	return 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;
+	
+	cpBodyActivate(constraint->a);
+	cpBodyActivate(constraint->b);
+	cpArrayPush(space->constraints, constraint);
+	
+	return constraint;
+}
+
+typedef struct removalContext {
+	cpSpace *space;
+	cpShape *shape;
+} removalContext;
+
+// Hashset filter func to throw away old arbiters.
+static cpBool
+contactSetFilterRemovedShape(cpArbiter *arb, removalContext *context)
+{
+	if(context->shape == arb->a || context->shape == arb->b){
+		if(arb->state != cpArbiterStateCached){
+			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),
+		"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?)");
+	cpAssertSpaceUnlocked(space);
+	
+	removalContext context = {space, shape};
+	cpHashSetFilter(space->contactSet, (cpHashSetFilterFunc)contactSetFilterRemovedShape, &context);
+	cpSpaceHashRemove(space->staticShapes, shape, shape->hashid);
+	
+	cpSpaceActivateShapesTouchingShape(space, shape);
+}
+
+void
+cpSpaceRemoveBody(cpSpace *space, cpBody *body)
+{
+	cpAssertWarn(body->space == space,
+		"Cannot remove a body that was not 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 not 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, (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);
+}
+
+void
+cpSpaceEachBody(cpSpace *space, cpSpaceBodyIterator func, void *data)
+{
+	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);
+	}
+}
+
+