Mercurial > projects > chipmunkd
view trunk/chipmunkd/cpSpaceComponent.d @ 23:4ceef5833c8c
updated to chipmunk 5.3.3
author | Extrawurst |
---|---|
date | Fri, 10 Dec 2010 02:10:27 +0100 |
parents | df4ebc8add66 |
children | 80058cee1a77 |
line wrap: on
line source
// written in the D programming language module chipmunkd.cpSpaceComponent; import chipmunkd.chipmunk; // 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."); assert(!space.locked, "Bodies can not be awakened during a query or a call to cpSpaceSte(). Put these calls into a post-step callback."); cpBody *_body = root; cpBody *next; do { next = _body.node.next; cpFloat idleTime = (cpBodyIsStatic(_body) ? cast(cpFloat)INFINITY : 0.0f); cpComponentNode node = {null, null, 0, idleTime}; _body.node = node; if(!cpBodyIsRogue(_body)) 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. if(!cpBodyIsStatic(_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(cpBodyIsRogue(a)) cpArrayPush(rogueBodies, a); if(cpBodyIsRogue(b)) 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(_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.a._body, arb.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 cpBodySleep(cpBody *_body) { cpBodySleepWithGroup(_body, null); } void cpBodySleepWithGroup(cpBody *_body, cpBody *group){ assert(!cpBodyIsStatic(_body) && !cpBodyIsRogue(_body), "Rogue and static bodies cannot be put to sleep."); cpSpace *space = _body.space; assert(space, "Cannot put a body to sleep that has not been added to a space."); assert(!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."); assert(!group || cpBodyIsSleeping(group), "Cannot use a non-sleeping body as a group identifier."); if(cpBodyIsSleeping(_body)) return; 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); } if(group){ cpBody *root = componentNodeRoot(group); cpComponentNode node = {root, root.node.next, 0, 0.0f}; _body.node = node; root.node.next = _body; } else { cpComponentNode node = {null, _body, 0, 0.0f}; _body.node = node; cpArrayPush(space.sleepingComponents, _body); } cpArrayDeleteObj(space.bodies, _body); } static void activateTouchingHelper(cpShape *shape, cpContactPointSet *points, cpArray **bodies){ if(*bodies == null) (*bodies) = cpArrayNew(0); cpArrayPush(*bodies, shape._body); } void cpSpaceActivateShapesTouchingShape(cpSpace *space, cpShape *shape){ cpArray *bodies = null; cpSpaceShapeQuery(space, shape, cast(cpSpaceShapeQueryFunc)&activateTouchingHelper, &bodies); if(bodies){ for(int i=0; i<bodies.num; i++){ cpBody *_body = cast(cpBody *)bodies.arr[i]; cpBodyActivate(_body); } } }