--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/trunk/chipmunkd/cpSpaceComponent.d	Thu Dec 02 02:11:26 2010 +0100
@@ -0,0 +1,224 @@
+
+// written in the D programming language
+
+module chipmunkd.cpSpaceComponent;
+
+import chipmunkd.chipmunk;
+import chipmunkd.chipmunk_types_h;
+import chipmunkd.cpVect,chipmunkd.cpVect_h;
+import chipmunkd.cpBody;
+
+// 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 cpBody *
+componentNodeRoot(cpBody *_body)
+{
+	cpBody *parent = _body.node.parent;
+	
+	if(parent){
+		// path compression, attaches this node directly to the root
+		return (_body.node.parent = componentNodeRoot(parent));
+	} else {
+		return _body;
+	}
+}
+
+static void
+componentNodeMerge(cpBody *a_root, cpBody *b_root)
+{
+	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++;
+	}
+}
+
+static void
+componentActivate(cpBody *root)
+{
+	if(!cpBodyIsSleeping(root)) return;
+	
+	cpSpace *space = root.space;
+	assert(space, "Trying to activate a body that was never added to a space.");
+	
+	cpBody* _body = root;
+	cpBody* next;
+	do {
+		next = _body.node.next;
+		
+		cpComponentNode node = {null, null, 0, 0.0f};
+		_body.node = node;
+		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);
+		}
+	} while((_body = next) != root);
+	
+	cpArrayDeleteObj(space.sleepingComponents, root);
+}
+
+void
+cpBodyActivate(cpBody *_body)
+{
+	// Reset the idle time even if it's not in a currently sleeping component
+	// Like a body resting on or jointed to a rogue body.
+	_body.node.idleTime = 0.0f;
+	componentActivate(componentNodeRoot(_body));
+}
+
+static void
+mergeBodies(cpSpace *space, cpArray *components, cpArray *rogueBodies, cpBody *a, cpBody *b)
+{
+	// Don't merge with the static body
+	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);
+	
+	if(a_sleep && b_sleep){
+		return;
+	} else if(a_sleep || b_sleep){
+		componentActivate(a_root);
+		componentActivate(b_root);
+	} 
+	
+	// Add any rogue bodies (bodies not added to the space)
+	if(!a.space) cpArrayPush(rogueBodies, a);
+	if(!b.space) cpArrayPush(rogueBodies, b);
+	
+	componentNodeMerge(a_root, b_root);
+}
+
+static cpBool
+componentActive(cpBody *root, cpFloat threshold)
+{
+	cpBody *_body = root;
+	cpBody *next;
+	do {
+		next = _body.node.next;
+		if(cpBodyIsRogue(_body) || _body.node.idleTime < threshold) return cpTrue;
+	} while((_body = next) != root);
+	
+	return cpFalse;
+}
+
+static 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;
+		root.node.next = _body;
+	}
+}
+
+// TODO this function needs more commenting.
+void
+cpSpaceProcessComponents(cpSpace *space, cpFloat dt)
+{
+	cpArray *bodies = space.bodies;
+	cpArray *newBodies = cpArrayNew(bodies.num);
+	cpArray *rogueBodies = cpArrayNew(16);
+	cpArray *arbiters = space.arbiters;
+	cpArray *constraints = space.constraints;
+	cpArray *components = cpArrayNew(bodies.num/8);
+	
+	cpFloat dv = space.idleSpeedThreshold;
+	cpFloat dvsq = (dv ? dv*dv : cpvdot(space.gravity, space.gravity)*dt*dt);
+	// update idling
+	for(int i=0; i<bodies.num; i++){
+		cpBody *_body = cast(cpBody*)bodies.arr[i];
+		
+		cpFloat thresh = (dvsq ? _body.m*dvsq : 0.0f);
+		_body.node.idleTime = (cpBodyKineticEnergy(_body) > thresh ? 0.0f : _body.node.idleTime + dt);
+	}
+	
+	// iterate graph edges and build forests
+	for(int i=0; i<arbiters.num; i++){
+		cpArbiter *arb = cast(cpArbiter*)arbiters.arr[i];
+		mergeBodies(space, components, rogueBodies, arb.private_a._body, arb.private_b._body);
+	}
+	for(int j=0; j<constraints.num; j++){
+		cpConstraint *constraint = cast(cpConstraint *)constraints.arr[j];
+		mergeBodies(space, components, rogueBodies, constraint.a, constraint.b);
+	}
+	
+	// iterate bodies and add them to their components
+	for(int i=0; i<bodies.num; i++)
+		addToComponent(cast(cpBody*)bodies.arr[i], components);
+	for(int i=0; i<rogueBodies.num; i++)
+		addToComponent(cast(cpBody*)rogueBodies.arr[i], components);
+	
+	// iterate components, copy or deactivate
+	for(int i=0; i<components.num; i++){
+		cpBody *root = cast(cpBody*)components.arr[i];
+		if(componentActive(root, space.sleepTimeThreshold)){
+			cpBody *_body = root;
+			cpBody *next;
+			do {
+				next = _body.node.next;
+				
+				if(!cpBodyIsRogue(_body)) cpArrayPush(newBodies, _body);
+				_body.node.next = null;
+				_body.node.parent = null;
+				_body.node.rank = 0;
+			} while((_body = next) != root);
+		} else {
+			cpBody *_body = root;
+			cpBody *next;
+			do {
+				next = _body.node.next;
+				
+				for(cpShape *shape = _body.shapesList; shape; shape = shape.next){
+					cpSpaceHashRemove(space.activeShapes, shape, shape.hashid);
+					cpSpaceHashInsert(space.staticShapes, shape, shape.hashid, shape.bb);
+				}
+			} while((_body = next) != root);
+			
+			cpArrayPush(space.sleepingComponents, root);
+		}
+	}
+	
+	space.bodies = newBodies;
+	cpArrayFree(bodies);
+	cpArrayFree(rogueBodies);
+	cpArrayFree(components);
+}
+
+void
+cpSpaceSleepBody(cpSpace *space, cpBody *_body){
+	cpComponentNode node = {null, _body, 0, 0.0f};
+	_body.node = node;
+	
+	for(cpShape *shape = _body.shapesList; shape; shape = shape.next){
+		cpSpaceHashRemove(space.activeShapes, shape, shape.hashid);
+		
+		cpShapeCacheBB(shape);
+		cpSpaceHashInsert(space.staticShapes, shape, shape.hashid, shape.bb);
+	}
+	
+	cpArrayPush(space.sleepingComponents, _body);
+	cpArrayDeleteObj(space.bodies, _body);
+}