chipmunk 5.3.4.5 → 6.1.3.0.rc1

data/ext/chipmunk/cpSpace.h

@@ -19,120 +19,138 @@
  * SOFTWARE.
  */
 
-struct cpSpace;
-
-// Number of frames that contact information should persist.
-extern cpTimestamp cp_contact_persistence;
-
-// User collision handler function types.
-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);
-
-// Structure for holding collision pair function information.
-// Used internally.
-typedef struct cpCollisionHandler {
-	cpCollisionType a;
-	cpCollisionType b;
-	cpCollisionBeginFunc begin;
-	cpCollisionPreSolveFunc preSolve;
-	cpCollisionPostSolveFunc postSolve;
-	cpCollisionSeparateFunc separate;
-	void *data;
-} cpCollisionHandler;
-
-typedef struct cpContactBufferHeader {
-	cpTimestamp stamp;
-	struct cpContactBufferHeader *next;
-	unsigned int numContacts;
-} cpContactBufferHeader;
-
-typedef struct cpSpace{
-	// *** User definable fields
-	
-	// Number of iterations to use in the impulse solver to solve contacts.
+/// @defgroup cpSpace cpSpace
+/// @{
+
+typedef struct cpContactBufferHeader cpContactBufferHeader;
+typedef void (*cpSpaceArbiterApplyImpulseFunc)(cpArbiter *arb);
+
+/// Basic Unit of Simulation in Chipmunk
+struct cpSpace {
+	/// Number of iterations to use in the impulse solver to solve contacts.
 	int iterations;
 	
-	// Number of iterations to use in the impulse solver to solve elastic collisions.
-	int elasticIterations;
-	
-	// Default gravity to supply when integrating rigid body motions.
+	/// Gravity to pass to rigid bodies when integrating velocity.
 	cpVect gravity;
 	
-	// Default damping to supply when integrating rigid body motions.
+	/// Damping rate expressed as the fraction of velocity bodies retain each second.
+	/// A value of 0.9 would mean that each body's velocity will drop 10% per second.
+	/// The default value is 1.0, meaning no damping is applied.
+	/// @note This damping value is different than those of cpDampedSpring and cpDampedRotarySpring.
 	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.
+	/// 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.
+	/// Time a group of bodies must remain idle in order to fall asleep.
+	/// Enabling sleeping also implicitly enables the the contact graph.
+	/// The default value of INFINITY disables the sleeping algorithm.
 	cpFloat sleepTimeThreshold;
 	
-	// *** Internally Used Fields
+	/// Amount of encouraged penetration between colliding shapes.
+	/// Used to reduce oscillating contacts and keep the collision cache warm.
+	/// Defaults to 0.1. If you have poor simulation quality,
+	/// increase this number as much as possible without allowing visible amounts of overlap.
+	cpFloat collisionSlop;
 	
-	// When the space lock count is non zero you cannot add or remove objects
-	CP_PRIVATE(int locked);
+	/// Determines how fast overlapping shapes are pushed apart.
+	/// Expressed as a fraction of the error remaining after each second.
+	/// Defaults to pow(1.0 - 0.1, 60.0) meaning that Chipmunk fixes 10% of overlap each frame at 60Hz.
+	cpFloat collisionBias;
+	
+	/// Number of frames that contact information should persist.
+	/// Defaults to 3. There is probably never a reason to change this value.
+	cpTimestamp collisionPersistence;
+	
+	/// Rebuild the contact graph during each step. Must be enabled to use the cpBodyEachArbiter() function.
+	/// Disabled by default for a small performance boost. Enabled implicitly when the sleeping feature is enabled.
+	cpBool enableContactGraph;
+	
+	/// User definable data pointer.
+	/// Generally this points to your game's controller or game state
+	/// class so you can access it when given a cpSpace reference in a callback.
+	cpDataPointer data;
+	
+	/// The designated static body for this space.
+	/// You can modify this body, or replace it with your own static body.
+	/// By default it points to a statically allocated cpBody in the cpSpace struct.
+	cpBody *staticBody;
 	
-	// Time stamp. Is incremented on every call to cpSpaceStep().
 	CP_PRIVATE(cpTimestamp stamp);
+	CP_PRIVATE(cpFloat curr_dt);
 
-	// The static and active shape spatial hashes.
-	CP_PRIVATE(cpSpaceHash *staticShapes);
-	CP_PRIVATE(cpSpaceHash *activeShapes);
-	
-	// List of bodies in the system.
 	CP_PRIVATE(cpArray *bodies);
-	
-	// List of groups of sleeping bodies.
+	CP_PRIVATE(cpArray *rousedBodies);
 	CP_PRIVATE(cpArray *sleepingComponents);
 	
-	// List of bodies that have been flagged to be awoken.
-	CP_PRIVATE(cpArray *rousedBodies);
+	CP_PRIVATE(cpSpatialIndex *staticShapes);
+	CP_PRIVATE(cpSpatialIndex *activeShapes);
 	
-	// List of active arbiters for the impulse solver.
 	CP_PRIVATE(cpArray *arbiters);
-	CP_PRIVATE(cpArray *pooledArbiters);
-	
-	// Linked list ring of contact buffers.
-	// Head is the newest buffer, and each buffer points to a newer buffer.
-	// Head wraps around and points to the oldest (tail) buffer.
 	CP_PRIVATE(cpContactBufferHeader *contactBuffersHead);
-	CP_PRIVATE(cpContactBufferHeader *_contactBuffersTail_Deprecated);
+	CP_PRIVATE(cpHashSet *cachedArbiters);
+	CP_PRIVATE(cpArray *pooledArbiters);
+	CP_PRIVATE(cpArray *constraints);
 	
-	// List of buffers to be free()ed when destroying the space.
 	CP_PRIVATE(cpArray *allocatedBuffers);
+	CP_PRIVATE(int locked);
 	
-	// Persistant contact set.
-	CP_PRIVATE(cpHashSet *contactSet);
-	
-	// List of constraints in the system.
-	CP_PRIVATE(cpArray *constraints);
-	
-	// Set of collisionpair functions.
-	CP_PRIVATE(cpHashSet *collFuncSet);
-	// Default collision handler.
+	CP_PRIVATE(cpHashSet *collisionHandlers);
 	CP_PRIVATE(cpCollisionHandler defaultHandler);
 	
-	CP_PRIVATE(cpHashSet *postStepCallbacks);
+	CP_PRIVATE(cpBool skipPostStep);
+	CP_PRIVATE(cpArray *postStepCallbacks);
 	
-	cpBody staticBody;
-} cpSpace;
+	CP_PRIVATE(cpBody _staticBody);
+};
 
-// Basic allocation/destruction functions.
+/// Allocate a cpSpace.
 cpSpace* cpSpaceAlloc(void);
+/// Initialize a cpSpace.
 cpSpace* cpSpaceInit(cpSpace *space);
+/// Allocate and initialize a cpSpace.
 cpSpace* cpSpaceNew(void);
 
+/// Destroy a cpSpace.
 void cpSpaceDestroy(cpSpace *space);
+/// Destroy and free a cpSpace.
 void cpSpaceFree(cpSpace *space);
 
-// Convenience function. Frees all referenced entities. (bodies, shapes and constraints)
-void cpSpaceFreeChildren(cpSpace *space);
+#define CP_DefineSpaceStructGetter(type, member, name) \
+static inline type cpSpaceGet##name(const cpSpace *space){return space->member;}
+
+#define CP_DefineSpaceStructSetter(type, member, name) \
+static inline void cpSpaceSet##name(cpSpace *space, type value){space->member = value;}
+
+#define CP_DefineSpaceStructProperty(type, member, name) \
+CP_DefineSpaceStructGetter(type, member, name) \
+CP_DefineSpaceStructSetter(type, member, name)
+
+CP_DefineSpaceStructProperty(int, iterations, Iterations)
+CP_DefineSpaceStructProperty(cpVect, gravity, Gravity)
+CP_DefineSpaceStructProperty(cpFloat, damping, Damping)
+CP_DefineSpaceStructProperty(cpFloat, idleSpeedThreshold, IdleSpeedThreshold)
+CP_DefineSpaceStructProperty(cpFloat, sleepTimeThreshold, SleepTimeThreshold)
+CP_DefineSpaceStructProperty(cpFloat, collisionSlop, CollisionSlop)
+CP_DefineSpaceStructProperty(cpFloat, collisionBias, CollisionBias)
+CP_DefineSpaceStructProperty(cpTimestamp, collisionPersistence, CollisionPersistence)
+CP_DefineSpaceStructProperty(cpBool, enableContactGraph, EnableContactGraph)
+CP_DefineSpaceStructProperty(cpDataPointer, data, UserData)
+CP_DefineSpaceStructGetter(cpBody*, staticBody, StaticBody)
+CP_DefineSpaceStructGetter(cpFloat, CP_PRIVATE(curr_dt), CurrentTimeStep)
+
+/// returns true from inside a callback and objects cannot be added/removed.
+static inline cpBool
+cpSpaceIsLocked(cpSpace *space)
+{
+	return space->CP_PRIVATE(locked);
+}
 
-// Collision handler management functions.
+/// Set a default collision handler for this space.
+/// The default collision handler is invoked for each colliding pair of shapes
+/// that isn't explicitly handled by a specific collision handler.
+/// You can pass NULL for any function you don't want to implement.
 void cpSpaceSetDefaultCollisionHandler(
 	cpSpace *space,
 	cpCollisionBeginFunc begin,
@@ -141,6 +159,9 @@ void cpSpaceSetDefaultCollisionHandler(
 	cpCollisionSeparateFunc separate,
 	void *data
 );
+
+/// Set a collision handler to be used whenever the two shapes with the given collision types collide.
+/// You can pass NULL for any function you don't want to implement.
 void cpSpaceAddCollisionHandler(
 	cpSpace *space,
 	cpCollisionType a, cpCollisionType b,
@@ -150,57 +171,106 @@ void cpSpaceAddCollisionHandler(
 	cpCollisionSeparateFunc separate,
 	void *data
 );
+
+/// Unset a collision handler.
 void cpSpaceRemoveCollisionHandler(cpSpace *space, cpCollisionType a, cpCollisionType b);
 
-// Add and remove entities from the system.
-cpShape *cpSpaceAddShape(cpSpace *space, cpShape *shape);
-cpShape *cpSpaceAddStaticShape(cpSpace *space, cpShape *shape);
-cpBody *cpSpaceAddBody(cpSpace *space, cpBody *body);
-cpConstraint *cpSpaceAddConstraint(cpSpace *space, cpConstraint *constraint);
+/// Add a collision shape to the simulation.
+/// If the shape is attached to a static body, it will be added as a static shape.
+cpShape* cpSpaceAddShape(cpSpace *space, cpShape *shape);
+/// Explicity add a shape as a static shape to the simulation.
+cpShape* cpSpaceAddStaticShape(cpSpace *space, cpShape *shape);
+/// Add a rigid body to the simulation.
+cpBody* cpSpaceAddBody(cpSpace *space, cpBody *body);
+/// Add a constraint to the simulation.
+cpConstraint* cpSpaceAddConstraint(cpSpace *space, cpConstraint *constraint);
 
+/// Remove a collision shape from the simulation.
 void cpSpaceRemoveShape(cpSpace *space, cpShape *shape);
+/// Remove a collision shape added using cpSpaceAddStaticShape() from the simulation.
 void cpSpaceRemoveStaticShape(cpSpace *space, cpShape *shape);
+/// Remove a rigid body from the simulation.
 void cpSpaceRemoveBody(cpSpace *space, cpBody *body);
+/// Remove a constraint from the simulation.
 void cpSpaceRemoveConstraint(cpSpace *space, cpConstraint *constraint);
 
-// Post Step function definition
-typedef void (*cpPostStepFunc)(cpSpace *space, void *obj, void *data);
-// Register a post step function to be called after cpSpaceStep() has finished.
-// obj is used a key, you can only register one callback per unique value for obj
-void cpSpaceAddPostStepCallback(cpSpace *space, cpPostStepFunc func, void *obj, void *data);
+/// Test if a collision shape has been added to the space.
+cpBool cpSpaceContainsShape(cpSpace *space, cpShape *shape);
+/// Test if a rigid body has been added to the space.
+cpBool cpSpaceContainsBody(cpSpace *space, cpBody *body);
+/// Test if a constraint has been added to the space.
+cpBool cpSpaceContainsConstraint(cpSpace *space, cpConstraint *constraint);
 
-// Point query callback function
+/// Post Step callback function type.
+typedef void (*cpPostStepFunc)(cpSpace *space, void *key, void *data);
+/// Schedule a post-step callback to be called when cpSpaceStep() finishes.
+/// You can only register one callback per unique value for @c key.
+/// Returns true only if @c key has never been scheduled before.
+/// It's possible to pass @c NULL for @c func if you only want to mark @c key as being used.
+cpBool cpSpaceAddPostStepCallback(cpSpace *space, cpPostStepFunc func, void *key, void *data);
+
+/// Point query callback function type.
 typedef void (*cpSpacePointQueryFunc)(cpShape *shape, void *data);
+/// Query the space at a point and call @c func for each shape found.
 void cpSpacePointQuery(cpSpace *space, cpVect point, cpLayers layers, cpGroup group, cpSpacePointQueryFunc func, void *data);
+/// Query the space at a point and return the first shape found. Returns NULL if no shapes were found.
 cpShape *cpSpacePointQueryFirst(cpSpace *space, cpVect point, cpLayers layers, cpGroup group);
 
-// Segment query callback function
+/// Nearest point query callback function type.
+typedef void (*cpSpaceNearestPointQueryFunc)(cpShape *shape, cpFloat distance, cpVect point, void *data);
+/// Query the space at a point and call @c func for each shape found.
+void cpSpaceNearestPointQuery(cpSpace *space, cpVect point, cpFloat maxDistance, cpLayers layers, cpGroup group, cpSpaceNearestPointQueryFunc func, void *data);
+/// Query the space at a point and return the nearest shape found. Returns NULL if no shapes were found.
+cpShape *cpSpaceNearestPointQueryNearest(cpSpace *space, cpVect point, cpFloat maxDistance, cpLayers layers, cpGroup group, cpNearestPointQueryInfo *out);
+
+/// Segment query callback function type.
 typedef void (*cpSpaceSegmentQueryFunc)(cpShape *shape, cpFloat t, cpVect n, void *data);
+/// Perform a directed line segment query (like a raycast) against the space calling @c func for each shape intersected.
 void cpSpaceSegmentQuery(cpSpace *space, cpVect start, cpVect end, cpLayers layers, cpGroup group, cpSpaceSegmentQueryFunc func, void *data);
+/// Perform a directed line segment query (like a raycast) against the space and return the first shape hit. Returns NULL if no shapes were hit.
 cpShape *cpSpaceSegmentQueryFirst(cpSpace *space, cpVect start, cpVect end, cpLayers layers, cpGroup group, cpSegmentQueryInfo *out);
 
-// BB query callback function
+/// Rectangle Query callback function type.
 typedef void (*cpSpaceBBQueryFunc)(cpShape *shape, void *data);
+/// Perform a fast rectangle query on the space calling @c func for each shape found.
+/// Only the shape's bounding boxes are checked for overlap, not their full shape.
 void cpSpaceBBQuery(cpSpace *space, cpBB bb, cpLayers layers, cpGroup group, cpSpaceBBQueryFunc func, void *data);
 
-// Shape query callback function
+/// Shape query callback function type.
 typedef void (*cpSpaceShapeQueryFunc)(cpShape *shape, cpContactPointSet *points, void *data);
+/// Query a space for any shapes overlapping the given shape and call @c func for each shape found.
 cpBool cpSpaceShapeQuery(cpSpace *space, cpShape *shape, cpSpaceShapeQueryFunc func, void *data);
 
-
+/// Call cpBodyActivate() for any shape that is overlaps the given shape.
 void cpSpaceActivateShapesTouchingShape(cpSpace *space, cpShape *shape);
 
 
-// Iterator function for iterating the bodies in a space.
-typedef void (*cpSpaceBodyIterator)(cpBody *body, void *data);
-void cpSpaceEachBody(cpSpace *space, cpSpaceBodyIterator func, void *data);
+/// Space/body iterator callback function type.
+typedef void (*cpSpaceBodyIteratorFunc)(cpBody *body, void *data);
+/// Call @c func for each body in the space.
+void cpSpaceEachBody(cpSpace *space, cpSpaceBodyIteratorFunc func, void *data);
+
+/// Space/body iterator callback function type.
+typedef void (*cpSpaceShapeIteratorFunc)(cpShape *shape, void *data);
+/// Call @c func for each shape in the space.
+void cpSpaceEachShape(cpSpace *space, cpSpaceShapeIteratorFunc func, void *data);
 
-// Spatial hash management functions.
-void cpSpaceResizeStaticHash(cpSpace *space, cpFloat dim, int count);
-void cpSpaceResizeActiveHash(cpSpace *space, cpFloat dim, int count);
-void cpSpaceRehashStatic(cpSpace *space);
+/// Space/constraint iterator callback function type.
+typedef void (*cpSpaceConstraintIteratorFunc)(cpConstraint *constraint, void *data);
+/// Call @c func for each shape in the space.
+void cpSpaceEachConstraint(cpSpace *space, cpSpaceConstraintIteratorFunc func, void *data);
 
-void cpSpaceRehashShape(cpSpace *space, cpShape *shape);
+/// Update the collision detection info for the static shapes in the space.
+void cpSpaceReindexStatic(cpSpace *space);
+/// Update the collision detection data for a specific shape in the space.
+void cpSpaceReindexShape(cpSpace *space, cpShape *shape);
+/// Update the collision detection data for all shapes attached to a body.
+void cpSpaceReindexShapesForBody(cpSpace *space, cpBody *body);
 
-// Update the space.
+/// Switch the space to use a spatial has as it's spatial index.
+void cpSpaceUseSpatialHash(cpSpace *space, cpFloat dim, int count);
+
+/// Step the space forward in time by @c dt.
 void cpSpaceStep(cpSpace *space, cpFloat dt);
+
+/// @}

data/ext/chipmunk/cpSpaceComponent.c

@@ -19,74 +19,121 @@
  * SOFTWARE.
  */
  
-#include <stdlib.h>
+#include <string.h>
 
 #include "chipmunk_private.h"
 
-#pragma mark Sleeping Functions
+//MARK: Sleeping Functions
 
-// Chipmunk uses a data structure called a disjoint set forest.
-// My attempts to find a way to splice circularly linked lists in
-// constant time failed, and so I found this neat data structure instead.
-
-static inline cpBody *
-componentNodeRoot(cpBody *body)
+void
+cpSpaceActivateBody(cpSpace *space, cpBody *body)
 {
-	cpBody *parent = body->node.parent;
+	cpAssertHard(!cpBodyIsRogue(body), "Internal error: Attempting to activate a rogue body.");
 	
-	if(parent){
-		// path compression, attaches this node directly to the root
-		return (body->node.parent = componentNodeRoot(parent));
+	if(space->locked){
+		// cpSpaceActivateBody() is called again once the space is unlocked
+		if(!cpArrayContains(space->rousedBodies, body)) cpArrayPush(space->rousedBodies, body);
 	} else {
-		return body;
+		cpArrayPush(space->bodies, body);
+
+		CP_BODY_FOREACH_SHAPE(body, shape){
+			cpSpatialIndexRemove(space->staticShapes, shape, shape->hashid);
+			cpSpatialIndexInsert(space->activeShapes, shape, shape->hashid);
+		}
+		
+		CP_BODY_FOREACH_ARBITER(body, arb){
+			cpBody *bodyA = arb->body_a;
+			
+			// Arbiters are shared between two bodies that are always woken up together.
+			// You only want to restore the arbiter once, so bodyA is arbitrarily chosen to own the arbiter.
+			// The edge case is when static bodies are involved as the static bodies never actually sleep.
+			// If the static body is bodyB then all is good. If the static body is bodyA, that can easily be checked.
+			if(body == bodyA || cpBodyIsStatic(bodyA)){
+				int numContacts = arb->numContacts;
+				cpContact *contacts = arb->contacts;
+				
+				// Restore contact values back to the space's contact buffer memory
+				arb->contacts = cpContactBufferGetArray(space);
+				memcpy(arb->contacts, contacts, numContacts*sizeof(cpContact));
+				cpSpacePushContacts(space, numContacts);
+				
+				// Reinsert the arbiter into the arbiter cache
+				cpShape *a = arb->a, *b = arb->b;
+				cpShape *shape_pair[] = {a, b};
+				cpHashValue arbHashID = CP_HASH_PAIR((cpHashValue)a, (cpHashValue)b);
+				cpHashSetInsert(space->cachedArbiters, arbHashID, shape_pair, arb, NULL);
+				
+				// Update the arbiter's state
+				arb->stamp = space->stamp;
+				arb->handler = cpSpaceLookupHandler(space, a->collision_type, b->collision_type);
+				cpArrayPush(space->arbiters, arb);
+				
+				cpfree(contacts);
+			}
+		}
+		
+		CP_BODY_FOREACH_CONSTRAINT(body, constraint){
+			cpBody *bodyA = constraint->a;
+			if(body == bodyA || cpBodyIsStatic(bodyA)) cpArrayPush(space->constraints, constraint);
+		}
 	}
 }
 
-static inline void
-componentNodeMerge(cpBody *a_root, cpBody *b_root)
+static void
+cpSpaceDeactivateBody(cpSpace *space, cpBody *body)
 {
-	if(a_root->node.rank < b_root->node.rank){
-		a_root->node.parent = b_root;
-	} else if(a_root->node.rank > b_root->node.rank){
-		b_root->node.parent = a_root;
-	} else if(a_root != b_root){
-		b_root->node.parent = a_root;
-		a_root->node.rank++;
+	cpAssertHard(!cpBodyIsRogue(body), "Internal error: Attempting to deactivate a rouge body.");
+	
+	cpArrayDeleteObj(space->bodies, body);
+	
+	CP_BODY_FOREACH_SHAPE(body, shape){
+		cpSpatialIndexRemove(space->activeShapes, shape, shape->hashid);
+		cpSpatialIndexInsert(space->staticShapes, shape, shape->hashid);
+	}
+	
+	CP_BODY_FOREACH_ARBITER(body, arb){
+		cpBody *bodyA = arb->body_a;
+		if(body == bodyA || cpBodyIsStatic(bodyA)){
+			cpSpaceUncacheArbiter(space, arb);
+			
+			// Save contact values to a new block of memory so they won't time out
+			size_t bytes = arb->numContacts*sizeof(cpContact);
+			cpContact *contacts = (cpContact *)cpcalloc(1, bytes);
+			memcpy(contacts, arb->contacts, bytes);
+			arb->contacts = contacts;
+		}
+	}
+		
+	CP_BODY_FOREACH_CONSTRAINT(body, constraint){
+		cpBody *bodyA = constraint->a;
+		if(body == bodyA || cpBodyIsStatic(bodyA)) cpArrayDeleteObj(space->constraints, constraint);
 	}
 }
 
-void
-cpSpaceActivateBody(cpSpace *space, cpBody *body)
+static inline cpBody *
+ComponentRoot(cpBody *body)
 {
-	if(space->locked){
-		// cpSpaceActivateBody() is called again once the space is unlocked
-		cpArrayPush(space->rousedBodies, body);
-	} else {
-		cpArrayPush(space->bodies, body);
-		for(cpShape *shape=body->shapesList; shape; shape=shape->next){
-			cpSpaceHashRemove(space->staticShapes, shape, shape->hashid);
-			cpSpaceHashInsert(space->activeShapes, shape, shape->hashid, shape->bb);
-		}
-	}
+	return (body ? body->node.root : NULL);
 }
 
 static inline void
-componentActivate(cpBody *root)
+ComponentActivate(cpBody *root)
 {
-	if(!cpBodyIsSleeping(root)) return;
+	if(!root || !cpBodyIsSleeping(root)) return;
+	cpAssertHard(!cpBodyIsRogue(root), "Internal Error: ComponentActivate() called on a rogue body.");
 	
 	cpSpace *space = root->space;
-	cpAssert(space, "Trying to activate a body that was never added to a space.");
-	
-	cpBody *body = root, *next;
-	do {
-		next = body->node.next;
-		
-		cpComponentNode node = {NULL, NULL, 0, 0.0f};
-		body->node = node;
+	cpBody *body = root;
+	while(body){
+		cpBody *next = body->node.next;
 		
+		body->node.idleTime = 0.0f;
+		body->node.root = NULL;
+		body->node.next = NULL;
 		cpSpaceActivateBody(space, body);
-	} while((body = next) != root);
+		
+		body = next;
+	}
 	
 	cpArrayDeleteObj(space->sleepingComponents, root);
 }
@@ -94,136 +141,167 @@ componentActivate(cpBody *root)
 void
 cpBodyActivate(cpBody *body)
 {
-	componentActivate(componentNodeRoot(body));
+	if(!cpBodyIsRogue(body)){
+		body->node.idleTime = 0.0f;
+		ComponentActivate(ComponentRoot(body));
+	}
+	
+	CP_BODY_FOREACH_ARBITER(body, arb){
+		// Reset the idle timer of things the body is touching as well.
+		// That way things don't get left hanging in the air.
+		cpBody *other = (arb->body_a == body ? arb->body_b : arb->body_a);
+		if(!cpBodyIsStatic(other)) other->node.idleTime = 0.0f;
+	}
 }
 
-static inline void
-mergeBodies(cpSpace *space, cpArray *components, cpArray *rogueBodies, cpBody *a, cpBody *b)
+void
+cpBodyActivateStatic(cpBody *body, cpShape *filter)
 {
-	// Ignore connections to static bodies
-	if(cpBodyIsStatic(a) || cpBodyIsStatic(b)) return;
-	
-	cpBody *a_root = componentNodeRoot(a);
-	cpBody *b_root = componentNodeRoot(b);
-	
-	cpBool a_sleep = cpBodyIsSleeping(a_root);
-	cpBool b_sleep = cpBodyIsSleeping(b_root);
+	cpAssertHard(cpBodyIsStatic(body), "cpBodyActivateStatic() called on a non-static body.");
 	
-	if(a_sleep && b_sleep){
-		return;
-	} else if(a_sleep || b_sleep){
-		componentActivate(a_root);
-		componentActivate(b_root);
-	} 
-	
-	// Add any rogue bodies found to the list and reset the idle time of anything they touch.
-	if(cpBodyIsRogue(a)){ cpArrayPush(rogueBodies, a); b->node.idleTime = 0.0f; }
-	if(cpBodyIsRogue(b)){ cpArrayPush(rogueBodies, b); a->node.idleTime = 0.0f; }
+	CP_BODY_FOREACH_ARBITER(body, arb){
+		if(!filter || filter == arb->a || filter == arb->b){
+			cpBodyActivate(arb->body_a == body ? arb->body_b : arb->body_a);
+		}
+	}
 	
-	componentNodeMerge(a_root, b_root);
+	// TODO should also activate joints?
 }
 
-static inline cpBool
-componentActive(cpBody *root, cpFloat threshold)
+static inline void
+cpBodyPushArbiter(cpBody *body, cpArbiter *arb)
 {
-	cpBody *body = root, *next;
-	do {
-		next = body->node.next;
-		if(body->node.idleTime < threshold) return cpTrue;
-	} while((body = next) != root);
+	cpAssertSoft(cpArbiterThreadForBody(arb, body)->next == NULL, "Internal Error: Dangling contact graph pointers detected. (A)");
+	cpAssertSoft(cpArbiterThreadForBody(arb, body)->prev == NULL, "Internal Error: Dangling contact graph pointers detected. (B)");
 	
-	return cpFalse;
+	cpArbiter *next = body->arbiterList;
+	cpAssertSoft(next == NULL || cpArbiterThreadForBody(next, body)->prev == NULL, "Internal Error: Dangling contact graph pointers detected. (C)");
+	cpArbiterThreadForBody(arb, body)->next = next;
+	
+	if(next) cpArbiterThreadForBody(next, body)->prev = arb;
+	body->arbiterList = arb;
 }
 
 static inline void
-addToComponent(cpBody *body, cpArray *components)
-{
-	// Check that the body is not already added to the component list
-	if(body->node.next) return;
-	cpBody *root = componentNodeRoot(body);
-	
-	cpBody *next = root->node.next;
-	if(!next){
-		// If the root isn't part of a list yet, then it hasn't been
-		// added to the components list. Do that now.
-		cpArrayPush(components, root);
-		// Start the list
-		body->node.next = root;
-		root->node.next = body;
-	} else if(root != body) {
-		// Splice in body after the root.
-		body->node.next = next;
+ComponentAdd(cpBody *root, cpBody *body){
+	body->node.root = root;
+
+	if(body != root){
+		body->node.next = root->node.next;
 		root->node.next = body;
 	}
 }
 
-// TODO this function needs more commenting.
+static inline void
+FloodFillComponent(cpBody *root, cpBody *body)
+{
+	// Rogue bodies cannot be put to sleep and prevent bodies they are touching from sleepining anyway.
+	// Static bodies (which are a type of rogue body) are effectively sleeping all the time.
+	if(!cpBodyIsRogue(body)){
+		cpBody *other_root = ComponentRoot(body);
+		if(other_root == NULL){
+			ComponentAdd(root, body);
+			CP_BODY_FOREACH_ARBITER(body, arb) FloodFillComponent(root, (body == arb->body_a ? arb->body_b : arb->body_a));
+			CP_BODY_FOREACH_CONSTRAINT(body, constraint) FloodFillComponent(root, (body == constraint->a ? constraint->b : constraint->a));
+		} else {
+			cpAssertSoft(other_root == root, "Internal Error: Inconsistency dectected in the contact graph.");
+		}
+	}
+}
+
+static inline cpBool
+ComponentActive(cpBody *root, cpFloat threshold)
+{
+	CP_BODY_FOREACH_COMPONENT(root, body){
+		if(body->node.idleTime < threshold) return cpTrue;
+	}
+	
+	return cpFalse;
+}
+
 void
 cpSpaceProcessComponents(cpSpace *space, cpFloat dt)
 {
+	cpBool sleep = (space->sleepTimeThreshold != INFINITY);
 	cpArray *bodies = space->bodies;
-	cpArray *newBodies = cpArrayNew(bodies->num);
-	cpArray *rogueBodies = cpArrayNew(16);
-	cpArray *arbiters = space->arbiters;
-	cpArray *constraints = space->constraints;
-	cpArray *components = cpArrayNew(space->sleepingComponents->num);
-	
-	cpFloat dv = space->idleSpeedThreshold;
-	cpFloat dvsq = (dv ? dv*dv : cpvdot(space->gravity, space->gravity)*dt*dt);
 	
-	// update idling
+#ifndef NDEBUG
 	for(int i=0; i<bodies->num; i++){
 		cpBody *body = (cpBody*)bodies->arr[i];
 		
-		cpFloat thresh = (dvsq ? body->m*dvsq : 0.0f);
-		body->node.idleTime = (cpBodyKineticEnergy(body) > thresh ? 0.0f : body->node.idleTime + dt);
+		cpAssertSoft(body->node.next == NULL, "Internal Error: Dangling next pointer detected in contact graph.");
+		cpAssertSoft(body->node.root == NULL, "Internal Error: Dangling root pointer detected in contact graph.");
 	}
+#endif
 	
-	// iterate graph edges and build forests
-	for(int i=0; i<arbiters->num; i++){
-		cpArbiter *arb = (cpArbiter*)arbiters->arr[i];
-		mergeBodies(space, components, rogueBodies, arb->a->body, arb->b->body);
-	}
-	for(int j=0; j<constraints->num; j++){
-		cpConstraint *constraint = (cpConstraint *)constraints->arr[j];
-		mergeBodies(space, components, rogueBodies, constraint->a, constraint->b);
+	// Calculate the kinetic energy of all the bodies.
+	if(sleep){
+		cpFloat dv = space->idleSpeedThreshold;
+		cpFloat dvsq = (dv ? dv*dv : cpvlengthsq(space->gravity)*dt*dt);
+		
+		// update idling and reset component nodes
+		for(int i=0; i<bodies->num; i++){
+			cpBody *body = (cpBody*)bodies->arr[i];
+			
+			// Need to deal with infinite mass objects
+			cpFloat keThreshold = (dvsq ? body->m*dvsq : 0.0f);
+			body->node.idleTime = (cpBodyKineticEnergy(body) > keThreshold ? 0.0f : body->node.idleTime + dt);
+		}
 	}
 	
-	// iterate bodies and add them to their components
-	for(int i=0; i<bodies->num; i++) addToComponent((cpBody*)bodies->arr[i], components);
-	for(int i=0; i<rogueBodies->num; i++) addToComponent((cpBody*)rogueBodies->arr[i], components);
+	// Awaken any sleeping bodies found and then push arbiters to the bodies' lists.
+	cpArray *arbiters = space->arbiters;
+	for(int i=0, count=arbiters->num; i<count; i++){
+		cpArbiter *arb = (cpArbiter*)arbiters->arr[i];
+		cpBody *a = arb->body_a, *b = arb->body_b;
+		
+		if(sleep){
+			if((cpBodyIsRogue(b) && !cpBodyIsStatic(b)) || cpBodyIsSleeping(a)) cpBodyActivate(a);
+			if((cpBodyIsRogue(a) && !cpBodyIsStatic(a)) || cpBodyIsSleeping(b)) cpBodyActivate(b);
+		}
+		
+		cpBodyPushArbiter(a, arb);
+		cpBodyPushArbiter(b, arb);
+	}
 	
-	// iterate components, copy or deactivate
-	for(int i=0; i<components->num; i++){
-		cpBody *root = (cpBody*)components->arr[i];
-		if(componentActive(root, space->sleepTimeThreshold)){
-			cpBody *body = root, *next;
-			do {
-				next = body->node.next;
-				
-				if(!cpBodyIsRogue(body)) cpArrayPush(newBodies, body);
-				cpComponentNode node = {NULL, NULL, 0, body->node.idleTime};
-				body->node = node;
-			} while((body = next) != root);
-		} else {
-			cpBody *body = root, *next;
-			do {
-				next = body->node.next;
+	if(sleep){
+		// Bodies should be held active if connected by a joint to a non-static rouge body.
+		cpArray *constraints = space->constraints;
+		for(int i=0; i<constraints->num; i++){
+			cpConstraint *constraint = (cpConstraint *)constraints->arr[i];
+			cpBody *a = constraint->a, *b = constraint->b;
+			
+			if(cpBodyIsRogue(b) && !cpBodyIsStatic(b)) cpBodyActivate(a);
+			if(cpBodyIsRogue(a) && !cpBodyIsStatic(a)) cpBodyActivate(b);
+		}
+		
+		// Generate components and deactivate sleeping ones
+		for(int i=0; i<bodies->num;){
+			cpBody *body = (cpBody*)bodies->arr[i];
+			
+			if(ComponentRoot(body) == NULL){
+				// Body not in a component yet. Perform a DFS to flood fill mark 
+				// the component in the contact graph using this body as the root.
+				FloodFillComponent(body, body);
 				
-				for(cpShape *shape = body->shapesList; shape; shape = shape->next){
-					cpSpaceHashRemove(space->activeShapes, shape, shape->hashid);
-					cpSpaceHashInsert(space->staticShapes, shape, shape->hashid, shape->bb);
+				// Check if the component should be put to sleep.
+				if(!ComponentActive(body, space->sleepTimeThreshold)){
+					cpArrayPush(space->sleepingComponents, body);
+					CP_BODY_FOREACH_COMPONENT(body, other) cpSpaceDeactivateBody(space, other);
+					
+					// cpSpaceDeactivateBody() removed the current body from the list.
+					// Skip incrementing the index counter.
+					continue;
 				}
-			} while((body = next) != root);
+			}
 			
-			cpArrayPush(space->sleepingComponents, root);
+			i++;
+			
+			// Only sleeping bodies retain their component node pointers.
+			body->node.root = NULL;
+			body->node.next = NULL;
 		}
 	}
-	
-	space->bodies = newBodies;
-	cpArrayFree(bodies);
-	cpArrayFree(rogueBodies);
-	cpArrayFree(components);
 }
 
 void
@@ -234,29 +312,29 @@ cpBodySleep(cpBody *body)
 
 void
 cpBodySleepWithGroup(cpBody *body, cpBody *group){
-	cpAssert(!cpBodyIsStatic(body) && !cpBodyIsRogue(body), "Rogue and static bodies cannot be put to sleep.");
+	cpAssertHard(!cpBodyIsRogue(body), "Rogue (and static) bodies cannot be put to sleep.");
 	
 	cpSpace *space = body->space;
-	cpAssert(space, "Cannot put a body to sleep that has not been added to a space.");
-	cpAssert(!space->locked, "Bodies can not be put to sleep during a query or a call to cpSpaceSte(). Put these calls into a post-step callback.");
-	cpAssert(!group || cpBodyIsSleeping(group), "Cannot use a non-sleeping body as a group identifier.");
-	
-	if(cpBodyIsSleeping(body)) return;
+	cpAssertHard(!space->locked, "Bodies cannot be put to sleep during a query or a call to cpSpaceStep(). Put these calls into a post-step callback.");
+	cpAssertHard(group == NULL || cpBodyIsSleeping(group), "Cannot use a non-sleeping body as a group identifier.");
 	
-	for(cpShape *shape = body->shapesList; shape; shape = shape->next){
-		cpShapeCacheBB(shape);
-		cpSpaceHashRemove(space->activeShapes, shape, shape->hashid);
-		cpSpaceHashInsert(space->staticShapes, shape, shape->hashid, shape->bb);
+	if(cpBodyIsSleeping(body)){
+		cpAssertHard(ComponentRoot(body) == ComponentRoot(group), "The body is already sleeping and it's group cannot be reassigned.");
+		return;
 	}
 	
+	CP_BODY_FOREACH_SHAPE(body, shape) cpShapeUpdate(shape, body->p, body->rot);
+	cpSpaceDeactivateBody(space, body);
+	
 	if(group){
-		cpBody *root = componentNodeRoot(group);
+		cpBody *root = ComponentRoot(group);
 		
-		cpComponentNode node = {root, root->node.next, 0, 0.0f};
+		cpComponentNode node = {root, root->node.next, 0.0f};
 		body->node = node;
+		
 		root->node.next = body;
 	} else {
-		cpComponentNode node = {NULL, body, 0, 0.0f};
+		cpComponentNode node = {body, NULL, 0.0f};
 		body->node = node;
 		
 		cpArrayPush(space->sleepingComponents, body);
@@ -266,14 +344,13 @@ cpBodySleepWithGroup(cpBody *body, cpBody *group){
 }
 
 static void
-activateTouchingHelper(cpShape *shape, cpContactPointSet *points, cpArray **bodies){
+activateTouchingHelper(cpShape *shape, cpContactPointSet *points, cpShape *other){
 	cpBodyActivate(shape->body);
 }
 
 void
 cpSpaceActivateShapesTouchingShape(cpSpace *space, cpShape *shape){
-	cpArray *bodies = NULL;
-	cpSpaceShapeQuery(space, shape, (cpSpaceShapeQueryFunc)activateTouchingHelper, &bodies);
+	if(space->sleepTimeThreshold != INFINITY){
+		cpSpaceShapeQuery(space, shape, (cpSpaceShapeQueryFunc)activateTouchingHelper, shape);
+	}
 }
-
-