issue #503, chippmunk library is upgraded from 5.1 to 5.3.4, which is usd in cocos2d-iphone 0.99.5 & 1.0.0-rc

2011-05-21 14:58:22 +08:00 · 2011-05-21 14:58:22 +08:00 · 948c6d750e
parent 57a14e90c9
commit 948c6d750e
43 changed files with 1074 additions and 1034 deletions
--- a/chipmunk/Android.mk
+++ b/chipmunk/Android.mk
@ -26,7 +26,10 @@ src/cpHashSet.c \
 src/cpPolyShape.c \
 src/cpShape.c \
 src/cpSpace.c \
+src/cpSpaceComponent.c \
 src/cpSpaceHash.c \
+src/cpSpaceQuery.c \
+src/cpSpaceStep.c \
 src/cpVect.c

 LOCAL_C_INCLUDES := $(LOCAL_PATH)/include/chipmunk
--- a/chipmunk/include/chipmunk/chipmunk.h
+++ b/chipmunk/include/chipmunk/chipmunk.h
@ -26,6 +26,16 @@
 extern "C" {
 #endif

+#ifndef CP_ALLOW_PRIVATE_ACCESS
+	#define CP_ALLOW_PRIVATE_ACCESS 1
+#endif
+
+#if CP_ALLOW_PRIVATE_ACCESS == 1
+	#define CP_PRIVATE(symbol) symbol
+#else
+	#define CP_PRIVATE(symbol) symbol##_private
+#endif
+
 void cpMessage(const char *message, const char *condition, const char *file, int line, int isError);
 #ifdef NDEBUG
 	#define	cpAssertWarn(condition, message)
@ -40,43 +50,7 @@ void cpMessage(const char *message, const char *condition, const char *file, int
 #endif

 #include "chipmunk_types.h"
-
-static inline cpFloat 
-cpfmax(cpFloat a, cpFloat b)
-{
-	return (a > b) ? a : b;
-}
-
-static inline cpFloat
-cpfmin(cpFloat a, cpFloat b)
-{
-	return (a < b) ? a : b;
-}
-
-static inline cpFloat
-cpfabs(cpFloat n)
-{
-	return (n < 0) ? -n : n;
-}
-
-static inline cpFloat
-cpfclamp(cpFloat f, cpFloat min, cpFloat max)
-{
-	return cpfmin(cpfmax(f, min), max);
-}
-
-static inline cpFloat
-cpflerp(cpFloat f1, cpFloat f2, cpFloat t)
-{
-	return f1*(1.0f - t) + f2*t;
-}
-
-static inline cpFloat
-cpflerpconst(cpFloat f1, cpFloat f2, cpFloat d)
-{
-	return f1 + cpfclamp(f2 - f1, -d, d);
-}
-
+	
 #ifndef INFINITY
 	#ifdef _MSC_VER
 		union MSVC_EVIL_FLOAT_HACK
@ -97,7 +71,7 @@ cpflerpconst(cpFloat f1, cpFloat f2, cpFloat d)
 	#endif
 #endif

-// Maximum allocated size for various Chipmunk buffer sizes
+// Maximum allocated size for various Chipmunk buffers
 #define CP_BUFFER_BYTES (32*1024)

 #define cpmalloc malloc
@ -107,11 +81,11 @@ cpflerpconst(cpFloat f1, cpFloat f2, cpFloat d)

 #include "cpVect.h"
 #include "cpBB.h"
-#include "cpBody.h"
 #include "cpArray.h"
 #include "cpHashSet.h"
 #include "cpSpaceHash.h"

+#include "cpBody.h"
 #include "cpShape.h"
 #include "cpPolyShape.h"

@ -125,24 +99,65 @@ cpflerpconst(cpFloat f1, cpFloat f2, cpFloat d)
 #define CP_HASH_COEF (3344921057ul)
 #define CP_HASH_PAIR(A, B) ((cpHashValue)(A)*CP_HASH_COEF ^ (cpHashValue)(B)*CP_HASH_COEF)

-extern char *cpVersionString;
+extern const char *cpVersionString;
 void cpInitChipmunk(void);

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

-// Calculate the moment of inertia for a line segment. (beveling radius not supported)
+/**
+	Calculate area of a hollow circle.
+*/
+cpFloat cpAreaForCircle(cpFloat r1, cpFloat r2);
+
+/**
+	Calculate the moment of inertia for a line segment.
+	Beveling radius is not supported.
+*/
 cpFloat cpMomentForSegment(cpFloat m, cpVect a, cpVect b);

-// Calculate the moment of inertia for a solid polygon shape.
-cpFloat cpMomentForPoly(cpFloat m, int numVerts, cpVect *verts, cpVect offset);
+/**
+	Calculate the area of a fattened (capsule shaped) line segment.
+*/
+cpFloat cpAreaForSegment(cpVect a, cpVect b, cpFloat r);

-// Calculate the moment of inertia for a solid box.
+/**
+	Calculate the moment of inertia for a solid polygon shape assuming it's center of gravity is at it's centroid. The offset is added to each vertex.
+*/
+cpFloat cpMomentForPoly(cpFloat m, int numVerts, const cpVect *verts, cpVect offset);
+
+/**
+	Calculate the signed area of a polygon.
+*/
+cpFloat cpAreaForPoly(const int numVerts, const cpVect *verts);
+
+/**
+	Calculate the natural centroid of a polygon.
+*/
+cpVect cpCentroidForPoly(const int numVerts, const cpVect *verts);
+
+/**
+	Center the polygon on the origin. (Subtracts the centroid of the polygon from each vertex)
+*/
+void cpRecenterPoly(const int numVerts, cpVect *verts);
+
+/**
+	Calculate the moment of inertia for a solid box.
+*/
 cpFloat cpMomentForBox(cpFloat m, cpFloat width, cpFloat height);

 #ifdef __cplusplus
 }
+
+static inline cpVect operator *(const cpVect v, const cpFloat s){return cpvmult(v, s);}
+static inline cpVect operator +(const cpVect v1, const cpVect v2){return cpvadd(v1, v2);}
+static inline cpVect operator -(const cpVect v1, const cpVect v2){return cpvsub(v1, v2);}
+static inline cpBool operator ==(const cpVect v1, const cpVect v2){return cpveql(v1, v2);}
+static inline cpVect operator -(const cpVect v){return cpvneg(v);}
+
 #endif

 #endif
--- a/chipmunk/include/chipmunk/chipmunk_ffi.h
+++ b/chipmunk/include/chipmunk/chipmunk_ffi.h
@ -15,6 +15,7 @@
 #endif

 MAKE_REF(cpv); // makes a variable named _cpv that contains the function pointer for cpv()
+MAKE_REF(cpveql);
 MAKE_REF(cpvadd);
 MAKE_REF(cpvneg);
 MAKE_REF(cpvsub);
@ -33,8 +34,8 @@ MAKE_REF(cpvnormalize_safe);
 MAKE_REF(cpvclamp);
 MAKE_REF(cpvlerpconst);
 MAKE_REF(cpvdist);
-MAKE_REF(cpvnear);
 MAKE_REF(cpvdistsq);
+MAKE_REF(cpvnear);

 MAKE_REF(cpBBNew);
 MAKE_REF(cpBBintersects);
@ -46,10 +47,18 @@ MAKE_REF(cpBBexpand);
 MAKE_REF(cpBodyWorld2Local);
 MAKE_REF(cpBodyLocal2World);
 MAKE_REF(cpBodyApplyImpulse);
+MAKE_REF(cpBodyIsSleeping);
+MAKE_REF(cpBodyIsRogue);
+MAKE_REF(cpBodyKineticEnergy);

 MAKE_REF(cpArbiterIsFirstContact);
 MAKE_REF(cpArbiterGetShapes);
 MAKE_REF(cpArbiterGetNormal);
 MAKE_REF(cpArbiterGetPoint);

+MAKE_REF(cpConstraintGetImpulse);
+
+MAKE_REF(cpSegmentQueryHitPoint);
+MAKE_REF(cpSegmentQueryHitDist);
+
 #endif // _CHIPMUNK_FFI_H_
--- a/chipmunk/include/chipmunk/chipmunk_types.h
+++ b/chipmunk/include/chipmunk/chipmunk_types.h
@ -3,14 +3,29 @@

 #ifdef __APPLE__
 #ifndef AIRPLAYUSECHIPMUNK
-	#import "TargetConditionals.h"
-#endif
+   #import "TargetConditionals.h"
 #endif

-// Use single precision floats on the iPhone.
-#if TARGET_OS_IPHONE==1
-	#define CP_USE_DOUBLES 0
-#else
+// dont' use CGPoints in cocos2d-x to make your code multi-platform
+// #if (defined TARGET_OS_IPHONE) && (!defined CP_USE_CGPOINTS)
+//	#define CP_USE_CGPOINTS
+// #endif
+
+#ifdef CP_USE_CGPOINTS
+	#if TARGET_OS_IPHONE
+		#import <CoreGraphics/CGGeometry.h>
+	#elif TARGET_OS_MAC
+		#import <ApplicationServices/ApplicationServices.h>
+	#endif
+	
+	#if defined(__LP64__) && __LP64__
+		#define CP_USE_DOUBLES 1
+	#else
+		#define CP_USE_DOUBLES 0
+	#endif
+#endif
+
+#ifndef CP_USE_DOUBLES
 	// use doubles by default for higher precision
 	#define CP_USE_DOUBLES 1
 #endif
@ -41,17 +56,67 @@
 	#define cpfceil ceilf
 #endif

-//#if TARGET_OS_IPHONE
-	// CCPoints are structurally the same, and allow
-	// easy interoperability with other iPhone libraries
-	//#import <CoreGraphics/CCGeometry.h>
-	//typedef CCPoint cpVect;
-//#else
+static inline cpFloat
+cpfmax(cpFloat a, cpFloat b)
+{
+	return (a > b) ? a : b;
+}
+
+static inline cpFloat
+cpfmin(cpFloat a, cpFloat b)
+{
+	return (a < b) ? a : b;
+}
+
+static inline cpFloat
+cpfabs(cpFloat n)
+{
+	return (n < 0) ? -n : n;
+}
+
+static inline cpFloat
+cpfclamp(cpFloat f, cpFloat min, cpFloat max)
+{
+	return cpfmin(cpfmax(f, min), max);
+}
+
+static inline cpFloat
+cpflerp(cpFloat f1, cpFloat f2, cpFloat t)
+{
+	return f1*(1.0f - t) + f2*t;
+}
+
+static inline cpFloat
+cpflerpconst(cpFloat f1, cpFloat f2, cpFloat d)
+{
+	return f1 + cpfclamp(f2 - f1, -d, d);
+}
+
+// CGPoints are structurally the same, and allow
+// easy interoperability with other Cocoa libraries
+#ifdef CP_USE_CGPOINTS
+	typedef CGPoint cpVect;
+#else
 	typedef struct cpVect{cpFloat x,y;} cpVect;
-//#endif
+#endif

 typedef unsigned int 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
+	typedef int cpBool;
+#endif
+
+#ifndef cpTrue
+	#define cpTrue 1
+#endif
+
+#ifndef cpFalse
+	#define cpFalse 0
+#endif
+
 #ifdef CP_DATA_POINTER_TYPE
 	typedef CP_DATA_POINTER_TYPE cpDataPointer;
 #else
@ -76,6 +141,12 @@ typedef unsigned int cpHashValue;
 	typedef unsigned int cpLayers;
 #endif

+#ifdef CP_TIMESTAMP_TYPE
+	typedef CP_TIMESTAMP_TYPE cpTimestamp;
+#else
+	typedef unsigned int cpTimestamp;
+#endif
+
 #ifndef CP_NO_GROUP
 	#define CP_NO_GROUP ((cpGroup)0)
 #endif
--- a/chipmunk/include/chipmunk/constraints/cpConstraint.h
+++ b/chipmunk/include/chipmunk/constraints/cpConstraint.h
@ -39,7 +39,7 @@ typedef struct cpConstraintClass {


 typedef struct cpConstraint {
-	const cpConstraintClass *klass;
+	CP_PRIVATE(const cpConstraintClass *klass);
 	
 	cpBody *a, *b;
 	cpFloat maxForce;
@ -56,14 +56,26 @@ typedef cpConstraint cpJoint;
 void cpConstraintDestroy(cpConstraint *constraint);
 void cpConstraintFree(cpConstraint *constraint);

+static inline void
+cpConstraintActivateBodies(cpConstraint *constraint)
+{
+	cpBody *a = constraint->a; if(a) cpBodyActivate(a);
+	cpBody *b = constraint->b; if(b) cpBodyActivate(b);
+}
+
+static inline cpFloat
+cpConstraintGetImpulse(cpConstraint *constraint)
+{
+	return constraint->CP_PRIVATE(klass)->getImpulse(constraint);
+}

 #define cpConstraintCheckCast(constraint, struct) \
-	cpAssert(constraint->klass == struct##GetClass(), "Constraint is not a "#struct);
+	cpAssert(constraint->CP_PRIVATE(klass) == struct##GetClass(), "Constraint is not a "#struct);


 #define CP_DefineConstraintGetter(struct, type, member, name) \
 static inline type \
-struct##Get##name(cpConstraint *constraint){ \
+struct##Get##name(const cpConstraint *constraint){ \
 	cpConstraintCheckCast(constraint, struct); \
 	return ((struct *)constraint)->member; \
 } \
@ -72,6 +84,7 @@ struct##Get##name(cpConstraint *constraint){ \
 static inline void \
 struct##Set##name(cpConstraint *constraint, type value){ \
 	cpConstraintCheckCast(constraint, struct); \
+	cpConstraintActivateBodies(constraint); \
 	((struct *)constraint)->member = value; \
 } \

--- a/chipmunk/include/chipmunk/constraints/cpDampedRotarySpring.h
+++ b/chipmunk/include/chipmunk/constraints/cpDampedRotarySpring.h
@ -30,8 +30,8 @@ typedef struct cpDampedRotarySpring {
 	cpFloat damping;
 	cpDampedRotarySpringTorqueFunc springTorqueFunc;
 	
-	cpFloat dt;
 	cpFloat target_wrn;
+	cpFloat w_coef;
 	
 	cpFloat iSum;
 } cpDampedRotarySpring;
--- a/chipmunk/include/chipmunk/constraints/cpDampedSpring.h
+++ b/chipmunk/include/chipmunk/constraints/cpDampedSpring.h
@ -33,8 +33,8 @@ typedef struct cpDampedSpring {
 	cpFloat damping;
 	cpDampedSpringForceFunc springForceFunc;
 	
-	cpFloat dt;
 	cpFloat target_vrn;
+	cpFloat v_coef;
 	
 	cpVect r1, r2;
 	cpFloat nMass;
--- a/chipmunk/include/chipmunk/constraints/cpGrooveJoint.h
+++ b/chipmunk/include/chipmunk/constraints/cpGrooveJoint.h
@ -40,5 +40,9 @@ cpGrooveJoint *cpGrooveJointAlloc(void);
 cpGrooveJoint *cpGrooveJointInit(cpGrooveJoint *joint, cpBody *a, cpBody *b, cpVect groove_a, cpVect groove_b, cpVect anchr2);
 cpConstraint *cpGrooveJointNew(cpBody *a, cpBody *b, cpVect groove_a, cpVect groove_b, cpVect anchr2);

-// TODO setters for the groove.
+
+CP_DefineConstraintGetter(cpGrooveJoint, cpVect, grv_a, GrooveA);
+void cpGrooveJointSetGrooveA(cpConstraint *constraint, cpVect value);
+CP_DefineConstraintGetter(cpGrooveJoint, cpVect, grv_b, GrooveB);
+void cpGrooveJointSetGrooveB(cpConstraint *constraint, cpVect value);
 CP_DefineConstraintProperty(cpGrooveJoint, cpVect, anchr2, Anchr2);
--- a/chipmunk/include/chipmunk/constraints/util.h
+++ b/chipmunk/include/chipmunk/constraints/util.h
@ -25,6 +25,17 @@ void cpConstraintInit(cpConstraint *constraint, const cpConstraintClass *klass,

 #define J_MAX(constraint, dt) (((cpConstraint *)constraint)->maxForce*(dt))

+// Get valid body pointers and exit early if the bodies are idle
+#define CONSTRAINT_BEGIN(constraint, a_var, b_var) \
+cpBody *a_var, *b_var; { \
+	a_var = ((cpConstraint *)constraint)->a; \
+	b_var = ((cpConstraint *)constraint)->b; \
+	if( \
+		(cpBodyIsSleeping(a_var) || cpBodyIsStatic(a_var)) && \
+		(cpBodyIsSleeping(b_var) || cpBodyIsStatic(b_var)) \
+	) return; \
+}
+
 static inline cpVect
 relative_velocity(cpBody *a, cpBody *b, cpVect r1, cpVect r2){
 	cpVect v1_sum = cpvadd(a->v, cpvmult(cpvperp(r1), a->w));
@ -45,11 +56,18 @@ apply_impulses(cpBody *a , cpBody *b, cpVect r1, cpVect r2, cpVect j)
 	cpBodyApplyImpulse(b, j, r2);
 }

+static inline 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 inline void
 apply_bias_impulses(cpBody *a , cpBody *b, cpVect r1, cpVect r2, cpVect j)
 {
-	cpBodyApplyBiasImpulse(a, cpvneg(j), r1);
-	cpBodyApplyBiasImpulse(b, j, r2);
+	apply_bias_impulse(a, cpvneg(j), r1);
+	apply_bias_impulse(b, j, r2);
 }

 static inline cpVect
--- a/chipmunk/include/chipmunk/cpArbiter.h
+++ b/chipmunk/include/chipmunk/cpArbiter.h
@ -31,73 +31,79 @@ extern cpFloat cp_collision_slop;
 // Data structure for contact points.
 typedef struct cpContact {
 	// Contact point and normal.
-	cpVect p, n;
+	cpVect CP_PRIVATE(p), CP_PRIVATE(n);
 	// Penetration distance.
-	cpFloat dist;
+	CP_PRIVATE(cpFloat dist);
 	
 	// Calculated by cpArbiterPreStep().
-	cpVect r1, r2;
-	cpFloat nMass, tMass, bounce;
+	cpVect CP_PRIVATE(r1), CP_PRIVATE(r2);
+	cpFloat CP_PRIVATE(nMass), CP_PRIVATE(tMass), CP_PRIVATE(bounce);

 	// Persistant contact information.
-	cpFloat jnAcc, jtAcc, jBias;
-	cpFloat bias;
+	cpFloat CP_PRIVATE(jnAcc), CP_PRIVATE(jtAcc), CP_PRIVATE(jBias);
+	CP_PRIVATE(cpFloat bias);
 	
 	// Hash value used to (mostly) uniquely identify a contact.
-	cpHashValue hash;
+	CP_PRIVATE(cpHashValue hash);
 } cpContact;

 // 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);
+cpVect CP_PRIVATE(cpContactsSumImpulses)(cpContact *contacts, int numContacts);
+cpVect CP_PRIVATE(cpContactsSumImpulsesWithFriction)(cpContact *contacts, int numContacts);
+
+#define CP_MAX_CONTACTS_PER_ARBITER 6

 typedef enum cpArbiterState {
 	cpArbiterStateNormal,
 	cpArbiterStateFirstColl,
 	cpArbiterStateIgnore,
+	cpArbiterStateSleep,
+	cpArbiterStateCached,
 } cpArbiterState;

 // Data structure for tracking collisions between shapes.
 typedef struct cpArbiter {
 	// Information on the contact points between the objects.
-	int numContacts;
-	cpContact *contacts;
+	CP_PRIVATE(int numContacts);
+	CP_PRIVATE(cpContact *contacts);
 	
-	// The two shapes involved in the collision.
+	// The two shapes and bodies involved in the collision.
 	// These variables are NOT in the order defined by the collision handler.
-	cpShape *private_a, *private_b;
+	// Using CP_ARBITER_GET_SHAPES and CP_ARBITER_GET_BODIES will save you from
+	// many headaches
+	cpShape CP_PRIVATE(*a), CP_PRIVATE(*b);
 	
 	// Calculated before calling the pre-solve collision handler
 	// Override them with custom values if you want specialized behavior
-	cpFloat e;
-	cpFloat u;
+	CP_PRIVATE(cpFloat e);
+	CP_PRIVATE(cpFloat u);
 	 // Used for surface_v calculations, implementation may change
-	cpVect surface_vr;
+	CP_PRIVATE(cpVect surface_vr);
 	
 	// Time stamp of the arbiter. (from cpSpace)
-	int stamp;
+	CP_PRIVATE(cpTimestamp stamp);
 	
-	struct cpCollisionHandler *handler;
+	CP_PRIVATE(struct cpCollisionHandler *handler);
 	
 	// Are the shapes swapped in relation to the collision handler?
-	char swappedColl;
-	char state;
+	CP_PRIVATE(cpBool swappedColl);
+	CP_PRIVATE(cpArbiterState state);
 } cpArbiter;

 // Arbiters are allocated in large buffers by the space and don't require a destroy function
-cpArbiter* cpArbiterInit(cpArbiter *arb, cpShape *a, cpShape *b);
+cpArbiter* CP_PRIVATE(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);
+void CP_PRIVATE(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);
+void CP_PRIVATE(cpArbiterPreStep)(cpArbiter *arb, cpFloat dt_inv);
+void CP_PRIVATE(cpArbiterApplyCachedImpulse)(cpArbiter *arb);
 // Run an iteration of the solver on the arbiter.
-void cpArbiterApplyImpulse(cpArbiter *arb, cpFloat eCoef);
+void CP_PRIVATE(cpArbiterApplyImpulse)(cpArbiter *arb, cpFloat eCoef);

 // Arbiter Helper Functions
 cpVect cpArbiterTotalImpulse(cpArbiter *arb);
@ -106,31 +112,77 @@ void cpArbiterIgnore(cpArbiter *arb);


 static inline void
-cpArbiterGetShapes(cpArbiter *arb, cpShape **a, cpShape **b)
+cpArbiterGetShapes(const cpArbiter *arb, cpShape **a, cpShape **b)
 {
-	if(arb->swappedColl){
-		(*a) = arb->private_b, (*b) = arb->private_a;
+	if(arb->CP_PRIVATE(swappedColl)){
+		(*a) = arb->CP_PRIVATE(b), (*b) = arb->CP_PRIVATE(a);
 	} else {
-		(*a) = arb->private_a, (*b) = arb->private_b;
+		(*a) = arb->CP_PRIVATE(a), (*b) = arb->CP_PRIVATE(b);
 	}
 }
 #define CP_ARBITER_GET_SHAPES(arb, a, b) cpShape *a, *b; cpArbiterGetShapes(arb, &a, &b);

+static inline void
+cpArbiterGetBodies(const cpArbiter *arb, cpBody **a, cpBody **b)
+{
+	CP_ARBITER_GET_SHAPES(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 inline cpBool
+cpArbiterIsFirstContact(const cpArbiter *arb)
+{
+	return arb->CP_PRIVATE(state) == cpArbiterStateFirstColl;
+}
+
 static inline int
-cpArbiterIsFirstContact(cpArbiter *arb)
+cpArbiterGetCount(const cpArbiter *arb)
 {
-	return arb->state == cpArbiterStateFirstColl;
+	return arb->CP_PRIVATE(numContacts);
 }

 static inline cpVect
-cpArbiterGetNormal(cpArbiter *arb, int i)
+cpArbiterGetNormal(const cpArbiter *arb, int i)
 {
-	cpVect n = arb->contacts[i].n;
-	return arb->swappedColl ? cpvneg(n) : n;
+	cpVect n = arb->CP_PRIVATE(contacts)[i].CP_PRIVATE(n);
+	return arb->CP_PRIVATE(swappedColl) ? cpvneg(n) : n;
 }

 static inline cpVect
-cpArbiterGetPoint(cpArbiter *arb, int i)
+cpArbiterGetPoint(const cpArbiter *arb, int i)
 {
-	return arb->contacts[i].p;
+	return arb->CP_PRIVATE(contacts)[i].CP_PRIVATE(p);
+}
+
+static inline cpFloat
+cpArbiteGetDepth(const cpArbiter *arb, int i)
+{
+	return arb->CP_PRIVATE(contacts)[i].CP_PRIVATE(dist);
+}
+
+typedef struct cpContactPointSet {
+	int count;
+	
+	struct {
+		cpVect point, normal;
+		cpFloat dist;
+	} points[CP_MAX_CONTACTS_PER_ARBITER];
+} cpContactPointSet;
+
+static inline 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->CP_PRIVATE(contacts)[i].CP_PRIVATE(p);
+		set.points[i].normal = arb->CP_PRIVATE(contacts)[i].CP_PRIVATE(n);
+		set.points[i].dist = arb->CP_PRIVATE(contacts)[i].CP_PRIVATE(dist);
+	}
+	
+	return set;
 }
--- a/chipmunk/include/chipmunk/cpArray.h
+++ b/chipmunk/include/chipmunk/cpArray.h
@ -22,8 +22,9 @@
 // NOTE: cpArray is rarely used and will probably go away.

 typedef struct cpArray{
-	int num, max;
-	void **arr;
+	CP_PRIVATE(int num);
+	CP_PRIVATE(int max);
+	CP_PRIVATE(void **arr);
 } cpArray;

 typedef void (*cpArrayIter)(void *ptr, void *data);
@ -42,5 +43,7 @@ void *cpArrayPop(cpArray *arr);
 void cpArrayDeleteIndex(cpArray *arr, int idx);
 void cpArrayDeleteObj(cpArray *arr, void *obj);

+void cpArrayAppend(cpArray *arr, cpArray *other);
+
 void cpArrayEach(cpArray *arr, cpArrayIter iterFunc, void *data);
-int cpArrayContains(cpArray *arr, void *ptr);
+cpBool cpArrayContains(cpArray *arr, void *ptr);
--- a/chipmunk/include/chipmunk/cpBB.h
+++ b/chipmunk/include/chipmunk/cpBB.h
@ -31,19 +31,19 @@ cpBBNew(const cpFloat l, const cpFloat b,
 	return bb;
 }

-static inline int
+static inline 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 inline int
+static inline 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 inline int
+static inline 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);
--- a/chipmunk/include/chipmunk/cpBody.h
+++ b/chipmunk/include/chipmunk/cpBody.h
@ -20,14 +20,27 @@
 */

 struct cpBody;
+struct cpShape;
+struct cpSpace;
+
 typedef void (*cpBodyVelocityFunc)(struct cpBody *body, cpVect gravity, cpFloat damping, cpFloat dt);
 typedef void (*cpBodyPositionFunc)(struct cpBody *body, cpFloat dt);

 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.
+typedef struct cpComponentNode {
+	struct cpBody *parent;
+	struct cpBody *next;
+	int rank;
+	cpFloat idleTime;
+} cpComponentNode;
+
 typedef struct cpBody{
-	// *** Integration Functions.ntoehu
+	// *** Integration Functions.

 	// Function that is called to integrate the body's velocity. (Defaults to cpBodyUpdateVelocity)
 	cpBodyVelocityFunc velocity_func;
@ -63,16 +76,26 @@ typedef struct cpBody{
 	// 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;
+	CP_PRIVATE(cpVect v_bias);
+	CP_PRIVATE(cpFloat w_bias);
 	
-//	int active;
+	// Space this body has been added to
+	CP_PRIVATE(struct cpSpace *space);
+	
+	// Pointer to the shape list.
+	// Shapes form a linked list using cpShape.next when added to a space.
+	CP_PRIVATE(struct cpShape *shapesList);
+	
+	// Used by cpSpaceStep() to store contact graph information.
+	CP_PRIVATE(cpComponentNode node);
 } cpBody;

 // Basic allocation/destruction functions
@ -80,11 +103,48 @@ cpBody *cpBodyAlloc(void);
 cpBody *cpBodyInit(cpBody *body, cpFloat m, cpFloat i);
 cpBody *cpBodyNew(cpFloat m, cpFloat i);

+cpBody *cpBodyInitStatic(cpBody *body);
+cpBody *cpBodyNewStatic();
+
 void cpBodyDestroy(cpBody *body);
 void cpBodyFree(cpBody *body);

-#define CP_DefineBodyGetter(type, member, name) static inline type cpBodyGet##name(cpBody *body){return body->member;}
-#define CP_DefineBodySetter(type, member, name) static inline void cpBodySet##name(cpBody *body, type value){body->member = value;}
+// Wake up a sleeping or idle body. (defined in cpSpace.c)
+void cpBodyActivate(cpBody *body);
+
+// Force a body to sleep;
+// defined in cpSpaceComponent.c
+void cpBodySleep(cpBody *body);
+void cpBodySleepWithGroup(cpBody *body, cpBody *group);
+
+static inline cpBool
+cpBodyIsSleeping(const cpBody *body)
+{
+	return (CP_PRIVATE(body->node).next != ((cpBody*)0));
+}
+
+static inline cpBool
+cpBodyIsStatic(const cpBody *body)
+{
+	return CP_PRIVATE(body->node).idleTime == INFINITY;
+}
+
+static inline cpBool
+cpBodyIsRogue(const cpBody *body)
+{
+	return (body->CP_PRIVATE(space) == ((struct cpSpace*)0));
+}
+
+
+#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) \
@ -120,41 +180,40 @@ void cpBodyUpdatePosition(cpBody *body, cpFloat dt);

 // Convert body local to world coordinates
 static inline cpVect
-cpBodyLocal2World(cpBody *body, cpVect v)
+cpBodyLocal2World(const cpBody *body, const cpVect v)
 {
 	return cpvadd(body->p, cpvrotate(v, body->rot));
 }

 // Convert world to body local coordinates
 static inline cpVect
-cpBodyWorld2Local(cpBody *body, cpVect v)
+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 inline void
-cpBodyApplyImpulse(cpBody *body, cpVect j, cpVect r)
+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);
 }

-// Not intended for external use. Used by cpArbiter.c and cpConstraint.c.
-static inline void
-cpBodyApplyBiasImpulse(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);
-}
-
 // 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, cpVect f, cpVect r);
+void cpBodyApplyForce(cpBody *body, const cpVect f, const cpVect r);
+
+static inline 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);
-
-//int cpBodyMarkLowEnergy(cpBody *body, cpFloat dvsq, int max);
--- a/chipmunk/include/chipmunk/cpCollision.h
+++ b/chipmunk/include/chipmunk/cpCollision.h
@ -19,5 +19,10 @@
 * SOFTWARE.
 */

-// Collides two cpShape structures. (this function is lonely :( )
-int cpCollideShapes(cpShape *a, cpShape *b, cpContact *arr);
+//TODO delete this header?
+
+// Collides two cpShape structures.
+// Returns the number of contact points added to arr
+// which should be at least CP_MAX_CONTACTS_PER_ARBITER in length.
+// This function is very lonely in this header :(
+int cpCollideShapes(const cpShape *a, const cpShape *b, cpContact *arr);
--- a/chipmunk/include/chipmunk/cpHashSet.h
+++ b/chipmunk/include/chipmunk/cpHashSet.h
@ -25,39 +25,36 @@
 // cpHashSetBin's form the linked lists in the chained hash table.
 typedef struct cpHashSetBin {
 	// Pointer to the element.
-	void *elt;
+	CP_PRIVATE(void *elt);
 	// Hash value of the element.
-	cpHashValue hash;
+	CP_PRIVATE(cpHashValue hash);
 	// Next element in the chain.
-	struct cpHashSetBin *next;
+	CP_PRIVATE(struct cpHashSetBin *next);
 } cpHashSetBin;

 // Equality function. Returns true if ptr is equal to elt.
-typedef int (*cpHashSetEqlFunc)(void *ptr, void *elt);
+typedef cpBool (*cpHashSetEqlFunc)(void *ptr, void *elt);
 // Used by cpHashSetInsert(). Called to transform the ptr into an element.
 typedef void *(*cpHashSetTransFunc)(void *ptr, void *data);
-// Iterator function for a hashset.
-typedef void (*cpHashSetIterFunc)(void *elt, void *data);
-// Filter function. Returns false if elt should be dropped.
-typedef int (*cpHashSetFilterFunc)(void *elt, void *data);

 typedef struct cpHashSet {
 	// Number of elements stored in the table.
-	int entries;
+	CP_PRIVATE(int entries);
 	// Number of cells in the table.
-	int size;
+	CP_PRIVATE(int size);
 	
-	cpHashSetEqlFunc eql;
-	cpHashSetTransFunc trans;
+	CP_PRIVATE(cpHashSetEqlFunc eql);
+	CP_PRIVATE(cpHashSetTransFunc trans);
 	
 	// Default value returned by cpHashSetFind() when no element is found.
 	// Defaults to NULL.
-	void *default_value;
+	CP_PRIVATE(void *default_value);
 	
 	// The table and recycled bins
-	cpHashSetBin **table, *pooledBins;
+	CP_PRIVATE(cpHashSetBin **table);
+	CP_PRIVATE(cpHashSetBin *pooledBins);
 	
-	cpArray *allocatedBuffers;
+	CP_PRIVATE(cpArray *allocatedBuffers);
 } cpHashSet;

 // Basic allocation/destruction functions.
@ -77,6 +74,9 @@ void *cpHashSetRemove(cpHashSet *set, cpHashValue hash, void *ptr);
 void *cpHashSetFind(cpHashSet *set, cpHashValue hash, void *ptr);

 // Iterate over a hashset.
+typedef void (*cpHashSetIterFunc)(void *elt, void *data);
 void cpHashSetEach(cpHashSet *set, cpHashSetIterFunc func, void *data);
-// Iterate over a hashset, retain .
+
+// Iterate over a hashset, drop the element if the func returns false.
+typedef cpBool (*cpHashSetFilterFunc)(void *elt, void *data);
 void cpHashSetFilter(cpHashSet *set, cpHashSetFilterFunc func, void *data);
--- a/chipmunk/include/chipmunk/cpPolyShape.h
+++ b/chipmunk/include/chipmunk/cpPolyShape.h
@ -29,16 +29,16 @@ typedef struct cpPolyShapeAxis{

 // Convex polygon shape structure.
 typedef struct cpPolyShape{
-	cpShape shape;
+	CP_PRIVATE(cpShape shape);
 	
 	// Vertex and axis lists.
-	int numVerts;
-	cpVect *verts;
-	cpPolyShapeAxis *axes;
+	CP_PRIVATE(int numVerts);
+	CP_PRIVATE(cpVect *verts);
+	CP_PRIVATE(cpPolyShapeAxis *axes);

 	// Transformed vertex and axis lists.
-	cpVect *tVerts;
-	cpPolyShapeAxis *tAxes;
+	CP_PRIVATE(cpVect *tVerts);
+	CP_PRIVATE(cpPolyShapeAxis *tAxes);
 } cpPolyShape;

 // Basic allocation functions.
@ -50,7 +50,7 @@ cpPolyShape *cpBoxShapeInit(cpPolyShape *poly, cpBody *body, cpFloat width, cpFl
 cpShape *cpBoxShapeNew(cpBody *body, cpFloat width, cpFloat height);

 // Check that a set of vertexes has a correct winding and that they are convex
-int cpPolyValidate(cpVect *verts, int numVerts);
+cpBool cpPolyValidate(const cpVect *verts, const int numVerts);

 int cpPolyShapeGetNumVerts(cpShape *shape);
 cpVect cpPolyShapeGetVert(cpShape *shape, int idx);
@ -61,43 +61,43 @@ cpVect cpPolyShapeGetVert(cpShape *shape, int idx);
 static inline cpFloat
 cpPolyShapeValueOnAxis(const cpPolyShape *poly, const cpVect n, const cpFloat d)
 {
-	cpVect *verts = poly->tVerts;
+	cpVect *verts = poly->CP_PRIVATE(tVerts);
 	cpFloat min = cpvdot(n, verts[0]);
 	
 	int i;
-	for(i=1; i<poly->numVerts; i++)
+	for(i=1; i<poly->CP_PRIVATE(numVerts); i++)
 		min = cpfmin(min, cpvdot(n, verts[i]));
 	
 	return min - d;
 }

 // Returns true if the polygon contains the vertex.
-static inline int
-cpPolyShapeContainsVert(cpPolyShape *poly, cpVect v)
+static inline cpBool
+cpPolyShapeContainsVert(const cpPolyShape *poly, const cpVect v)
 {
-	cpPolyShapeAxis *axes = poly->tAxes;
+	cpPolyShapeAxis *axes = poly->CP_PRIVATE(tAxes);
 	
 	int i;
-	for(i=0; i<poly->numVerts; i++){
+	for(i=0; i<poly->CP_PRIVATE(numVerts); i++){
 		cpFloat dist = cpvdot(axes[i].n, v) - axes[i].d;
-		if(dist > 0.0f) return 0;
+		if(dist > 0.0f) return cpFalse;
 	}
 	
-	return 1;
+	return cpTrue;
 }

 // Same as cpPolyShapeContainsVert() but ignores faces pointing away from the normal.
-static inline int
-cpPolyShapeContainsVertPartial(cpPolyShape *poly, cpVect v, cpVect n)
+static inline cpBool
+cpPolyShapeContainsVertPartial(const cpPolyShape *poly, const cpVect v, const cpVect n)
 {
-	cpPolyShapeAxis *axes = poly->tAxes;
+	cpPolyShapeAxis *axes = poly->CP_PRIVATE(tAxes);
 	
 	int i;
-	for(i=0; i<poly->numVerts; i++){
+	for(i=0; i<poly->CP_PRIVATE(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 0;
+		if(dist > 0.0f) return cpFalse;
 	}
 	
-	return 1;
+	return cpTrue;
 }
--- a/chipmunk/include/chipmunk/cpShape.h
+++ b/chipmunk/include/chipmunk/cpShape.h
@ -47,7 +47,7 @@ typedef struct cpShapeClass {
 	void (*destroy)(struct cpShape *shape);
 	
 	// called by cpShapePointQuery().
-	int (*pointQuery)(struct cpShape *shape, cpVect p);
+	cpBool (*pointQuery)(struct cpShape *shape, cpVect p);
 	
 	// called by cpShapeSegmentQuery()
 	 void (*segmentQuery)(struct cpShape *shape, cpVect a, cpVect b, cpSegmentQueryInfo *info);
@ -56,7 +56,7 @@ typedef struct cpShapeClass {
 // Basic shape struct that the others inherit from.
 typedef struct cpShape{
 	// The "class" of a shape as defined above 
-	const cpShapeClass *klass;
+	CP_PRIVATE(const cpShapeClass *klass);
 	
 	// cpBody that the shape is attached to.
 	cpBody *body;
@ -65,7 +65,7 @@ typedef struct cpShape{
 	cpBB bb;
 	
 	// Sensors invoke callbacks, but do not generate collisions
-	int sensor;
+	cpBool sensor;
 	
 	// *** Surface properties.
 	
@ -90,8 +90,11 @@ typedef struct cpShape{
 	
 	// *** Internally Used Fields
 	
+	// Shapes form a linked list when added to space on a non-NULL body
+	CP_PRIVATE(struct cpShape *next);
+	
 	// Unique id used as the hash value.
-	cpHashValue hashid;
+	CP_PRIVATE(cpHashValue hashid);
 } cpShape;

 // Low level shape initialization func.
@ -105,21 +108,21 @@ void cpShapeFree(cpShape *shape);
 cpBB cpShapeCacheBB(cpShape *shape);

 // Test if a point lies within a shape.
-int cpShapePointQuery(cpShape *shape, cpVect p);
+cpBool cpShapePointQuery(cpShape *shape, cpVect p);

 #define CP_DeclareShapeGetter(struct, type, name) type struct##Get##name(cpShape *shape)

 // Circle shape structure.
 typedef struct cpCircleShape{
-	cpShape shape;
+	CP_PRIVATE(cpShape shape);
 	
 	// Center in body space coordinates
-	cpVect c;
+	CP_PRIVATE(cpVect c);
 	// Radius.
-	cpFloat r;
+	CP_PRIVATE(cpFloat r);
 	
 	// Transformed center. (world space coordinates)
-	cpVect tc;
+	CP_PRIVATE(cpVect tc);
 } cpCircleShape;

 // Basic allocation functions for cpCircleShape.
@ -132,15 +135,15 @@ CP_DeclareShapeGetter(cpCircleShape, cpFloat, Radius);

 // Segment shape structure.
 typedef struct cpSegmentShape{
-	cpShape shape;
+	CP_PRIVATE(cpShape shape);
 	
 	// Endpoints and normal of the segment. (body space coordinates)
-	cpVect a, b, n;
+	cpVect CP_PRIVATE(a), CP_PRIVATE(b), CP_PRIVATE(n);
 	// Radius of the segment. (Thickness)
-	cpFloat r;
+	cpFloat CP_PRIVATE(r);

 	// Transformed endpoints and normal. (world space coordinates)
-	cpVect ta, tb, tn;
+	cpVect CP_PRIVATE(ta), CP_PRIVATE(tb), CP_PRIVATE(tn);
 } cpSegmentShape;

 // Basic allocation functions for cpSegmentShape.
@ -159,16 +162,16 @@ void cpResetShapeIdCounter(void);
 // Directed segment queries against individual shapes.
 void cpSegmentQueryInfoPrint(cpSegmentQueryInfo *info);

-int cpShapeSegmentQuery(cpShape *shape, cpVect a, cpVect b, cpSegmentQueryInfo *info);
+cpBool cpShapeSegmentQuery(cpShape *shape, cpVect a, cpVect b, cpSegmentQueryInfo *info);

 static inline cpVect
-cpSegmentQueryHitPoint(cpVect start, cpVect end, cpSegmentQueryInfo info)
+cpSegmentQueryHitPoint(const cpVect start, const cpVect end, const cpSegmentQueryInfo info)
 {
 	return cpvlerp(start, end, info.t);
 }

 static inline cpFloat
-cpSegmentQueryHitDist(cpVect start, cpVect end, cpSegmentQueryInfo info)
+cpSegmentQueryHitDist(const cpVect start, const cpVect end, const cpSegmentQueryInfo info)
 {
 	return cpvdist(start, end)*info.t;
 }
--- a/chipmunk/include/chipmunk/cpSpace.h
+++ b/chipmunk/include/chipmunk/cpSpace.h
@ -22,11 +22,11 @@
 struct cpSpace;

 // Number of frames that contact information should persist.
-extern int cp_contact_persistence;
+extern cpTimestamp cp_contact_persistence;

 // User collision handler function types.
-typedef int (*cpCollisionBeginFunc)(cpArbiter *arb, struct cpSpace *space, void *data);
-typedef int (*cpCollisionPreSolveFunc)(cpArbiter *arb, struct cpSpace *space, void *data);
+typedef cpBool (*cpCollisionBeginFunc)(cpArbiter *arb, struct cpSpace *space, void *data);
+typedef cpBool (*cpCollisionPreSolveFunc)(cpArbiter *arb, struct cpSpace *space, void *data);
 typedef void (*cpCollisionPostSolveFunc)(cpArbiter *arb, struct cpSpace *space, void *data);
 typedef void (*cpCollisionSeparateFunc)(cpArbiter *arb, struct cpSpace *space, void *data);

@ -42,9 +42,8 @@ typedef struct cpCollisionHandler {
 	void *data;
 } cpCollisionHandler;

-#define CP_MAX_CONTACTS_PER_ARBITER 6
 typedef struct cpContactBufferHeader {
-	int stamp;
+	cpTimestamp stamp;
 	struct cpContactBufferHeader *next;
 	unsigned int numContacts;
 } cpContactBufferHeader;
@ -64,44 +63,62 @@ typedef struct cpSpace{
 	// 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;
-	int locked;
+	// When the space lock count is non zero you cannot add or remove objects
+	CP_PRIVATE(int locked);
 	
 	// Time stamp. Is incremented on every call to cpSpaceStep().
-	int stamp;
+	CP_PRIVATE(cpTimestamp stamp);

 	// The static and active shape spatial hashes.
-	cpSpaceHash *staticShapes;
-	cpSpaceHash *activeShapes;
+	CP_PRIVATE(cpSpaceHash *staticShapes);
+	CP_PRIVATE(cpSpaceHash *activeShapes);
 	
 	// List of bodies in the system.
-	cpArray *bodies;
+	CP_PRIVATE(cpArray *bodies);
+	
+	// List of groups of sleeping bodies.
+	CP_PRIVATE(cpArray *sleepingComponents);
+	
+	// List of bodies that have been flagged to be awoken.
+	CP_PRIVATE(cpArray *rousedBodies);
 	
 	// List of active arbiters for the impulse solver.
-	cpArray *arbiters, *pooledArbiters;
+	CP_PRIVATE(cpArray *arbiters);
+	CP_PRIVATE(cpArray *pooledArbiters);
 	
 	// Linked list ring of contact buffers.
-	// Head is the current buffer. Tail is the oldest buffer.
-	// The list points in the direction of tail->head.
-	cpContactBufferHeader *contactBuffersHead, *contactBuffersTail;
+	// Head is the newest buffer, and each buffer points to a newer buffer.
+	// Head wraps around and points to the oldest (tail) buffer.
+	CP_PRIVATE(cpContactBufferHeader *contactBuffersHead);
+	CP_PRIVATE(cpContactBufferHeader *_contactBuffersTail_Deprecated);
 	
 	// List of buffers to be free()ed when destroying the space.
-	cpArray *allocatedBuffers;
+	CP_PRIVATE(cpArray *allocatedBuffers);
 	
 	// Persistant contact set.
-	cpHashSet *contactSet;
+	CP_PRIVATE(cpHashSet *contactSet);
 	
 	// List of constraints in the system.
-	cpArray *constraints;
+	CP_PRIVATE(cpArray *constraints);
 	
 	// Set of collisionpair functions.
-	cpHashSet *collFuncSet;
+	CP_PRIVATE(cpHashSet *collFuncSet);
 	// Default collision handler.
-	cpCollisionHandler defaultHandler;
+	CP_PRIVATE(cpCollisionHandler defaultHandler);
 	
-	cpHashSet *postStepCallbacks;
+	CP_PRIVATE(cpHashSet *postStepCallbacks);
+	
+	cpBody staticBody;
 } cpSpace;

 // Basic allocation/destruction functions.
@ -159,13 +176,20 @@ cpShape *cpSpacePointQueryFirst(cpSpace *space, cpVect point, cpLayers layers, c

 // Segment query callback function
 typedef void (*cpSpaceSegmentQueryFunc)(cpShape *shape, cpFloat t, cpVect n, void *data);
-int cpSpaceSegmentQuery(cpSpace *space, cpVect start, cpVect end, cpLayers layers, cpGroup group, cpSpaceSegmentQueryFunc func, void *data);
+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
 typedef void (*cpSpaceBBQueryFunc)(cpShape *shape, void *data);
 void cpSpaceBBQuery(cpSpace *space, cpBB bb, cpLayers layers, cpGroup group, cpSpaceBBQueryFunc func, void *data);

+// Shape query callback function
+typedef void (*cpSpaceShapeQueryFunc)(cpShape *shape, cpContactPointSet *points, void *data);
+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.
 typedef void (*cpSpaceBodyIterator)(cpBody *body, void *data);
@ -176,5 +200,7 @@ 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);
--- a/chipmunk/include/chipmunk/cpSpaceHash.h
+++ b/chipmunk/include/chipmunk/cpSpaceHash.h
@ -30,7 +30,7 @@ typedef struct cpHandle{
 	int retain;
 	// Query stamp. Used to make sure two objects
 	// aren't identified twice in the same query.
-	int stamp;
+	cpTimestamp stamp;
 } cpHandle;

 // Linked list element for in the chains.
@ -44,25 +44,26 @@ typedef cpBB (*cpSpaceHashBBFunc)(void *obj);

 typedef struct cpSpaceHash{
 	// Number of cells in the table.
-	int numcells;
+	CP_PRIVATE(int numcells);
 	// Dimentions of the cells.
-	cpFloat celldim;
+	CP_PRIVATE(cpFloat celldim);
 	
 	// BBox callback.
-	cpSpaceHashBBFunc bbfunc;
+	CP_PRIVATE(cpSpaceHashBBFunc bbfunc);

 	// Hashset of the handles and the recycled ones.
-	cpHashSet *handleSet;
-	cpArray *pooledHandles;
+	CP_PRIVATE(cpHashSet *handleSet);
+	CP_PRIVATE(cpArray *pooledHandles);
 	
 	// The table and the recycled bins.
-	cpSpaceHashBin **table, *pooledBins;
+	CP_PRIVATE(cpSpaceHashBin **table);
+	CP_PRIVATE(cpSpaceHashBin *pooledBins);
 	
 	// list of buffers to free on destruction.
-	cpArray *allocatedBuffers;
+	CP_PRIVATE(cpArray *allocatedBuffers);
 	
 	// Incremented on each query. See cpHandle.stamp.
-	int stamp;
+	CP_PRIVATE(cpTimestamp stamp);
 } cpSpaceHash;

 //Basic allocation/destruction functions.
@ -77,7 +78,7 @@ void cpSpaceHashFree(cpSpaceHash *hash);
 void cpSpaceHashResize(cpSpaceHash *hash, cpFloat celldim, int numcells);

 // Add an object to the hash.
-void cpSpaceHashInsert(cpSpaceHash *hash, void *obj, cpHashValue id, cpBB bb);
+void cpSpaceHashInsert(cpSpaceHash *hash, void *obj, cpHashValue id, cpBB _deprecated_ignored);
 // Remove an object from the hash.
 void cpSpaceHashRemove(cpSpaceHash *hash, void *obj, cpHashValue id);

--- a/chipmunk/include/chipmunk/cpVect.h
+++ b/chipmunk/include/chipmunk/cpVect.h
@ -19,8 +19,10 @@
 * SOFTWARE.
 */

-static const cpVect cpvzero={0.0f,0.0f};
+/// Constant for the zero vector.
+static const cpVect cpvzero = {0.0f,0.0f};

+/// Convenience constructor for cpVect structs.
 static inline cpVect
 cpv(const cpFloat x, const cpFloat y)
 {
@ -29,128 +31,176 @@ cpv(const cpFloat x, const cpFloat y)
 }

 // non-inlined functions
-cpFloat cpvlength(const cpVect v);
-cpVect cpvslerp(const cpVect v1, const cpVect v2, const cpFloat t);
-cpVect cpvslerpconst(const cpVect v1, const cpVect v2, const cpFloat a);
-cpVect cpvforangle(const cpFloat a); // convert radians to a normalized vector
-cpFloat cpvtoangle(const cpVect v); // convert a vector to radians
-char *cpvstr(const cpVect v); // get a string representation of a vector

+/// Returns the length of v.
+cpFloat cpvlength(const cpVect v);
+
+/// Spherical linearly interpolate between v1 and v2.
+cpVect cpvslerp(const cpVect v1, const cpVect v2, const cpFloat t);
+
+/// Spherical linearly interpolate between v1 towards v2 by no more than angle a radians
+cpVect cpvslerpconst(const cpVect v1, const cpVect v2, const cpFloat a);
+
+/// Returns the unit length vector for the given angle (in radians).
+cpVect cpvforangle(const cpFloat a);
+
+/// Returns the angular direction v is pointing in (in radians).
+cpFloat cpvtoangle(const cpVect v);
+
+/**
+	Returns a string representation of v. Intended mostly for debugging purposes and not production use.
+	
+	@attention The string points to a static local and is reset every time the function is called.
+	If you want to print more than one vector you will have to split up your printing onto separate lines.
+*/
+char *cpvstr(const cpVect v);
+
+/// Check if two vectors are equal. (Be careful when comparing floating point numbers!)
+static inline cpBool
+cpveql(const cpVect v1, const cpVect v2)
+{
+	return (v1.x == v2.x && v1.y == v2.y);
+}
+
+/// Add two vectors
 static inline cpVect
 cpvadd(const cpVect v1, const cpVect v2)
 {
 	return cpv(v1.x + v2.x, v1.y + v2.y);
 }

+/// Negate a vector.
 static inline cpVect
 cpvneg(const cpVect v)
 {
 	return cpv(-v.x, -v.y);
 }

+/// Subtract two vectors.
 static inline cpVect
 cpvsub(const cpVect v1, const cpVect v2)
 {
 	return cpv(v1.x - v2.x, v1.y - v2.y);
 }

+/// Scalar multiplication.
 static inline cpVect
 cpvmult(const cpVect v, const cpFloat s)
 {
 	return cpv(v.x*s, v.y*s);
 }

+/// Vector dot product.
 static inline cpFloat
 cpvdot(const cpVect v1, const cpVect v2)
 {
 	return v1.x*v2.x + v1.y*v2.y;
 }

+/**
+	2D vector cross product analog.
+	The cross product of 2D vectors results in a 3D vector with only a z component.
+	This function returns the magnitude of the z value.
+*/
 static inline cpFloat
 cpvcross(const cpVect v1, const cpVect v2)
 {
 	return v1.x*v2.y - v1.y*v2.x;
 }

+/// Returns a perpendicular vector. (90 degree rotation)
 static inline cpVect
 cpvperp(const cpVect v)
 {
 	return cpv(-v.y, v.x);
 }

+/// Returns a perpendicular vector. (-90 degree rotation)
 static inline cpVect
 cpvrperp(const cpVect v)
 {
 	return cpv(v.y, -v.x);
 }

+/// Returns the vector projection of v1 onto v2.
 static inline cpVect
 cpvproject(const cpVect v1, const cpVect v2)
 {
 	return cpvmult(v2, cpvdot(v1, v2)/cpvdot(v2, v2));
 }

+/// Uses complex number multiplication to rotate v1 by v2. Scaling will occur if v1 is not a unit vector.
 static inline cpVect
 cpvrotate(const cpVect v1, const cpVect v2)
 {
 	return cpv(v1.x*v2.x - v1.y*v2.y, v1.x*v2.y + v1.y*v2.x);
 }

+/// Inverse of cpvrotate().
 static inline cpVect
 cpvunrotate(const cpVect v1, const cpVect v2)
 {
 	return cpv(v1.x*v2.x + v1.y*v2.y, v1.y*v2.x - v1.x*v2.y);
 }

+/// Returns the squared length of v. Faster than cpvlength() when you only need to compare lengths.
 static inline cpFloat
 cpvlengthsq(const cpVect v)
 {
 	return cpvdot(v, v);
 }

+/// Linearly interpolate between v1 and v2.
 static inline cpVect
 cpvlerp(const cpVect v1, const cpVect v2, const cpFloat t)
 {
 	return cpvadd(cpvmult(v1, 1.0f - t), cpvmult(v2, t));
 }

+/// Returns a normalized copy of v.
 static inline cpVect
 cpvnormalize(const cpVect v)
 {
 	return cpvmult(v, 1.0f/cpvlength(v));
 }

+/// Returns a normalized copy of v or cpvzero if v was already cpvzero. Protects against divide by zero errors.
 static inline cpVect
 cpvnormalize_safe(const cpVect v)
 {
 	return (v.x == 0.0f && v.y == 0.0f ? cpvzero : cpvnormalize(v));
 }

+/// Clamp v to length len.
 static inline cpVect
 cpvclamp(const cpVect v, const cpFloat len)
 {
 	return (cpvdot(v,v) > len*len) ? cpvmult(cpvnormalize(v), len) : v;
 }

+/// Linearly interpolate between v1 towards v2 by distance d.
 static inline cpVect
 cpvlerpconst(cpVect v1, cpVect v2, cpFloat d)
 {
 	return cpvadd(v1, cpvclamp(cpvsub(v2, v1), d));
 }

+/// Returns the distance between v1 and v2.
 static inline cpFloat
 cpvdist(const cpVect v1, const cpVect v2)
 {
 	return cpvlength(cpvsub(v1, v2));
 }

+/// Returns the squared distance between v1 and v2. Faster than cpvdist() when you only need to compare distances.
 static inline cpFloat
 cpvdistsq(const cpVect v1, const cpVect v2)
 {
 	return cpvlengthsq(cpvsub(v1, v2));
 }

-static inline int
+/// Returns true if the distance between v1 and v2 is less than dist.
+static inline cpBool
 cpvnear(const cpVect v1, const cpVect v2, const cpFloat dist)
 {
 	return cpvdistsq(v1, v2) < dist*dist;
--- a/chipmunk/proj.wophone/Makefile.ARM
+++ b/chipmunk/proj.wophone/Makefile.ARM
@ -38,8 +38,11 @@ OBJECTS = \
 	$(OBJECTS_DIR)/cpHashSet.o \
 	$(OBJECTS_DIR)/cpPolyShape.o \
 	$(OBJECTS_DIR)/cpShape.o \
-	$(OBJECTS_DIR)/cpSpace.o \
-	$(OBJECTS_DIR)/cpSpaceHash.o \
+	$(OBJECTS_DIR)/cpSpace.o \
+	$(OBJECTS_DIR)/cpSpaceComponent.o \
+	$(OBJECTS_DIR)/cpSpaceHash.o \
+	$(OBJECTS_DIR)/cpSpaceQuery.o \
+	$(OBJECTS_DIR)/cpSpaceStep.o \
 	$(OBJECTS_DIR)/cpVect.o \
 	$(OBJECTS_DIR)/cpConstraint.o \
 	$(OBJECTS_DIR)/cpDampedRotarySpring.o \
@ -99,9 +102,18 @@ $(OBJECTS_DIR)/cpShape.o : ../src/cpShape.c

 $(OBJECTS_DIR)/cpSpace.o : ../src/cpSpace.c
 	$(CC) -c $(CC_FLAGS) $(INCLUDE_PATH) $(LAST_INCLUDE_PATH) -o $(OBJECTS_DIR)/cpSpace.o ../src/cpSpace.c
+
+$(OBJECTS_DIR)/cpSpace.o : ../src/cpSpaceComponent.c
+	$(CC) -c $(CC_FLAGS) $(INCLUDE_PATH) $(LAST_INCLUDE_PATH) -o $(OBJECTS_DIR)/cpSpace.o ../src/cpSpaceComponent.c

 $(OBJECTS_DIR)/cpSpaceHash.o : ../src/cpSpaceHash.c
 	$(CC) -c $(CC_FLAGS) $(INCLUDE_PATH) $(LAST_INCLUDE_PATH) -o $(OBJECTS_DIR)/cpSpaceHash.o ../src/cpSpaceHash.c
+
+$(OBJECTS_DIR)/cpSpace.o : ../src/cpSpaceQuery.c
+	$(CC) -c $(CC_FLAGS) $(INCLUDE_PATH) $(LAST_INCLUDE_PATH) -o $(OBJECTS_DIR)/cpSpace.o ../src/cpSpaceQuery.c
+
+$(OBJECTS_DIR)/cpSpace.o : ../src/cpSpaceStep.c
+	$(CC) -c $(CC_FLAGS) $(INCLUDE_PATH) $(LAST_INCLUDE_PATH) -o $(OBJECTS_DIR)/cpSpace.o ../src/cpSpaceStep.c

 $(OBJECTS_DIR)/cpVect.o : ../src/cpVect.c
 	$(CC) -c $(CC_FLAGS) $(INCLUDE_PATH) $(LAST_INCLUDE_PATH) -o $(OBJECTS_DIR)/cpVect.o ../src/cpVect.c
--- a/chipmunk/src/CMakeLists.txt
+++ b/chipmunk/src/CMakeLists.txt
@ -9,7 +9,7 @@ if(BUILD_SHARED)
    ${chipmunk_source_files}
  )
  # set the lib's version number
-  set_target_properties(chipmunk PROPERTIES VERSION 5.1)
+  set_target_properties(chipmunk PROPERTIES VERSION 5.3.4)
  install(TARGETS chipmunk RUNTIME DESTINATION lib LIBRARY DESTINATION lib)
 endif(BUILD_SHARED)

--- a/chipmunk/src/chipmunk.c
+++ b/chipmunk/src/chipmunk.c
@ -18,9 +18,11 @@
 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
 * SOFTWARE.
 */
-
+ 
 #include <stdlib.h>
 #include <stdio.h>
+#define _USE_MATH_DEFINES
+#include <math.h>

 #include "chipmunk.h"

@ -43,7 +45,7 @@ cpMessage(const char *message, const char *condition, const char *file, int line
 }


-char *cpVersionString = "5.x.x";
+const char *cpVersionString = "5.3.4";

 void
 cpInitChipmunk(void)
@ -59,7 +61,13 @@ cpInitChipmunk(void)
 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));
+}
+
+cpFloat
+cpAreaForCircle(cpFloat r1, cpFloat r2)
+{
+	return 2.0f*(cpFloat)M_PI*cpfabs(r1*r1 - r2*r2);
 }

 cpFloat
@ -68,21 +76,23 @@ 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);
+	return m*(length*length/12.0f + cpvlengthsq(offset));
 }

 cpFloat
-cpMomentForPoly(cpFloat m, const int numVerts, cpVect *verts, cpVect offset)
+cpAreaForSegment(cpVect a, cpVect b, cpFloat r)
+{
+	return 2.0f*r*((cpFloat)M_PI*r + cpvdist(a, b));
+}
+
+cpFloat
+cpMomentForPoly(cpFloat m, const int numVerts, const cpVect *verts, cpVect offset)
 {
-	cpVect *tVerts = (cpVect *)cpcalloc(numVerts, sizeof(cpVect));
-	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];
+		cpVect v1 = cpvadd(verts[i], offset);
+		cpVect v2 = cpvadd(verts[(i+1)%numVerts], offset);
 		
 		cpFloat a = cpvcross(v2, v1);
 		cpFloat b = cpvdot(v1, v1) + cpvdot(v1, v2) + cpvdot(v2, v2);
@ -91,15 +101,51 @@ cpMomentForPoly(cpFloat m, const int numVerts, cpVect *verts, cpVect offset)
 		sum2 += a;
 	}
 	
-	cpfree(tVerts);
 	return (m*sum1)/(6.0f*sum2);
 }

+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;
+}
+
+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));
+}
+
+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);
+	}
+}
+
 cpFloat
 cpMomentForBox(cpFloat m, cpFloat width, cpFloat height)
 {
-	return m*(width*width + height*height)/12;
+	return m*(width*width + height*height)/12.0f;
 }

-
 #include "chipmunk_ffi.h"
--- a/chipmunk/src/constraints/cpConstraint.c
+++ b/chipmunk/src/constraints/cpConstraint.c
@ -21,7 +21,7 @@

 #include <stdlib.h>

-#include "chipmunk.h"
+#include "chipmunk_private.h"
 #include "constraints/util.h"

 // TODO: Comment me!
--- a/chipmunk/src/constraints/cpDampedRotarySpring.c
+++ b/chipmunk/src/constraints/cpDampedRotarySpring.c
@ -22,7 +22,7 @@
 #include <stdlib.h>
 #include <math.h>

-#include "chipmunk.h"
+#include "chipmunk_private.h"
 #include "constraints/util.h"

 static cpFloat
@ -33,12 +33,12 @@ defaultSpringTorque(cpDampedRotarySpring *spring, cpFloat relativeAngle){
 static void
 preStep(cpDampedRotarySpring *spring, cpFloat dt, cpFloat dt_inv)
 {
-	cpBody *a = spring->constraint.a;
-	cpBody *b = spring->constraint.b;
+	CONSTRAINT_BEGIN(spring, a, b);
 	
-	spring->iSum = 1.0f/(a->i_inv + b->i_inv);
+	cpFloat moment = a->i_inv + b->i_inv;
+	spring->iSum = 1.0f/moment;

-	spring->dt = dt;
+	spring->w_coef = 1.0f - cpfexp(-spring->damping*dt*moment);
 	spring->target_wrn = 0.0f;

 	// apply spring torque
@ -50,15 +50,14 @@ preStep(cpDampedRotarySpring *spring, cpFloat dt, cpFloat dt_inv)
 static void
 applyImpulse(cpDampedRotarySpring *spring)
 {
-	cpBody *a = spring->constraint.a;
-	cpBody *b = spring->constraint.b;
+	CONSTRAINT_BEGIN(spring, a, b);
 	
 	// compute relative velocity
 	cpFloat wrn = a->w - b->w;//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 w_damp = wrn*(1.0f - cpfexp(-spring->damping*spring->dt/spring->iSum));
+	cpFloat w_damp = wrn*spring->w_coef;
 	spring->target_wrn = wrn - w_damp;
 	
 	//apply_impulses(a, b, spring->r1, spring->r2, cpvmult(spring->n, v_damp*spring->nMass));
--- a/chipmunk/src/constraints/cpDampedSpring.c
+++ b/chipmunk/src/constraints/cpDampedSpring.c
@ -22,7 +22,7 @@
 #include <stdlib.h>
 #include <math.h>

-#include "chipmunk.h"
+#include "chipmunk_private.h"
 #include "constraints/util.h"

 static cpFloat
@ -33,8 +33,7 @@ defaultSpringForce(cpDampedSpring *spring, cpFloat dist){
 static void
 preStep(cpDampedSpring *spring, cpFloat dt, cpFloat dt_inv)
 {
-	cpBody *a = spring->constraint.a;
-	cpBody *b = spring->constraint.b;
+	CONSTRAINT_BEGIN(spring, a, b);
 	
 	spring->r1 = cpvrotate(spring->anchr1, a->rot);
 	spring->r2 = cpvrotate(spring->anchr2, b->rot);
@ -43,11 +42,11 @@ preStep(cpDampedSpring *spring, cpFloat dt, cpFloat dt_inv)
 	cpFloat dist = cpvlength(delta);
 	spring->n = cpvmult(delta, 1.0f/(dist ? dist : INFINITY));
 	
-	// calculate mass normal
-	spring->nMass = 1.0f/k_scalar(a, b, spring->r1, spring->r2, spring->n);
-
-	spring->dt = dt;
+	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((cpConstraint *)spring, dist);
@ -57,8 +56,7 @@ preStep(cpDampedSpring *spring, cpFloat dt, cpFloat dt_inv)
 static void
 applyImpulse(cpDampedSpring *spring)
 {
-	cpBody *a = spring->constraint.a;
-	cpBody *b = spring->constraint.b;
+	CONSTRAINT_BEGIN(spring, a, b);
 	
 	cpVect n = spring->n;
 	cpVect r1 = spring->r1;
@ -69,7 +67,7 @@ applyImpulse(cpDampedSpring *spring)
 	
 	// compute velocity loss from drag
 	// not 100% certain this is derived correctly, though it makes sense
-	cpFloat v_damp = -vrn*(1.0f - cpfexp(-spring->damping*spring->dt/spring->nMass));
+	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));
--- a/chipmunk/src/constraints/cpGearJoint.c
+++ b/chipmunk/src/constraints/cpGearJoint.c
@ -21,14 +21,13 @@

 #include <stdlib.h>

-#include "chipmunk.h"
+#include "chipmunk_private.h"
 #include "constraints/util.h"

 static void
 preStep(cpGearJoint *joint, cpFloat dt, cpFloat dt_inv)
 {
-	cpBody *a = joint->constraint.a;
-	cpBody *b = joint->constraint.b;
+	CONSTRAINT_BEGIN(joint, a, b);
 	
 	// calculate moment of inertia coefficient.
 	joint->iSum = 1.0f/(a->i_inv*joint->ratio_inv + joint->ratio*b->i_inv);
@ -49,8 +48,7 @@ preStep(cpGearJoint *joint, cpFloat dt, cpFloat dt_inv)
 static void
 applyImpulse(cpGearJoint *joint)
 {
-	cpBody *a = joint->constraint.a;
-	cpBody *b = joint->constraint.b;
+	CONSTRAINT_BEGIN(joint, a, b);
 	
 	// compute relative rotational velocity
 	cpFloat wr = b->w*joint->ratio - a->w;
@ -111,4 +109,5 @@ cpGearJointSetRatio(cpConstraint *constraint, cpFloat value)
 	cpConstraintCheckCast(constraint, cpGearJoint);
 	((cpGearJoint *)constraint)->ratio = value;
 	((cpGearJoint *)constraint)->ratio_inv = 1.0f/value;
+	cpConstraintActivateBodies(constraint);
 }
--- a/chipmunk/src/constraints/cpGrooveJoint.c
+++ b/chipmunk/src/constraints/cpGrooveJoint.c
@ -21,14 +21,13 @@

 #include <stdlib.h>

-#include "chipmunk.h"
+#include "chipmunk_private.h"
 #include "constraints/util.h"

 static void
 preStep(cpGrooveJoint *joint, cpFloat dt, cpFloat dt_inv)
 {
-	cpBody *a = joint->constraint.a;
-	cpBody *b = joint->constraint.b;
+	CONSTRAINT_BEGIN(joint, a, b);
 	
 	// calculate endpoints in worldspace
 	cpVect ta = cpBodyLocal2World(a, joint->grv_a);
@ -79,8 +78,7 @@ grooveConstrain(cpGrooveJoint *joint, cpVect j){
 static void
 applyImpulse(cpGrooveJoint *joint)
 {
-	cpBody *a = joint->constraint.a;
-	cpBody *b = joint->constraint.b;
+	CONSTRAINT_BEGIN(joint, a, b);
 	
 	cpVect r1 = joint->r1;
 	cpVect r2 = joint->r2;
@ -136,3 +134,28 @@ cpGrooveJointNew(cpBody *a, cpBody *b, cpVect groove_a, cpVect groove_b, cpVect
 {
 	return (cpConstraint *)cpGrooveJointInit(cpGrooveJointAlloc(), a, b, groove_a, groove_b, anchr2);
 }
+
+void
+cpGrooveJointSetGrooveA(cpConstraint *constraint, cpVect value)
+{
+	cpGrooveJoint *g = (cpGrooveJoint *)constraint;
+	cpConstraintCheckCast(constraint, cpGrooveJoint);
+	
+	g->grv_a = value;
+	g->grv_n = cpvperp(cpvnormalize(cpvsub(g->grv_b, value)));
+	
+	cpConstraintActivateBodies(constraint);
+}
+
+void
+cpGrooveJointSetGrooveB(cpConstraint *constraint, cpVect value)
+{
+	cpGrooveJoint *g = (cpGrooveJoint *)constraint;
+	cpConstraintCheckCast(constraint, cpGrooveJoint);
+	
+	g->grv_b = value;
+	g->grv_n = cpvperp(cpvnormalize(cpvsub(value, g->grv_a)));
+	
+	cpConstraintActivateBodies(constraint);
+}
+
--- a/chipmunk/src/constraints/cpPinJoint.c
+++ b/chipmunk/src/constraints/cpPinJoint.c
@ -22,14 +22,13 @@
 #include <stdlib.h>
 //#include <math.h>

-#include "chipmunk.h"
+#include "chipmunk_private.h"
 #include "constraints/util.h"

 static void
 preStep(cpPinJoint *joint, cpFloat dt, cpFloat dt_inv)
 {
-	cpBody *a = joint->constraint.a;
-	cpBody *b = joint->constraint.b;
+	CONSTRAINT_BEGIN(joint, a, b);
 	
 	joint->r1 = cpvrotate(joint->anchr1, a->rot);
 	joint->r2 = cpvrotate(joint->anchr2, b->rot);
@ -56,8 +55,7 @@ preStep(cpPinJoint *joint, cpFloat dt, cpFloat dt_inv)
 static void
 applyImpulse(cpPinJoint *joint)
 {
-	cpBody *a = joint->constraint.a;
-	cpBody *b = joint->constraint.b;
+	CONSTRAINT_BEGIN(joint, a, b);
 	cpVect n = joint->n;

 	// compute relative velocity
@ -101,8 +99,9 @@ cpPinJointInit(cpPinJoint *joint, cpBody *a, cpBody *b, cpVect anchr1, cpVect an
 	joint->anchr1 = anchr1;
 	joint->anchr2 = anchr2;
 	
-	cpVect p1 = cpvadd(a->p, cpvrotate(anchr1, a->rot));
-	cpVect p2 = cpvadd(b->p, cpvrotate(anchr2, b->rot));
+	// 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;
--- a/chipmunk/src/constraints/cpPivotJoint.c
+++ b/chipmunk/src/constraints/cpPivotJoint.c
@ -21,14 +21,13 @@

 #include <stdlib.h>

-#include "chipmunk.h"
+#include "chipmunk_private.h"
 #include "constraints/util.h"

 static void
 preStep(cpPivotJoint *joint, cpFloat dt, cpFloat dt_inv)
 {
-	cpBody *a = joint->constraint.a;
-	cpBody *b = joint->constraint.b;
+	CONSTRAINT_BEGIN(joint, a, b);
 	
 	joint->r1 = cpvrotate(joint->anchr1, a->rot);
 	joint->r2 = cpvrotate(joint->anchr2, b->rot);
@ -50,8 +49,7 @@ preStep(cpPivotJoint *joint, cpFloat dt, cpFloat dt_inv)
 static void
 applyImpulse(cpPivotJoint *joint)
 {
-	cpBody *a = joint->constraint.a;
-	cpBody *b = joint->constraint.b;
+	CONSTRAINT_BEGIN(joint, a, b);
 	
 	cpVect r1 = joint->r1;
 	cpVect r2 = joint->r2;
@ -110,5 +108,7 @@ cpPivotJointNew2(cpBody *a, cpBody *b, cpVect anchr1, cpVect anchr2)
 cpConstraint *
 cpPivotJointNew(cpBody *a, cpBody *b, cpVect pivot)
 {
-	return cpPivotJointNew2(a, b, cpBodyWorld2Local(a, pivot), cpBodyWorld2Local(b, pivot));
+	cpVect anchr1 = (a ? cpBodyWorld2Local(a, pivot) : pivot);
+	cpVect anchr2 = (b ? cpBodyWorld2Local(b, pivot) : pivot);
+	return cpPivotJointNew2(a, b, anchr1, anchr2);
 }
--- a/chipmunk/src/constraints/cpRatchetJoint.c
+++ b/chipmunk/src/constraints/cpRatchetJoint.c
@ -19,18 +19,16 @@
 * SOFTWARE.
 */

-#include <stdio.h>
 #include <stdlib.h>
 #include <math.h>

-#include "chipmunk.h"
+#include "chipmunk_private.h"
 #include "constraints/util.h"

 static void
 preStep(cpRatchetJoint *joint, cpFloat dt, cpFloat dt_inv)
 {
-	cpBody *a = joint->constraint.a;
-	cpBody *b = joint->constraint.b;
+	CONSTRAINT_BEGIN(joint, a, b);
 	
 	cpFloat angle = joint->angle;
 	cpFloat phase = joint->phase;
@ -70,8 +68,7 @@ applyImpulse(cpRatchetJoint *joint)
 {
 	if(!joint->bias) return; // early exit

-	cpBody *a = joint->constraint.a;
-	cpBody *b = joint->constraint.b;
+	CONSTRAINT_BEGIN(joint, a, b);
 	
 	// compute relative rotational velocity
 	cpFloat wr = b->w - a->w;
@ -116,7 +113,8 @@ cpRatchetJointInit(cpRatchetJoint *joint, cpBody *a, cpBody *b, cpFloat phase, c
 	joint->phase = phase;
 	joint->ratchet = ratchet;
 	
-	joint->angle = b->a - a->a;
+	// STATIC_BODY_CHECK
+	joint->angle = (b ? b->a : 0.0f) - (a ? a->a : 0.0f);
 	
 	return joint;
 }
--- a/chipmunk/src/constraints/cpRotaryLimitJoint.c
+++ b/chipmunk/src/constraints/cpRotaryLimitJoint.c
@ -21,14 +21,13 @@

 #include <stdlib.h>

-#include "chipmunk.h"
+#include "chipmunk_private.h"
 #include "constraints/util.h"

 static void
 preStep(cpRotaryLimitJoint *joint, cpFloat dt, cpFloat dt_inv)
 {
-	cpBody *a = joint->constraint.a;
-	cpBody *b = joint->constraint.b;
+	CONSTRAINT_BEGIN(joint, a, b);
 	
 	cpFloat dist = b->a - a->a;
 	cpFloat pdist = 0.0f;
@ -62,8 +61,7 @@ applyImpulse(cpRotaryLimitJoint *joint)
 {
 	if(!joint->bias) return; // early exit

-	cpBody *a = joint->constraint.a;
-	cpBody *b = joint->constraint.b;
+	CONSTRAINT_BEGIN(joint, a, b);
 	
 	// compute relative rotational velocity
 	cpFloat wr = b->w - a->w;
--- a/chipmunk/src/constraints/cpSimpleMotor.c
+++ b/chipmunk/src/constraints/cpSimpleMotor.c
@ -21,14 +21,13 @@

 #include <stdlib.h>

-#include "chipmunk.h"
+#include "chipmunk_private.h"
 #include "constraints/util.h"

 static void
 preStep(cpSimpleMotor *joint, cpFloat dt, cpFloat dt_inv)
 {
-	cpBody *a = joint->constraint.a;
-	cpBody *b = joint->constraint.b;
+	CONSTRAINT_BEGIN(joint, a, b);
 	
 	// calculate moment of inertia coefficient.
 	joint->iSum = 1.0f/(a->i_inv + b->i_inv);
@ -44,8 +43,7 @@ preStep(cpSimpleMotor *joint, cpFloat dt, cpFloat dt_inv)
 static void
 applyImpulse(cpSimpleMotor *joint)
 {
-	cpBody *a = joint->constraint.a;
-	cpBody *b = joint->constraint.b;
+	CONSTRAINT_BEGIN(joint, a, b);
 	
 	// compute relative rotational velocity
 	cpFloat wr = b->w - a->w + joint->rate;
--- a/chipmunk/src/constraints/cpSlideJoint.c
+++ b/chipmunk/src/constraints/cpSlideJoint.c
@ -21,14 +21,13 @@

 #include <stdlib.h>

-#include "chipmunk.h"
+#include "chipmunk_private.h"
 #include "constraints/util.h"

 static void
 preStep(cpSlideJoint *joint, cpFloat dt, cpFloat dt_inv)
 {
-	cpBody *a = joint->constraint.a;
-	cpBody *b = joint->constraint.b;
+	CONSTRAINT_BEGIN(joint, a, b);
 	
 	joint->r1 = cpvrotate(joint->anchr1, a->rot);
 	joint->r2 = cpvrotate(joint->anchr2, b->rot);
@ -69,8 +68,7 @@ applyImpulse(cpSlideJoint *joint)
 {
 	if(!joint->bias) return;  // early exit

-	cpBody *a = joint->constraint.a;
-	cpBody *b = joint->constraint.b;
+	CONSTRAINT_BEGIN(joint, a, b);
 	
 	cpVect n = joint->n;
 	cpVect r1 = joint->r1;
--- a/chipmunk/src/cpArbiter.c
+++ b/chipmunk/src/cpArbiter.c
@ -21,7 +21,7 @@
 
 #include <stdlib.h>

-#include "chipmunk.h"
+#include "chipmunk_private.h"
 #include "constraints/util.h"

 cpFloat cp_bias_coef = 0.1f;
@ -104,13 +104,20 @@ cpArbiterAlloc(void)
 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 = -1;
+	arb->stamp = 0;
 	arb->state = cpArbiterStateFirstColl;
 	
 	return arb;
@ -157,8 +164,6 @@ cpArbiterUpdate(cpArbiter *arb, cpContact *contacts, int numContacts, cpCollisio
 				}
 			}
 		}
-
-//		cpfree(arb->contacts);
 	}
 	
 	arb->contacts = contacts;
@ -172,17 +177,18 @@ cpArbiterUpdate(cpArbiter *arb, cpContact *contacts, int numContacts, cpCollisio
 	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 == cpArbiterStateCached) arb->state = cpArbiterStateFirstColl;
 }

 void
 cpArbiterPreStep(cpArbiter *arb, cpFloat dt_inv)
 {
-	cpShape *shapea = arb->private_a;
-	cpShape *shapeb = arb->private_b;
-
-	cpBody *a = shapea->body;
-	cpBody *b = shapeb->body;
+	cpBody *a = arb->a->body;
+	cpBody *b = arb->b->body;
 	
 	for(int i=0; i<arb->numContacts; i++){
 		cpContact *con = &arb->contacts[i];
@ -207,8 +213,8 @@ cpArbiterPreStep(cpArbiter *arb, cpFloat dt_inv)
 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);
@ -225,8 +231,8 @@ cpArbiterApplyCachedImpulse(cpArbiter *arb)
 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/chipmunk/src/cpArray.c
+++ b/chipmunk/src/cpArray.c
@ -22,7 +22,7 @@
 #include <stdlib.h>
 #include <string.h>

-#include "chipmunk.h"
+#include "chipmunk_private.h"


 //#define CP_ARRAY_INCREMENT 10
@ -57,6 +57,7 @@ void
 cpArrayDestroy(cpArray *arr)
 {
 	cpfree(arr->arr);
+	arr->arr = NULL;
 }

 void
@ -111,6 +112,20 @@ cpArrayDeleteObj(cpArray *arr, void *obj)
 	}
 }

+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 = (void **)cprealloc(arr->arr, arr->max*sizeof(void**));
+	}
+	
+	memcpy(tail, other->arr, other->num*sizeof(void**));
+}
+
 void
 cpArrayEach(cpArray *arr, cpArrayIter iterFunc, void *data)
 {
@ -118,11 +133,11 @@ cpArrayEach(cpArray *arr, cpArrayIter iterFunc, void *data)
 		iterFunc(arr->arr[i], data);
 }

-int
+cpBool
 cpArrayContains(cpArray *arr, void *ptr)
 {
 	for(int i=0; i<arr->num; i++)
-		if(arr->arr[i] == ptr) return 1;
+		if(arr->arr[i] == ptr) return cpTrue;
 	
-	return 0;
+	return cpFalse;
 }
--- a/chipmunk/src/cpBody.c
+++ b/chipmunk/src/cpBody.c
@ -21,8 +21,12 @@
 
 #include <stdlib.h>
 #include <float.h>
+#include <stdarg.h>

-#include "chipmunk.h"
+#include "chipmunk_private.h"
+
+// initialized in cpInitChipmunk()
+cpBody cpStaticBodySingleton;

 cpBody*
 cpBodyAlloc(void)
@ -56,8 +60,13 @@ cpBodyInit(cpBody *body, cpFloat m, cpFloat i)
 	body->data = NULL;
 	body->v_limit = (cpFloat)INFINITY;
 	body->w_limit = (cpFloat)INFINITY;
-//	body->active = 1;
-
+	
+	body->space = NULL;
+	body->shapesList = NULL;
+	
+	cpComponentNode node = {NULL, NULL, 0, 0.0f};
+	body->node = node;
+	
 	return body;
 }

@ -67,6 +76,21 @@ cpBodyNew(cpFloat m, cpFloat i)
 	return cpBodyInit(cpBodyAlloc(), m, i);
 }

+cpBody *
+cpBodyInitStatic(cpBody *body)
+{
+	cpBodyInit(body, (cpFloat)INFINITY, (cpFloat)INFINITY);
+	body->node.idleTime = (cpFloat)INFINITY;
+	
+	return body;
+}
+
+cpBody *
+cpBodyNewStatic()
+{
+	return cpBodyInitStatic(cpBodyAlloc());
+}
+
 void cpBodyDestroy(cpBody *body){}

 void
@ -166,23 +190,3 @@ cpApplyDampedSpring(cpBody *a, cpBody *b, cpVect anchr1, cpVect anchr2, cpFloat
 	cpBodyApplyForce(a, f, r1);
 	cpBodyApplyForce(b, cpvneg(f), r2);
 }
-
-//int
-//cpBodyMarkLowEnergy(cpBody *body, cpFloat dvsq, int max)
-//{
-//	cpFloat ke = body->m*cpvdot(body->v, body->v);
-//	cpFloat re = body->i*body->w*body->w;
-//	
-//	if(ke + re > body->m*dvsq)
-//		body->active = 1;
-//	else if(body->active)
-//		body->active = (body->active + 1)%(max + 1);
-//	else {
-//		body->v = cpvzero;
-//		body->v_bias = cpvzero;
-//		body->w = 0.0f;
-//		body->w_bias = 0.0f;
-//	}
-//	
-//	return body->active;
-//}
--- a/chipmunk/src/cpCollision.c
+++ b/chipmunk/src/cpCollision.c
@ -21,16 +21,16 @@
 
 #include <stdlib.h>
 #include <math.h>
-#include <stdio.h>
+//#include <stdio.h>

-#include "chipmunk.h"
+#include "chipmunk_private.h"

-typedef int (*collisionFunc)(cpShape *, cpShape *, cpContact *);
+typedef int (*collisionFunc)(const cpShape *, const cpShape *, cpContact *);

 // Add contact points for circle to circle collisions.
 // Used by several collision tests.
 static int
-circle2circleQuery(cpVect p1, cpVect p2, cpFloat r1, cpFloat r2, cpContact *con)
+circle2circleQuery(const cpVect p1, const cpVect p2, const cpFloat r1, const cpFloat r2, cpContact *con)
 {
 	cpFloat mindist = r1 + r2;
 	cpVect delta = cpvsub(p2, p1);
@ -38,14 +38,12 @@ circle2circleQuery(cpVect p1, cpVect p2, cpFloat r1, cpFloat r2, cpContact *con)
 	if(distsq >= mindist*mindist) return 0;
 	
 	cpFloat dist = cpfsqrt(distsq);
-	// To avoid singularities, do nothing in the case of dist = 0.
-	cpFloat non_zero_dist = (dist ? dist : INFINITY);

 	// Allocate and initialize the contact.
 	cpContactInit(
 		con,
-		cpvadd(p1, cpvmult(delta, 0.5f + (r1 - 0.5f*mindist)/non_zero_dist)),
-		cpvmult(delta, 1.0f/non_zero_dist),
+		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
 	);
@ -55,7 +53,7 @@ circle2circleQuery(cpVect p1, cpVect p2, cpFloat r1, cpFloat r2, cpContact *con)

 // Collide circle shapes.
 static int
-circle2circle(cpShape *shape1, cpShape *shape2, cpContact *arr)
+circle2circle(const cpShape *shape1, const cpShape *shape2, cpContact *arr)
 {
 	cpCircleShape *circ1 = (cpCircleShape *)shape1;
 	cpCircleShape *circ2 = (cpCircleShape *)shape2;
@ -65,7 +63,7 @@ circle2circle(cpShape *shape1, cpShape *shape2, cpContact *arr)

 // Collide circles to segment shapes.
 static int
-circle2segment(cpShape *circleShape, cpShape *segmentShape, cpContact *con)
+circle2segment(const cpShape *circleShape, const cpShape *segmentShape, cpContact *con)
 {
 	cpCircleShape *circ = (cpCircleShape *)circleShape;
 	cpSegmentShape *seg = (cpSegmentShape *)segmentShape;
@ -118,17 +116,19 @@ circle2segment(cpShape *circleShape, cpShape *segmentShape, cpContact *con)
 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.
 static inline int
-findMSA(cpPolyShape *poly, cpPolyShapeAxis *axes, int num, cpFloat *min_out)
+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);
@ -148,9 +148,11 @@ findMSA(cpPolyShape *poly, cpPolyShapeAxis *axes, int num, cpFloat *min_out)
 	return min_index;
 }

-// Add contacts for penetrating vertexes.
+// 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 inline int
-findVerts(cpContact *arr, cpPolyShape *poly1, cpPolyShape *poly2, cpVect n, cpFloat dist)
+findVertsFallback(cpContact *arr, const cpPolyShape *poly1, const cpPolyShape *poly2, const cpVect n, const cpFloat dist)
 {
 	int num = 0;
 	
@ -166,15 +168,33 @@ findVerts(cpContact *arr, cpPolyShape *poly1, cpPolyShape *poly2, cpVect n, cpFl
 			cpContactInit(nextContactPoint(arr, &num), v, n, dist, CP_HASH_PAIR(poly2->shape.hashid, i));
 	}
 	
-	//	if(!num)
-	//		addContactPoint(arr, &size, &num, cpContactNew(shape1->body->p, n, dist, 0));
-
 	return num;
 }

+// Add contacts for penetrating vertexes.
+static inline 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, 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, i));
+	}
+	
+	return (num ? num : findVertsFallback(arr, poly1, poly2, n, dist));
+}
+
 // Collide poly shapes together.
 static int
-poly2poly(cpShape *shape1, cpShape *shape2, cpContact *arr)
+poly2poly(const cpShape *shape1, const cpShape *shape2, cpContact *arr)
 {
 	cpPolyShape *poly1 = (cpPolyShape *)shape1;
 	cpPolyShape *poly2 = (cpPolyShape *)shape2;
@ -196,7 +216,7 @@ poly2poly(cpShape *shape1, cpShape *shape2, cpContact *arr)

 // Like cpPolyValueOnAxis(), but for segments.
 static inline cpFloat
-segValueOnAxis(cpSegmentShape *seg, cpVect n, cpFloat d)
+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;
@ -205,7 +225,7 @@ segValueOnAxis(cpSegmentShape *seg, cpVect n, cpFloat d)

 // Identify vertexes that have penetrated the segment.
 static inline void
-findPointsBehindSeg(cpContact *arr, int *num, cpSegmentShape *seg, cpPolyShape *poly, cpFloat pDist, cpFloat coef) 
+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);
@ -225,7 +245,7 @@ findPointsBehindSeg(cpContact *arr, int *num, cpSegmentShape *seg, cpPolyShape *
 // This one is complicated and gross. Just don't go there...
 // TODO: Comment me!
 static int
-seg2poly(cpShape *shape1, cpShape *shape2, cpContact *arr)
+seg2poly(const cpShape *shape1, const cpShape *shape2, cpContact *arr)
 {
 	cpSegmentShape *seg = (cpSegmentShape *)shape1;
 	cpPolyShape *poly = (cpPolyShape *)shape2;
@ -294,7 +314,7 @@ seg2poly(cpShape *shape1, cpShape *shape2, cpContact *arr)
 // This one is less gross, but still gross.
 // TODO: Comment me!
 static int
-circle2poly(cpShape *shape1, cpShape *shape2, cpContact *con)
+circle2poly(const cpShape *shape1, const cpShape *shape2, cpContact *con)
 {
 	cpCircleShape *circ = (cpCircleShape *)shape1;
 	cpPolyShape *poly = (cpPolyShape *)shape2;
@ -352,7 +372,7 @@ circle2poly(cpShape *shape1, cpShape *shape2, cpContact *con)
 static collisionFunc *colfuncs = NULL;

 static void
-addColFunc(cpShapeType a, cpShapeType b, collisionFunc func)
+addColFunc(const cpShapeType a, const cpShapeType b, const collisionFunc func)
 {
 	colfuncs[a + b*CP_NUM_SHAPES] = func;
 }
@ -381,7 +401,7 @@ extern "C" {
 #endif

 int
-cpCollideShapes(cpShape *a, cpShape *b, cpContact *arr)
+cpCollideShapes(const cpShape *a, const cpShape *b, cpContact *arr)
 {
 	// Their shape types must be in order.
 	cpAssert(a->klass->type <= b->klass->type, "Collision shapes passed to cpCollideShapes() are not sorted.");
--- a/chipmunk/src/cpHashSet.c
+++ b/chipmunk/src/cpHashSet.c
@ -22,7 +22,7 @@
 #include <stdlib.h>
 #include <assert.h>

-#include "chipmunk.h"
+#include "chipmunk_private.h"
 #include "prime.h"

 static void freeWrap(void *ptr, void *unused){cpfree(ptr);}
--- a/chipmunk/src/cpPolyShape.c
+++ b/chipmunk/src/cpPolyShape.c
@ -20,9 +20,8 @@
 */
 
 #include <stdlib.h>
-#include <stdio.h>

-#include "chipmunk.h"
+#include "chipmunk_private.h"
 #include "chipmunk_unsafe.h"

 cpPolyShape *
@ -94,7 +93,7 @@ cpPolyShapeDestroy(cpShape *shape)
 	cpfree(poly->tAxes);
 }

-static int
+static cpBool
 cpPolyShapePointQuery(cpShape *shape, cpVect p){
 	return cpBBcontainsVect(shape->bb, p) && cpPolyShapeContainsVert((cpPolyShape *)shape, p);
 }
@ -137,8 +136,8 @@ static const cpShapeClass polyClass = {
 	cpPolyShapeSegmentQuery,
 };

-int
-cpPolyValidate(cpVect *verts, int numVerts)
+cpBool
+cpPolyValidate(const cpVect *verts, const int numVerts)
 {
 	for(int i=0; i<numVerts; i++){
 		cpVect a = verts[i];
@ -146,10 +145,10 @@ cpPolyValidate(cpVect *verts, int numVerts)
 		cpVect c = verts[(i+2)%numVerts];
 		
 		if(cpvcross(cpvsub(b, a), cpvsub(c, b)) > 0.0f)
-			return 0;
+			return cpFalse;
 	}
 	
-	return 1;
+	return cpTrue;
 }

 int
@ -211,8 +210,8 @@ cpPolyShapeNew(cpBody *body, int numVerts, cpVect *verts, cpVect offset)
 cpPolyShape *
 cpBoxShapeInit(cpPolyShape *poly, cpBody *body, cpFloat width, cpFloat height)
 {
-	cpFloat hw = width/2;
-	cpFloat hh = height/2;
+	cpFloat hw = width/2.0f;
+	cpFloat hh = height/2.0f;
 	
 	cpVect verts[] = {
 		cpv(-hw,-hh),
--- a/chipmunk/src/cpShape.c
+++ b/chipmunk/src/cpShape.c
@ -23,7 +23,7 @@
 #include <stdio.h>
 #include <math.h>

-#include "chipmunk.h"
+#include "chipmunk_private.h"
 #include "chipmunk_unsafe.h"

 #define CP_DefineShapeGetter(struct, type, member, name) \
@ -60,8 +60,9 @@ cpShapeInit(cpShape *shape, const cpShapeClass *klass, cpBody *body)
 	shape->layers = CP_ALL_LAYERS;
 	
 	shape->data = NULL;
+	shape->next = NULL;
 	
-	cpShapeCacheBB(shape);
+//	cpShapeCacheBB(shape);
 	
 	return shape;
 }
@ -81,6 +82,7 @@ cpShapeFree(cpShape *shape)
 	}
 }

+// TODO this function should really take a position and rotation explicitly and be renamed
 cpBB
 cpShapeCacheBB(cpShape *shape)
 {
@ -90,12 +92,12 @@ cpShapeCacheBB(cpShape *shape)
 	return shape->bb;
 }

-int
+cpBool
 cpShapePointQuery(cpShape *shape, cpVect p){
 	return shape->klass->pointQuery(shape, p);
 }

-int
+cpBool
 cpShapeSegmentQuery(cpShape *shape, cpVect a, cpVect b, cpSegmentQueryInfo *info){
 	cpSegmentQueryInfo blank = {NULL, 0.0f, cpvzero};
 	(*info) = blank;
@ -138,7 +140,7 @@ cpCircleShapeCacheData(cpShape *shape, cpVect p, cpVect rot)
 	return bbFromCircle(circle->tc, circle->r);
 }

-static int
+static cpBool
 cpCircleShapePointQuery(cpShape *shape, cpVect p){
 	cpCircleShape *circle = (cpCircleShape *)shape;
 	return cpvnear(circle->tc, p, circle->r);
@ -147,7 +149,7 @@ cpCircleShapePointQuery(cpShape *shape, cpVect p){
 static void
 circleSegmentQuery(cpShape *shape, cpVect center, cpFloat r, cpVect a, cpVect b, cpSegmentQueryInfo *info)
 {
-	// umm... gross I normally frown upon such things
+	// offset the line to be relative to the circle
 	a = cpvsub(a, center);
 	b = cpvsub(b, center);
 	
@ -239,16 +241,16 @@ cpSegmentShapeCacheData(cpShape *shape, cpVect p, cpVect rot)
 	return cpBBNew(l - rad, s - rad, r + rad, t + rad);
 }

-static int
+static cpBool
 cpSegmentShapePointQuery(cpShape *shape, cpVect p){
-	if(!cpBBcontainsVect(shape->bb, p)) return 0;
+	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 0;
+	if(dist > 0.0f) return cpFalse;
 	
 	// Calculate tangential distance along segment.
 	cpFloat dt = -cpvcross(seg->tn, p);
@ -258,28 +260,32 @@ cpSegmentShapePointQuery(cpShape *shape, cpVect p){
 	// Decision tree to decide which feature of the segment to collide with.
 	if(dt <= dtMin){
 		if(dt < (dtMin - seg->r)){
-			return 0;
+			return cpFalse;
 		} else {
 			return cpvlengthsq(cpvsub(seg->ta, p)) < (seg->r*seg->r);
 		}
 	} else {
 		if(dt < dtMax){
-			return 1;
+			return cpTrue;
 		} else {
 			if(dt < (dtMax + seg->r)) {
 				return cpvlengthsq(cpvsub(seg->tb, p)) < (seg->r*seg->r);
 			} else {
-				return 0;
+				return cpFalse;
 			}
 		}
 	}
 	
-	return 1;	
+	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
@ -288,40 +294,37 @@ cpSegmentShapeSegmentQuery(cpShape *shape, cpVect a, cpVect b, cpSegmentQueryInf
 	
 	cpFloat an = cpvdot(a, n);
 	cpFloat bn = cpvdot(b, n);
-	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(an != bn){
+		cpFloat d = cpvdot(seg->ta, n) + seg->r;
+		cpFloat t = (d - an)/(bn - an);
 		
-		if(dtMin < dt && dt < dtMax){
-			info->shape = shape;
-			info->t = t;
-			info->n = n;
+		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);
 			
-			return; // don't continue on and check endcaps
+			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; info1.shape = NULL;
-		cpSegmentQueryInfo info2; info2.shape = NULL;
+		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.shape && !info2.shape){
+		if(info1.t < info2.t){
 			(*info) = info1;
-		} else if(info2.shape && !info1.shape){
+		} else {
 			(*info) = info2;
-		} else if(info1.shape && info2.shape){
-			if(info1.t < info2.t){
-				(*info) = info1;
-			} else {
-				(*info) = info2;
-			}
 		}
 	}
 }
--- a/chipmunk/src/cpSpace.c
+++ b/chipmunk/src/cpSpace.c
@ -20,24 +20,24 @@
 */
 
 #include <stdlib.h>
-#include <stdio.h>
+//#include <stdio.h>
 #include <string.h>
 #include <math.h>

-#include "chipmunk.h"
+#include "chipmunk_private.h"

-int cp_contact_persistence = 1;
+cpTimestamp cp_contact_persistence = 3;

 #pragma mark Contact Set Helpers

 // Equal function for contactSet.
-static int
+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));
+	return ((a == arb->a && b == arb->b) || (b == arb->a && a == arb->b));
 }

 // Transformation function for contactSet.
@ -55,13 +55,13 @@ contactSetTrans(cpShape **shapes, cpSpace *space)
 		for(int i=0; i<count; i++) cpArrayPush(space->pooledArbiters, buffer + i);
 	}
 	
-	return cpArbiterInit( (cpArbiter *)cpArrayPop(space->pooledArbiters), shapes[0], shapes[1]);
+	return cpArbiterInit((cpArbiter *) cpArrayPop(space->pooledArbiters), shapes[0], shapes[1]);
 }

 #pragma mark Collision Pair Function Helpers

 // Equals function for collFuncSet.
-static int
+static cpBool
 collFuncSetEql(cpCollisionHandler *check, cpCollisionHandler *pair)
 {
 	return ((check->a == pair->a && check->b == pair->b) || (check->b == pair->a && check->a == pair->b));
@ -77,32 +77,10 @@ collFuncSetTrans(cpCollisionHandler *handler, void *unused)
 	return copy;
 }

-#pragma mark Post Step Function Helpers
-
-typedef struct postStepCallback {
-	cpPostStepFunc func;
-	void *obj;
-	void *data;
-} postStepCallback;
-
-static int
-postStepFuncSetEql(postStepCallback *a, postStepCallback *b){
-	return a->obj == b->obj;
-}
-
-static void *
-postStepFuncSetTrans(postStepCallback *callback, void *ignored)
-{
-	postStepCallback *value = (postStepCallback *)cpmalloc(sizeof(postStepCallback));
-	(*value) = (*callback);
-	
-	return value;
-}
-
 #pragma mark Misc Helper Funcs

 // Default collision functions.
-static int alwaysCollide(cpArbiter *arb, cpSpace *space, void *data){return 1;}
+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.
@ -116,30 +94,6 @@ static void   constraintFreeWrap(cpConstraint *ptr, void *unused){cpConstraintFr

 #pragma mark Memory Management Functions

-#define CP_CONTACTS_BUFFER_SIZE ((CP_BUFFER_BYTES - sizeof(cpContactBufferHeader))/sizeof(cpContact))
-typedef struct cpContactBuffer {
-	cpContactBufferHeader header;
-	cpContact contacts[CP_CONTACTS_BUFFER_SIZE];
-} cpContactBuffer;
-
-static cpContactBufferHeader *
-cpSpaceAllocContactBuffer(cpSpace *space)
-{
-	cpContactBuffer *buffer = (cpContactBuffer *)malloc(sizeof(cpContactBuffer));
-	cpArrayPush(space->allocatedBuffers, buffer);
-	return (cpContactBufferHeader *)buffer;
-}
-
-static cpContactBufferHeader *
-cpContactBufferHeaderInit(cpContactBufferHeader *header, cpSpace *space)
-{
-	header->stamp = space->stamp;
-	header->next = space->contactBuffersTail;
-	header->numContacts = 0;
-	
-	return header;
-}
-
 cpSpace *
 cpSpaceAlloc(void)
 {
@ -151,8 +105,6 @@ cpSpaceAlloc(void)
 #define DEFAULT_ITERATIONS 10
 #define DEFAULT_ELASTIC_ITERATIONS 0

-#define MAX_CONTACTS 10000
-
 cpCollisionHandler defaultHandler = {0, 0, alwaysCollide, alwaysCollide, nothing, nothing, NULL};

 cpSpace*
@ -174,14 +126,16 @@ cpSpaceInit(cpSpace *space)
 	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);
 	
-	cpContactBufferHeader *header = cpContactBufferHeaderInit(cpSpaceAllocContactBuffer(space), space);
-	space->contactBuffersHead = header;
-	space->contactBuffersTail = header;
-	header->next = header; // Buffers will form a ring, start the ring explicitly
-	
+	space->contactBuffersHead = NULL;
 	space->contactSet = cpHashSetNew(0, (cpHashSetEqlFunc)contactSetEql, (cpHashSetTransFunc)contactSetTrans);
 	
 	space->constraints = cpArrayNew(0);
@ -190,7 +144,9 @@ cpSpaceInit(cpSpace *space)
 	space->collFuncSet = cpHashSetNew(0, (cpHashSetEqlFunc)collFuncSetEql, (cpHashSetTransFunc)collFuncSetTrans);
 	space->collFuncSet->default_value = &space->defaultHandler;
 	
-	space->postStepCallbacks = cpHashSetNew(0, (cpHashSetEqlFunc)postStepFuncSetEql, (cpHashSetTransFunc)postStepFuncSetTrans);
+	space->postStepCallbacks = NULL;
+	
+	cpBodyInitStatic(&space->staticBody);
 	
 	return space;
 }
@ -208,6 +164,8 @@ cpSpaceDestroy(cpSpace *space)
 	cpSpaceHashFree(space->activeShapes);
 	
 	cpArrayFree(space->bodies);
+	cpArrayFree(space->sleepingComponents);
+	cpArrayFree(space->rousedBodies);
 	
 	cpArrayFree(space->constraints);
 	
@ -244,6 +202,9 @@ 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);
@ -310,31 +271,62 @@ cpSpaceSetDefaultCollisionHandler(

 #define cpAssertSpaceUnlocked(space) \
 	cpAssert(!space->locked, \
-		"This addition/removal cannot be done safely during a call to cpSpaceStep(). " \
-		"Put these calls into a Post Step Callback." \
+		"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)
 {
-	cpAssert(shape->body, "Cannot add a shape with a NULL body.");
+	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(shape->body, "Cannot add a static shape with a NULL body.");
 	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;
@ -343,10 +335,12 @@ cpSpaceAddStaticShape(cpSpace *space, cpShape *shape)
 cpBody *
 cpSpaceAddBody(cpSpace *space, cpBody *body)
 {
-	cpAssert(!cpArrayContains(space->bodies, body), "Cannot add the same body more than once.");
+	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;
 }
@ -357,6 +351,11 @@ 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;
@ -368,24 +367,37 @@ typedef struct removalContext {
 } removalContext;

 // Hashset filter func to throw away old arbiters.
-static int
+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 != cpArbiterStateCached){
+			arb->handler->separate(arb, context->space, arb->handler->data);
+		}
+		
 		cpArrayPush(context->space->pooledArbiters, arb);
-		return 0;
+		return cpFalse;
 	}
 	
-	return 1;
+	return cpTrue;
 }

 void
 cpSpaceRemoveShape(cpSpace *space, cpShape *shape)
 {
-	cpAssertWarn(cpHashSetFind(space->activeShapes->handleSet, shape->hashid, shape),
-		"Cannot remove a shape that was never added to the space. (Removed twice maybe?)");
+	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);
@ -396,238 +408,43 @@ void
 cpSpaceRemoveStaticShape(cpSpace *space, cpShape *shape)
 {
 	cpAssertWarn(cpHashSetFind(space->staticShapes->handleSet, shape->hashid, shape),
-		"Cannot remove a static shape that was never added to the space. (Removed twice maybe?)");
+		"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(cpArrayContains(space->bodies, body),
-		"Cannot remove a body that was never added to the space. (Removed twice maybe?)");
+	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 never added to the space. (Removed twice maybe?)");
+		"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 Post Step Functions
-
-void
-cpSpaceAddPostStepCallback(cpSpace *space, cpPostStepFunc func, void *obj, void *data)
-{
-	postStepCallback callback = {func, obj, data};
-	cpHashSetInsert(space->postStepCallbacks, (cpHashValue)(size_t)obj, &callback, NULL);
-}
-
-static void
-removeAndFreeShapeAndBody(cpShape *shape, cpSpace *space)
-{
-	cpSpaceRemoveShape(space, shape);
-	cpShapeFree(shape);
-}
-
-void
-cpSpacePostStepRemoveAndFreeShapeAndBody(cpSpace *space, cpShape *shape)
-{
-	cpSpaceAddPostStepCallback(space, (cpPostStepFunc)removeAndFreeShapeAndBody, shape, space);
-}
-
-#pragma mark Point Query Functions
-
-typedef struct pointQueryContext {
-	cpLayers layers;
-	cpGroup group;
-	cpSpacePointQueryFunc func;
-	void *data;
-} pointQueryContext;
-
-static void 
-pointQueryHelper(cpVect *point, cpShape *shape, pointQueryContext *context)
-{
-	if(
-		!(shape->group && context->group == shape->group) && (context->layers&shape->layers) &&
-		cpShapePointQuery(shape, *point)
-	){
-		context->func(shape, context->data);
-	}
-}
-
-void
-cpSpacePointQuery(cpSpace *space, cpVect point, cpLayers layers, cpGroup group, cpSpacePointQueryFunc func, void *data)
-{
-	pointQueryContext context = {layers, group, func, data};
-	cpSpaceHashPointQuery(space->activeShapes, point, (cpSpaceHashQueryFunc)pointQueryHelper, &context);
-	cpSpaceHashPointQuery(space->staticShapes, point, (cpSpaceHashQueryFunc)pointQueryHelper, &context);
-}
-
-static void
-rememberLastPointQuery(cpShape *shape, cpShape **outShape)
-{
-	(*outShape) = shape;
-}
-
-cpShape *
-cpSpacePointQueryFirst(cpSpace *space, cpVect point, cpLayers layers, cpGroup group)
-{
-	cpShape *shape = NULL;
-	cpSpacePointQuery(space, point, layers, group, (cpSpacePointQueryFunc)rememberLastPointQuery, &shape);
-	
-	return shape;
-}
-
-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);
-}
-
-#pragma mark Segment Query Functions
-
-typedef struct segQueryContext {
-	cpVect start, end;
-	cpLayers layers;
-	cpGroup group;
-	cpSpaceSegmentQueryFunc func;
-	int anyCollision;
-} segQueryContext;
-
-static cpFloat
-segQueryFunc(segQueryContext *context, cpShape *shape, void *data)
-{
-	cpSegmentQueryInfo info;
-	
-	if(
-		!(shape->group && context->group == shape->group) && (context->layers&shape->layers) &&
-		cpShapeSegmentQuery(shape, context->start, context->end, &info)
-	){
-		if(context->func){
-			context->func(shape, info.t, info.n, data);
-		}
-		
-		context->anyCollision = 1;
-	}
-	
-	return 1.0f;
-}
-
-int
-cpSpaceSegmentQuery(cpSpace *space, cpVect start, cpVect end, cpLayers layers, cpGroup group, cpSpaceSegmentQueryFunc func, void *data)
-{
-	segQueryContext context = {
-		start, end,
-		layers, group,
-		func,
-		0,
-	};
-	
-	cpSpaceHashSegmentQuery(space->staticShapes, &context, start, end, 1.0f, (cpSpaceHashSegmentQueryFunc)segQueryFunc, data);
-	cpSpaceHashSegmentQuery(space->activeShapes, &context, start, end, 1.0f, (cpSpaceHashSegmentQueryFunc)segQueryFunc, data);
-	
-	return context.anyCollision;
-}
-
-typedef struct segQueryFirstContext {
-	cpVect start, end;
-	cpLayers layers;
-	cpGroup group;
-} segQueryFirstContext;
-
-static cpFloat
-segQueryFirst(segQueryFirstContext *context, cpShape *shape, cpSegmentQueryInfo *out)
-{
-	cpSegmentQueryInfo info;// = {NULL, 1.0f, cpvzero};
-	if(
-		!(shape->group && context->group == shape->group) && (context->layers&shape->layers) &&
-		cpShapeSegmentQuery(shape, context->start, context->end, &info)
-	){
-		if(info.t < out->t){
-			out->shape = info.shape;
-			out->t = info.t;
-			out->n = info.n;
-		}
-		
-		return info.t;
-	}
-	
-	return 1.0f;
-}
-
-cpShape *
-cpSpaceSegmentQueryFirst(cpSpace *space, cpVect start, cpVect end, cpLayers layers, cpGroup group, cpSegmentQueryInfo *out)
-{
-	cpSegmentQueryInfo info = {NULL, 1.0f, cpvzero};
-	if(out){
-		(*out) = info;
-  } else {
-		out = &info;
-	}
-	
-	out->t = 1.0f;
-	
-	segQueryFirstContext context = {
-		start, end,
-		layers, group
-	};
-	
-	cpSpaceHashSegmentQuery(space->staticShapes, &context, start, end, 1.0f, (cpSpaceHashSegmentQueryFunc)segQueryFirst, out);
-	cpSpaceHashSegmentQuery(space->activeShapes, &context, start, end, out->t, (cpSpaceHashSegmentQueryFunc)segQueryFirst, out);
-	
-	return out->shape;
-}
-
-#pragma mark BB Query functions
-
-typedef struct bbQueryContext {
-	cpLayers layers;
-	cpGroup group;
-	cpSpaceBBQueryFunc func;
-	void *data;
-} bbQueryContext;
-
-static void 
-bbQueryHelper(cpBB *bb, cpShape *shape, bbQueryContext *context)
-{
-	if(
-		!(shape->group && context->group == shape->group) && (context->layers&shape->layers) &&
-		cpBBintersects(*bb, shape->bb)
-	){
-		context->func(shape, context->data);
-	}
-}
-
-void
-cpSpaceBBQuery(cpSpace *space, cpBB bb, cpLayers layers, cpGroup group, cpSpaceBBQueryFunc func, void *data)
-{
-	bbQueryContext context = {layers, group, func, data};
-	cpSpaceHashQuery(space->activeShapes, &bb, bb, (cpSpaceHashQueryFunc)bbQueryHelper, &context);
-	cpSpaceHashQuery(space->staticShapes, &bb, bb, (cpSpaceHashQueryFunc)bbQueryHelper, &context);
-}
-
 #pragma mark Spatial Hash Management

-// Iterator function used for updating shape BBoxes.
-static void
-updateBBCache(cpShape *shape, void *unused)
-{
-	cpShapeCacheBB(shape);
-}
+static void updateBBCache(cpShape *shape, void *unused){cpShapeCacheBB(shape);}

 void
 cpSpaceResizeStaticHash(cpSpace *space, cpFloat dim, int count)
@ -649,251 +466,34 @@ cpSpaceRehashStatic(cpSpace *space)
 	cpSpaceHashRehash(space->staticShapes);
 }

-#pragma mark Collision Detection Functions
-
-static cpContactBufferHeader *
-cpSpaceGetFreeContactBuffer(cpSpace *space)
+void
+cpSpaceRehashShape(cpSpace *space, cpShape *shape)
 {
-	if(space->stamp - space->contactBuffersTail->stamp > cp_contact_persistence){
-		cpContactBufferHeader *header = space->contactBuffersTail;
-		space->contactBuffersTail = header->next;
-		
-		return cpContactBufferHeaderInit(header, space);
-	} else {
-		cpContactBufferHeader *header = cpSpaceAllocContactBuffer(space);
-		return cpContactBufferHeaderInit(header, space);
-	}
+	cpShapeCacheBB(shape);
+	
+	// attempt to rehash the shape in both hashes
+	cpSpaceHashRehashObject(space->activeShapes, shape, shape->hashid);
+	cpSpaceHashRehashObject(space->staticShapes, shape, shape->hashid);
 }

-static void
-cpSpacePushNewContactBuffer(cpSpace *space)
-{
-//	for(cpContactBuffer *buffer = space->contactBuffersTail; buffer != space->contactBuffersHead; buffer = buffer->next){
-//		printf("%p -> ", buffer);
-//	}
-//	printf("%p (head)\n", space->contactBuffersHead);
-	
-	cpContactBufferHeader *buffer = cpSpaceGetFreeContactBuffer(space);
-	space->contactBuffersHead->next = buffer;
-	space->contactBuffersHead = buffer;
-}
-
-static inline int
-queryReject(cpShape *a, cpShape *b)
-{
-	return
-		// BBoxes must overlap
-		!cpBBintersects(a->bb, b->bb)
-		// Don't collide shapes attached to the same body.
-		|| a->body == b->body
-		// Don't collide objects in the same non-zero group
-		|| (a->group && b->group && a->group == b->group)
-		// Don't collide objects that don't share at least on layer.
-		|| !(a->layers & b->layers);
-}
-
-// Callback from the spatial hash.
-static void
-queryFunc(cpShape *a, cpShape *b, cpSpace *space)
-{
-	// Reject any of the simple cases
-	if(queryReject(a,b)) return;
-	
-	// Find the collision pair function for the shapes.
-	struct{cpCollisionType a, b;} ids = {a->collision_type, b->collision_type};
-	cpHashValue collHashID = CP_HASH_PAIR(a->collision_type, b->collision_type);
-	cpCollisionHandler *handler = (cpCollisionHandler *)cpHashSetFind(space->collFuncSet, collHashID, &ids);
-	
-	int sensor = a->sensor || b->sensor;
-	if(sensor && handler == &space->defaultHandler) return;
-	
-	// Shape 'a' should have the lower shape type. (required by cpCollideShapes() )
-	if(a->klass->type > b->klass->type){
-		cpShape *temp = a;
-		a = b;
-		b = temp;
-	}
-	
-	if(space->contactBuffersHead->numContacts + CP_MAX_CONTACTS_PER_ARBITER > CP_CONTACTS_BUFFER_SIZE){
-		// contact buffer could overflow on the next collision, push a fresh one.
-		cpSpacePushNewContactBuffer(space);
-	}
-	
-	// Narrow-phase collision detection.
-	cpContact *contacts = ((cpContactBuffer *)(space->contactBuffersHead))->contacts + space->contactBuffersHead->numContacts;
-	int numContacts = cpCollideShapes(a, b, contacts);
-	if(!numContacts) return; // Shapes are not colliding.
-	space->contactBuffersHead->numContacts += numContacts;
-	
-	// Get an arbiter from space->contactSet for the two shapes.
-	// This is where the persistant contact magic comes from.
-	cpShape *shape_pair[] = {a, b};
-	cpHashValue arbHashID = CP_HASH_PAIR((size_t)a, (size_t)b);
-	cpArbiter *arb = (cpArbiter *)cpHashSetInsert(space->contactSet, arbHashID, shape_pair, space);
-	cpArbiterUpdate(arb, contacts, numContacts, handler, a, b); // retains the contacts array
-	
-	// Call the begin function first if it's the first step
-	if(arb->stamp == -1 && !handler->begin(arb, space, handler->data)){
-		cpArbiterIgnore(arb); // permanently ignore the collision until separation
-	}
-	
-	if(
-		// Ignore the arbiter if it has been flagged
-		(arb->state != cpArbiterStateIgnore) && 
-		// Call preSolve
-		handler->preSolve(arb, space, handler->data) &&
-		// Process, but don't add collisions for sensors.
-		!sensor
-	){
-		cpArrayPush(space->arbiters, arb);
-	} else {
-//		cpfree(arb->contacts);
-		space->contactBuffersHead->numContacts -= numContacts;
-		arb->contacts = NULL;
-		arb->numContacts = 0;
-	}
-	
-	// Time stamp the arbiter so we know it was used recently.
-	arb->stamp = space->stamp;
-}
-
-// Iterator for active/static hash collisions.
-static void
-active2staticIter(cpShape *shape, cpSpace *space)
-{
-	cpSpaceHashQuery(space->staticShapes, shape, shape->bb, (cpSpaceHashQueryFunc)queryFunc, space);
-}
-
-// Hashset filter func to throw away old arbiters.
-static int
-contactSetFilter(cpArbiter *arb, cpSpace *space)
-{
-	int ticks = space->stamp - arb->stamp;
-	
-	// was used last frame, but not this one
-	if(ticks == 1){
-		arb->handler->separate(arb, space, arb->handler->data);
-		arb->stamp = -1; // mark it as a new pair again.
-	}
-	
-	if(ticks >= cp_contact_persistence){
-		cpArrayPush(space->pooledArbiters, arb);
-		return 0;
-	}
-	
-	return 1;
-}
-
-// Hashset filter func to call and throw away post step callbacks.
-static int
-postStepCallbackSetFilter(postStepCallback *callback, cpSpace *space)
-{
-	callback->func(space, callback->obj, callback->data);
-	cpfree(callback);
-	
-	return 0;
-}
-
-#pragma mark All Important cpSpaceStep() Function
-
 void
-cpSpaceStep(cpSpace *space, cpFloat dt)
+cpSpaceEachBody(cpSpace *space, cpSpaceBodyIterator func, void *data)
 {
-	if(!dt) return; // don't step if the timestep is 0!
-	
-	cpFloat dt_inv = 1.0f/dt;
-
 	cpArray *bodies = space->bodies;
-	cpArray *constraints = space->constraints;
 	
-	space->locked = 1;
-	
-	// Empty the arbiter list.
-	space->arbiters->num = 0;
-
-	// Integrate positions.
 	for(int i=0; i<bodies->num; i++){
-		cpBody *body = (cpBody *)bodies->arr[i];
-		body->position_func(body, dt);
+		func((cpBody *)bodies->arr[i], data);
 	}
 	
-	// Pre-cache BBoxes and shape data.
-	cpSpaceHashEach(space->activeShapes, (cpSpaceHashIterator)updateBBCache, NULL);
-	
-	// Collide!
-	cpSpacePushNewContactBuffer(space);
-	cpSpaceHashEach(space->activeShapes, (cpSpaceHashIterator)active2staticIter, space);
-	cpSpaceHashQueryRehash(space->activeShapes, (cpSpaceHashQueryFunc)queryFunc, space);
-	
-	// Clear out old cached arbiters and dispatch untouch functions
-	cpHashSetFilter(space->contactSet, (cpHashSetFilterFunc)contactSetFilter, space);
-
-	// Prestep the arbiters.
-	cpArray *arbiters = space->arbiters;
-	for(int i=0; i<arbiters->num; i++)
-		cpArbiterPreStep((cpArbiter *)arbiters->arr[i], dt_inv);
-
-	// Prestep the constraints.
-	for(int i=0; i<constraints->num; i++){
-		cpConstraint *constraint = (cpConstraint *)constraints->arr[i];
-		constraint->klass->preStep(constraint, dt, dt_inv);
+	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);
 	}
-
-	for(int i=0; i<space->elasticIterations; i++){
-		for(int j=0; j<arbiters->num; j++)
-			cpArbiterApplyImpulse((cpArbiter *)arbiters->arr[j], 1.0f);
-			
-		for(int j=0; j<constraints->num; j++){
-			cpConstraint *constraint = (cpConstraint *)constraints->arr[j];
-			constraint->klass->applyImpulse(constraint);
-		}
-	}
-
-	// Integrate velocities.
-	cpFloat damping = cpfpow(1.0f/space->damping, -dt);
-	for(int i=0; i<bodies->num; i++){
-		cpBody *body = (cpBody *)bodies->arr[i];
-		body->velocity_func(body, space->gravity, damping, dt);
-	}
-
-	for(int i=0; i<arbiters->num; i++)
-		cpArbiterApplyCachedImpulse((cpArbiter *)arbiters->arr[i]);
-	
-	// run the old-style elastic solver if elastic iterations are disabled
-	cpFloat elasticCoef = (space->elasticIterations ? 0.0f : 1.0f);
-	
-	// Run the impulse solver.
-	for(int i=0; i<space->iterations; i++){
-		for(int j=0; j<arbiters->num; j++)
-			cpArbiterApplyImpulse((cpArbiter *)arbiters->arr[j], elasticCoef);
-			
-		for(int j=0; j<constraints->num; j++){
-			cpConstraint *constraint = (cpConstraint *)constraints->arr[j];
-			constraint->klass->applyImpulse(constraint);
-		}
-	}
-	
-	space->locked = 0;
-	
-	// run the post solve callbacks
-	for(int i=0; i<arbiters->num; i++){
-		cpArbiter *arb = (cpArbiter *) arbiters->arr[i];
-		
-		cpCollisionHandler *handler = arb->handler;
-		handler->postSolve(arb, space, handler->data);
-		
-		arb->state = cpArbiterStateNormal;
-	}
-	
-	// Run the post step callbacks
-	// Use filter as an easy way to clear out the queue as it runs
-	cpHashSetFilter(space->postStepCallbacks, (cpHashSetFilterFunc)postStepCallbackSetFilter, space);
-	
-//	cpFloat dvsq = cpvdot(space->gravity, space->gravity);
-//	dvsq *= dt*dt * space->damping*space->damping;
-//	for(int i=0; i<bodies->num; i++)
-//		cpBodyMarkLowEnergy(bodies->arr[i], dvsq, space->sleepTicks);
-	
-	// Increment the stamp.
-	space->stamp++;
 }
+
+
--- a/chipmunk/src/cpSpaceHash.c
+++ b/chipmunk/src/cpSpaceHash.c
@ -21,9 +21,8 @@

 #include <math.h>
 #include <stdlib.h>
-#include <stdio.h>

-#include "chipmunk.h"
+#include "chipmunk_private.h"
 #include "prime.h"

 static cpHandle*
@ -36,11 +35,7 @@ cpHandleInit(cpHandle *hand, void *obj)
 	return hand;
 }

-static inline void
-cpHandleRetain(cpHandle *hand)
-{
-	hand->retain++;
-}
+static inline void cpHandleRetain(cpHandle *hand){hand->retain++;}

 static inline void
 cpHandleRelease(cpHandle *hand, cpArray *pooledHandles)
@ -55,7 +50,7 @@ cpSpaceHashAlloc(void)
 	return (cpSpaceHash *)cpcalloc(1, sizeof(cpSpaceHash));
 }

-// Frees the old table, and allocates a new one.
+// Frees the old table, and allocate a new one.
 static void
 cpSpaceHashAllocTable(cpSpaceHash *hash, int numcells)
 {
@ -66,12 +61,7 @@ cpSpaceHashAllocTable(cpSpaceHash *hash, int numcells)
 }

 // Equality function for the handleset.
-static int
-handleSetEql(void *obj, void *elt)
-{
-	cpHandle *hand = (cpHandle *)elt;
-	return (obj == hand->obj);
-}
+static int handleSetEql(void *obj, cpHandle *hand){return (obj == hand->obj);}

 // Transformation function for the handleset.
 static void *
@ -101,7 +91,7 @@ cpSpaceHashInit(cpSpaceHash *hash, cpFloat celldim, int numcells, cpSpaceHashBBF
 	hash->celldim = celldim;
 	hash->bbfunc = bbfunc;
 	
-	hash->handleSet = cpHashSetNew(0, handleSetEql, (cpHashSetTransFunc)handleSetTrans);
+	hash->handleSet = cpHashSetNew(0, (cpHashSetEqlFunc)handleSetEql, (cpHashSetTransFunc)handleSetTrans);
 	hash->pooledHandles = cpArrayNew(0);
 	
 	hash->pooledBins = NULL;
@ -132,7 +122,6 @@ clearHashCell(cpSpaceHash *hash, int idx)
 	while(bin){
 		cpSpaceHashBin *next = bin->next;
 		
-		// Release the lock on the handle.
 		cpHandleRelease(bin->handle, hash->pooledHandles);
 		recycleBin(hash, bin);
 		
@ -186,15 +175,15 @@ cpSpaceHashResize(cpSpaceHash *hash, cpFloat celldim, int numcells)
 }

 // Return true if the chain contains the handle.
-static inline int
+static inline cpBool
 containsHandle(cpSpaceHashBin *bin, cpHandle *hand)
 {
 	while(bin){
-		if(bin->handle == hand) return 1;
+		if(bin->handle == hand) return cpTrue;
 		bin = bin->next;
 	}
 	
-	return 0;
+	return cpFalse;
 }

 // Get a recycled or new bin.
@ -266,36 +255,32 @@ hashHandle(cpSpaceHash *hash, cpHandle *hand, cpBB bb)
 }

 void
-cpSpaceHashInsert(cpSpaceHash *hash, void *obj, cpHashValue hashid, cpBB bb)
+cpSpaceHashInsert(cpSpaceHash *hash, void *obj, cpHashValue hashid, cpBB _deprecated_unused)
 {
 	cpHandle *hand = (cpHandle *)cpHashSetInsert(hash->handleSet, hashid, obj, hash);
-	hashHandle(hash, hand, bb);
+	hashHandle(hash, hand, hash->bbfunc(obj));
 }

 void
 cpSpaceHashRehashObject(cpSpaceHash *hash, void *obj, cpHashValue hashid)
 {
-	cpHandle *hand = (cpHandle *)cpHashSetFind(hash->handleSet, hashid, obj);
-	hashHandle(hash, hand, hash->bbfunc(obj));
+	cpHandle *hand = (cpHandle *)cpHashSetRemove(hash->handleSet, hashid, obj);
+	
+	if(hand){
+		hand->obj = NULL;
+		cpHandleRelease(hand, hash->pooledHandles);
+		
+		cpSpaceHashInsert(hash, obj, hashid, cpBBNew(0.0f, 0.0f, 0.0f, 0.0f));
+	}
 }

-// Hashset iterator function for rehashing the spatial hash. (hash hash hash hash?)
-static void
-handleRehashHelper(void *elt, void *data)
-{
-	cpHandle *hand = (cpHandle *)elt;
-	cpSpaceHash *hash = (cpSpaceHash *)data;
-	
-	hashHandle(hash, hand, hash->bbfunc(hand->obj));
-}
+static void handleRehashHelper(cpHandle *hand, cpSpaceHash *hash){hashHandle(hash, hand, hash->bbfunc(hand->obj));}

 void
 cpSpaceHashRehash(cpSpaceHash *hash)
 {
 	clearHash(hash);
-	
-	// Rehash all of the handles.
-	cpHashSetEach(hash->handleSet, &handleRehashHelper, hash);
+	cpHashSetEach(hash->handleSet, (cpHashSetIterFunc)handleRehashHelper, hash);
 }

 void
@ -309,54 +294,63 @@ cpSpaceHashRemove(cpSpaceHash *hash, void *obj, cpHashValue hashid)
 	}
 }

-// Used by the cpSpaceHashEach() iterator.
 typedef struct eachPair {
 	cpSpaceHashIterator func;
 	void *data;
 } eachPair;

-// Calls the user iterator function. (Gross I know.)
-static void
-eachHelper(void *elt, void *data)
-{
-	cpHandle *hand = (cpHandle *)elt;
-	eachPair *pair = (eachPair *)data;
-	
-	pair->func(hand->obj, pair->data);
-}
+static void eachHelper(cpHandle *hand, eachPair *pair){pair->func(hand->obj, pair->data);}

 // Iterate over the objects in the spatial hash.
 void
 cpSpaceHashEach(cpSpaceHash *hash, cpSpaceHashIterator func, void *data)
 {
-	// Bundle the callback up to send to the hashset iterator.
 	eachPair pair = {func, data};
-	
-	cpHashSetEach(hash->handleSet, &eachHelper, &pair);
+	cpHashSetEach(hash->handleSet, (cpHashSetIterFunc)eachHelper, &pair);
+}
+
+static inline void
+removeOrphanedHandles(cpSpaceHash *hash, cpSpaceHashBin **bin_ptr)
+{
+	cpSpaceHashBin *bin = *bin_ptr;
+	while(bin){
+		cpHandle *hand = bin->handle;
+		cpSpaceHashBin *next = bin->next;
+		
+		if(!hand->obj){
+			// orphaned handle, unlink and recycle the bin
+			(*bin_ptr) = bin->next;
+			recycleBin(hash, bin);
+			
+			cpHandleRelease(hand, hash->pooledHandles);
+		} else {
+			bin_ptr = &bin->next;
+		}
+		
+		bin = next;
+	}
 }

 // Calls the callback function for the objects in a given chain.
 static inline void
-query(cpSpaceHash *hash, cpSpaceHashBin *bin, void *obj, cpSpaceHashQueryFunc func, void *data)
+query(cpSpaceHash *hash, cpSpaceHashBin **bin_ptr, void *obj, cpSpaceHashQueryFunc func, void *data)
 {
-	for(; bin; bin = bin->next){
+	restart:
+	for(cpSpaceHashBin *bin = *bin_ptr; bin; bin = bin->next){
 		cpHandle *hand = bin->handle;
 		void *other = hand->obj;
 		
-		// Skip over certain conditions
-		if(
-			// Have we already tried this pair in this query?
-			hand->stamp == hash->stamp
-			// Is obj the same as other?
-			|| obj == other 
-			// Has other been removed since the last rehash?
-			|| !other
-			) continue;
-		
-		func(obj, other, data);
-
-		// Stamp that the handle was checked already against this object.
-		hand->stamp = hash->stamp;
+		if(hand->stamp == hash->stamp || obj == other){
+			continue;
+		} else if(other){
+			func(obj, other, data);
+			hand->stamp = hash->stamp;
+		} else {
+			// The object for this handle has been removed
+			// cleanup this cell and restart the query
+			removeOrphanedHandles(hash, bin_ptr);
+			goto restart; // GCC not smart enough/able to tail call an inlined function.
+		}
 	}
 }

@ -366,10 +360,7 @@ cpSpaceHashPointQuery(cpSpaceHash *hash, cpVect point, cpSpaceHashQueryFunc func
 	cpFloat dim = hash->celldim;
 	int idx = hash_func(floor_int(point.x/dim), floor_int(point.y/dim), hash->numcells);  // Fix by ShiftZ
 	
-	query(hash, hash->table[idx], &point, func, data);
-
-	// Increment the stamp.
-	// Only one cell is checked, but query() requires it anyway.
+	query(hash, &hash->table[idx], &point, func, data);
 	hash->stamp++;
 }

@ -384,16 +375,15 @@ cpSpaceHashQuery(cpSpaceHash *hash, void *obj, cpBB bb, cpSpaceHashQueryFunc fun
 	int t = floor_int(bb.t/dim);
 	
 	int n = hash->numcells;
+	cpSpaceHashBin **table = hash->table;
 	
 	// Iterate over the cells and query them.
 	for(int i=l; i<=r; i++){
 		for(int j=b; j<=t; j++){
-			int idx = hash_func(i,j,n);
-			query(hash, hash->table[idx], obj, func, data);
+			query(hash, &table[hash_func(i,j,n)], obj, func, data);
 		}
 	}
 	
-	// Increment the stamp.
 	hash->stamp++;
 }

@ -425,29 +415,27 @@ handleQueryRehashHelper(void *elt, void *data)
 	int r = floor_int(bb.r/dim);
 	int b = floor_int(bb.b/dim);
 	int t = floor_int(bb.t/dim);
+	
+	cpSpaceHashBin **table = hash->table;

 	for(int i=l; i<=r; i++){
 		for(int j=b; j<=t; j++){
-//			// exit the loops if the object has been deleted in func().
-//			if(!hand->obj) goto break_out;
-			
 			int idx = hash_func(i,j,n);
-			cpSpaceHashBin *bin = hash->table[idx];
+			cpSpaceHashBin *bin = table[idx];
 			
 			if(containsHandle(bin, hand)) continue;
 			
 			cpHandleRetain(hand); // this MUST be done first in case the object is removed in func()
-			query(hash, bin, obj, func, pair->data);
+			query(hash, &bin, obj, func, pair->data);
 			
 			cpSpaceHashBin *newBin = getEmptyBin(hash);
 			newBin->handle = hand;
 			newBin->next = bin;
-			hash->table[idx] = newBin;
+			table[idx] = newBin;
 		}
 	}
 	
-//	break_out:
-	// Increment the stamp for each object we hash.
+	// Increment the stamp for each object hashed.
 	hash->stamp++;
 }

@ -461,26 +449,27 @@ cpSpaceHashQueryRehash(cpSpaceHash *hash, cpSpaceHashQueryFunc func, void *data)
 }

 static inline cpFloat
-segmentQuery(cpSpaceHash *hash, cpSpaceHashBin *bin, void *obj, cpSpaceHashSegmentQueryFunc func, void *data)
+segmentQuery(cpSpaceHash *hash, cpSpaceHashBin **bin_ptr, void *obj, cpSpaceHashSegmentQueryFunc func, void *data)
 {
 	cpFloat t = 1.0f;
 	 
-	for(; bin; bin = bin->next){
+	restart:
+	for(cpSpaceHashBin *bin = *bin_ptr; bin; bin = bin->next){
 		cpHandle *hand = bin->handle;
 		void *other = hand->obj;
 		
 		// Skip over certain conditions
-		if(
-			// Have we already tried this pair in this query?
-			hand->stamp == hash->stamp
-			// Has other been removed since the last rehash?
-			|| !other
-			) continue;
-		
-		// Stamp that the handle was checked already against this object.
-		hand->stamp = hash->stamp;
-		
-		t = cpfmin(t, func(obj, other, data));
+		if(hand->stamp == hash->stamp){
+			continue;
+		} else if(other){
+			t = cpfmin(t, func(obj, other, data));
+			hand->stamp = hash->stamp;
+		} else {
+			// The object for this handle has been removed
+			// cleanup this cell and restart the query
+			removeOrphanedHandles(hash, bin_ptr);
+			goto restart; // GCC not smart enough/able to tail call an inlined function.
+		}
 	}
 	
 	return t;
@ -492,8 +481,6 @@ void cpSpaceHashSegmentQuery(cpSpaceHash *hash, void *obj, cpVect a, cpVect b, c
 	a = cpvmult(a, 1.0f/hash->celldim);
 	b = cpvmult(b, 1.0f/hash->celldim);
 	
-	cpFloat dt_dx = 1.0f/cpfabs(b.x - a.x), dt_dy = 1.0f/cpfabs(b.y - a.y);
-	
 	int cell_x = floor_int(a.x), cell_y = floor_int(a.y);

 	cpFloat t = 0;
@ -517,14 +504,20 @@ void cpSpaceHashSegmentQuery(cpSpaceHash *hash, void *obj, cpVect a, cpVect b, c
 		temp_v = (a.y - cpffloor(a.y));
 	}
 	
+	// Division by zero is *very* slow on ARM
+	cpFloat dx = cpfabs(b.x - a.x), dy = cpfabs(b.y - a.y);
+	cpFloat dt_dx = (dx ? 1.0f/dx : INFINITY), dt_dy = (dy ? 1.0f/dy : INFINITY);
+	
 	// fix NANs in horizontal directions
 	cpFloat next_h = (temp_h ? temp_h*dt_dx : dt_dx);
 	cpFloat next_v = (temp_v ? temp_v*dt_dy : dt_dy);
+	
+	cpSpaceHashBin **table = hash->table;

 	int n = hash->numcells;
 	while(t < t_exit){
 		int idx = hash_func(cell_x, cell_y, n);
-		t_exit = cpfmin(t_exit, segmentQuery(hash, hash->table[idx], obj, func, data));
+		t_exit = cpfmin(t_exit, segmentQuery(hash, &table[idx], obj, func, data));

 		if (next_v < next_h){
 			cell_y += y_inc;