Mercurial > projects > chipmunkd
view trunk/chipmunkd/cpSpace.d @ 13:c03a41d47b60
- finished all constraints properties
- implemented cpAssertWarn the mixin way
author | Extrawurst |
---|---|
date | Fri, 03 Dec 2010 21:38:01 +0100 |
parents | 7ebbd4d05553 |
children | df4ebc8add66 |
line wrap: on
line source
// written in the D programming language module chipmunkd.cpSpace; import chipmunkd.chipmunk; import chipmunkd.chipmunk_types_h; import chipmunkd.cpVect,chipmunkd.cpVect_h; import chipmunkd.cpHashSet; import chipmunkd.cpCollision; import chipmunkd.cpBody; import chipmunkd.cpArray; import chipmunkd.cpShape; import chipmunkd.cpBB; import chipmunkd.cpArbiter; import chipmunkd.cpSpaceHash; import chipmunkd.constraints.cpConstraint; import chipmunkd.cpSpaceQuery; // Number of frames that contact information should persist. //extern cpTimestamp cp_contact_persistence; // User collision handler function types. alias cpBool function(cpArbiter *arb, cpSpace *space, void *data)cpCollisionBeginFunc; alias cpBool function(cpArbiter *arb, cpSpace *space, void *data)cpCollisionPreSolveFunc; alias void function(cpArbiter *arb, cpSpace *space, void *data)cpCollisionPostSolveFunc; alias void function(cpArbiter *arb, cpSpace *space, void *data)cpCollisionSeparateFunc; // Structure for holding collision pair function information. // Used internally. struct cpCollisionHandler { cpCollisionType a; cpCollisionType b; cpCollisionBeginFunc begin; cpCollisionPreSolveFunc preSolve; cpCollisionPostSolveFunc postSolve; cpCollisionSeparateFunc separate; void *data; } enum CP_MAX_CONTACTS_PER_ARBITER = 6; struct cpContactBufferHeader { cpTimestamp stamp; cpContactBufferHeader *next; uint numContacts; } struct cpSpace{ // *** User definable fields // 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. cpVect gravity; // 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; cpBool locked; // Time stamp. Is incremented on every call to cpSpaceStep(). cpTimestamp stamp; // The static and active shape spatial hashes. cpSpaceHash* staticShapes; cpSpaceHash* activeShapes; // List of bodies in the system. cpArray *bodies; // List of groups of sleeping bodies. cpArray *sleepingComponents; // List of active arbiters for the impulse solver. cpArray* arbiters, 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. cpContactBufferHeader* contactBuffersHead, _contactBuffersTail; // List of buffers to be free()ed when destroying the space. cpArray *allocatedBuffers; // Persistant contact set. cpHashSet *contactSet; // List of constraints in the system. cpArray *constraints; // Set of collisionpair functions. cpHashSet *collFuncSet; // Default collision handler. cpCollisionHandler defaultHandler; cpHashSet *postStepCallbacks; cpBody staticBody; } //// Basic allocation/destruction functions. //cpSpace* cpSpaceAlloc(void); //cpSpace* cpSpaceInit(cpSpace *space); //cpSpace* cpSpaceNew(void); // //void cpSpaceDestroy(cpSpace *space); //void cpSpaceFree(cpSpace *space); // //// Convenience function. Frees all referenced entities. (bodies, shapes and constraints) //void cpSpaceFreeChildren(cpSpace *space); // //// Collision handler management functions. //void cpSpaceSetDefaultCollisionHandler( // cpSpace *space, // cpCollisionBeginFunc begin, // cpCollisionPreSolveFunc preSolve, // cpCollisionPostSolveFunc postSolve, // cpCollisionSeparateFunc separate, // void *data //); //void cpSpaceAddCollisionHandler( // cpSpace *space, // cpCollisionType a, cpCollisionType b, // cpCollisionBeginFunc begin, // cpCollisionPreSolveFunc preSolve, // cpCollisionPostSolveFunc postSolve, // cpCollisionSeparateFunc separate, // void *data //); //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); // //void cpSpaceRemoveShape(cpSpace *space, cpShape *shape); //void cpSpaceRemoveStaticShape(cpSpace *space, cpShape *shape); //void cpSpaceRemoveBody(cpSpace *space, cpBody *body); //void cpSpaceRemoveConstraint(cpSpace *space, cpConstraint *constraint); // //// Post Step function definition alias void function(cpSpace *space, void *obj, void *data) cpPostStepFunc; //// 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); // //// Point query callback function alias void function(cpShape *shape, void *data) cpSpacePointQueryFunc; //void cpSpacePointQuery(cpSpace *space, cpVect point, cpLayers layers, cpGroup group, cpSpacePointQueryFunc func, void *data); //cpShape *cpSpacePointQueryFirst(cpSpace *space, cpVect point, cpLayers layers, cpGroup group); // //// Segment query callback function alias void function(cpShape *shape, cpFloat t, cpVect n, void *data)cpSpaceSegmentQueryFunc; //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 alias void function(cpShape *shape, void *data)cpSpaceBBQueryFunc; //void cpSpaceBBQuery(cpSpace *space, cpBB bb, cpLayers layers, cpGroup group, cpSpaceBBQueryFunc func, void *data); // // //// Iterator function for iterating the bodies in a space. alias void function(cpBody *_body, void *data)cpSpaceBodyIterator; //void cpSpaceEachBody(cpSpace *space, cpSpaceBodyIterator 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); // //void cpSpaceRehashShape(cpSpace *space, cpShape *shape); // //// Update the space. //void cpSpaceStep(cpSpace *space, cpFloat dt); cpTimestamp cp_contact_persistence = 3; // Equal function for contactSet. 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)); } // Transformation function for contactSet. static void * contactSetTrans(cpShape **shapes, cpSpace *space) { if(space.pooledArbiters.num == 0){ // arbiter pool is exhausted, make more int count = CP_BUFFER_BYTES/cpArbiter.sizeof; assert(count, "Buffer size too small."); cpArbiter *buffer = cast(cpArbiter *)cpmalloc(CP_BUFFER_BYTES); cpArrayPush(space.allocatedBuffers, buffer); for(int i=0; i<count; i++) cpArrayPush(space.pooledArbiters, buffer + i); } return cpArbiterInit(cast(cpArbiter *) cpArrayPop(space.pooledArbiters), shapes[0], shapes[1]); } // Equals function for collFuncSet. static cpBool collFuncSetEql(cpCollisionHandler *check, cpCollisionHandler *pair) { return ((check.a == pair.a && check.b == pair.b) || (check.b == pair.a && check.a == pair.b)); } // Transformation function for collFuncSet. static void * collFuncSetTrans(cpCollisionHandler *handler, void *unused) { cpCollisionHandler *copy = cast(cpCollisionHandler *)cpmalloc(cpCollisionHandler.sizeof); (*copy) = (*handler); return copy; } // Default collision functions. static cpBool alwaysCollide(cpArbiter *arb, cpSpace *space, void *data){return 1;} static void nothing(cpArbiter *arb, cpSpace *space, void *data){} // BBfunc callback for the spatial hash. static cpBB shapeBBFunc(cpShape *shape){return shape.bb;} // Iterator functions for destructors. static void freeWrap(void *ptr, void *unused){ cpfree(ptr);} static void shapeFreeWrap(cpShape *ptr, void *unused){ cpShapeFree(ptr);} static void bodyFreeWrap(cpBody *ptr, void *unused){ cpBodyFree(ptr);} static void constraintFreeWrap(cpConstraint *ptr, void *unused){cpConstraintFree(ptr);} cpSpace * cpSpaceAlloc() { return cast(cpSpace *)cpcalloc(1, cpSpace.sizeof); } enum DEFAULT_DIM_SIZE = 100.0f; enum DEFAULT_COUNT = 1000; enum DEFAULT_ITERATIONS = 10; enum DEFAULT_ELASTIC_ITERATIONS = 0; cpCollisionHandler defaultHandler = {0, 0, &alwaysCollide, &alwaysCollide, ¬hing, ¬hing, null}; cpSpace* cpSpaceInit(cpSpace *space) { space.iterations = DEFAULT_ITERATIONS; space.elasticIterations = DEFAULT_ELASTIC_ITERATIONS; // space.sleepTicks = 300; space.gravity = cpvzero; space.damping = 1.0f; space.locked = 0; space.stamp = 0; space.staticShapes = cpSpaceHashNew(DEFAULT_DIM_SIZE, DEFAULT_COUNT, cast(cpSpaceHashBBFunc)&shapeBBFunc); space.activeShapes = cpSpaceHashNew(DEFAULT_DIM_SIZE, DEFAULT_COUNT, cast(cpSpaceHashBBFunc)&shapeBBFunc); space.allocatedBuffers = cpArrayNew(0); space.bodies = cpArrayNew(0); space.sleepingComponents = cpArrayNew(0); space.sleepTimeThreshold = INFINITY; space.idleSpeedThreshold = 0.0f; space.arbiters = cpArrayNew(0); space.pooledArbiters = cpArrayNew(0); space.contactBuffersHead = null; space.contactSet = cpHashSetNew(0, cast(cpHashSetEqlFunc)&contactSetEql, cast(cpHashSetTransFunc)&contactSetTrans); space.constraints = cpArrayNew(0); space.defaultHandler = defaultHandler; space.collFuncSet = cpHashSetNew(0, cast(cpHashSetEqlFunc)&collFuncSetEql, cast(cpHashSetTransFunc)&collFuncSetTrans); space.collFuncSet.default_value = &space.defaultHandler; space.postStepCallbacks = null; cpBodyInit(&space.staticBody, INFINITY, INFINITY); space.staticBody.space = space; return space; } cpSpace* cpSpaceNew() { return cpSpaceInit(cpSpaceAlloc()); } void cpSpaceDestroy(cpSpace *space) { cpSpaceHashFree(space.staticShapes); cpSpaceHashFree(space.activeShapes); cpArrayFree(space.bodies); cpArrayFree(space.sleepingComponents); cpArrayFree(space.constraints); cpHashSetFree(space.contactSet); cpArrayFree(space.arbiters); cpArrayFree(space.pooledArbiters); if(space.allocatedBuffers){ cpArrayEach(space.allocatedBuffers, &freeWrap, null); cpArrayFree(space.allocatedBuffers); } if(space.postStepCallbacks){ cpHashSetEach(space.postStepCallbacks, &freeWrap, null); cpHashSetFree(space.postStepCallbacks); } if(space.collFuncSet){ cpHashSetEach(space.collFuncSet, &freeWrap, null); cpHashSetFree(space.collFuncSet); } } void cpSpaceFree(cpSpace *space) { if(space){ cpSpaceDestroy(space); cpfree(space); } } void cpSpaceFreeChildren(cpSpace *space) { cpArray *components = space.sleepingComponents; while(components.num) cpBodyActivate(cast(cpBody *)components.arr[0]); cpSpaceHashEach(space.staticShapes, cast(cpSpaceHashIterator)&shapeFreeWrap, null); cpSpaceHashEach(space.activeShapes, cast(cpSpaceHashIterator)&shapeFreeWrap, null); cpArrayEach(space.bodies, cast(cpArrayIter)&bodyFreeWrap, null); cpArrayEach(space.constraints, cast(cpArrayIter)&constraintFreeWrap, null); } void cpSpaceAddCollisionHandler( cpSpace *space, cpCollisionType a, cpCollisionType b, cpCollisionBeginFunc begin, cpCollisionPreSolveFunc preSolve, cpCollisionPostSolveFunc postSolve, cpCollisionSeparateFunc separate, void *data ){ // Remove any old function so the new one will get added. cpSpaceRemoveCollisionHandler(space, a, b); cpCollisionHandler handler = { a, b, begin ? begin : &alwaysCollide, preSolve ? preSolve : &alwaysCollide, postSolve ? postSolve : ¬hing, separate ? separate : ¬hing, data }; cpHashSetInsert(space.collFuncSet, CP_HASH_PAIR(a, b), &handler, null); } void cpSpaceRemoveCollisionHandler(cpSpace *space, cpCollisionType a, cpCollisionType b) { struct tmp{cpCollisionType a, b;} tmp ids = {a, b}; cpCollisionHandler *old_handler = cast(cpCollisionHandler *) cpHashSetRemove(space.collFuncSet, CP_HASH_PAIR(a, b), &ids); cpfree(old_handler); } void cpSpaceSetDefaultCollisionHandler( cpSpace *space, cpCollisionBeginFunc begin, cpCollisionPreSolveFunc preSolve, cpCollisionPostSolveFunc postSolve, cpCollisionSeparateFunc separate, void *data ){ cpCollisionHandler handler = { 0, 0, begin ? begin : &alwaysCollide, preSolve ? preSolve : &alwaysCollide, postSolve ? postSolve : ¬hing, separate ? separate : ¬hing, data }; space.defaultHandler = handler; } //#pragma mark Body, Shape, and Joint Management void cpAssertSpaceUnlocked(cpSpace* _space){ assert(!_space.locked, "This addition/removal cannot be done safely during a call to cpSpaceStep(). " "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; } assert(node, "Attempted to remove a shape from a body it was never attached to."); (*prev_ptr) = node.next; } cpShape * cpSpaceAddShape(cpSpace *space, cpShape *shape) { cpBody *_body = shape._body; if(!_body || _body == &space.staticBody) return cpSpaceAddStaticShape(space, shape); assert(!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; } static void activateShapesTouchingShapeHelper(cpShape *shape, void *unused) { cpBodyActivate(shape._body); } static void activateShapesTouchingShape(cpSpace *space, cpShape *shape) { // TODO this query should be more precise // Use shape queries once they are written cpSpaceBBQuery(space, shape.bb, shape.layers, shape.group, &activateShapesTouchingShapeHelper, null); } cpShape * cpSpaceAddStaticShape(cpSpace *space, cpShape *shape) { assert(!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); activateShapesTouchingShape(space, shape); cpSpaceHashInsert(space.staticShapes, shape, shape.hashid, shape.bb); return shape; } cpBody * cpSpaceAddBody(cpSpace *space, cpBody *_body) { mixin(cpAssertWarn!("_body.m != INFINITY", "Did you really mean to add an infinite mass body to the space?",__FILE__,__LINE__)); assert(!_body.space, "Cannot add a body to a more than one space or to the same space twice."); // cpAssertSpaceUnlocked(space); This should be safe as long as it's not from an integration callback cpArrayPush(space.bodies, _body); _body.space = space; return _body; } cpConstraint * cpSpaceAddConstraint(cpSpace *space, cpConstraint *constraint) { assert(!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; } struct removalContext { cpSpace *space; cpShape *shape; } // Hashset filter func to throw away old arbiters. 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); cpArrayPush(context.space.pooledArbiters, arb); return cpFalse; } return cpTrue; } void cpSpaceRemoveShape(cpSpace *space, cpShape *shape) { cpBody *_body = shape._body; if(cpBodyIsStatic(_body)){ cpSpaceRemoveStaticShape(space, shape); return; } cpBodyActivate(_body); cpAssertSpaceUnlocked(space); mixin(cpAssertWarn!("cpHashSetFind(space.activeShapes.handleSet, shape.hashid, shape) !is null", "Cannot remove a shape that was never added to the space. (Removed twice maybe?)",__FILE__,__LINE__)); cpBodyRemoveShape(_body, shape); removalContext context = {space, shape}; cpHashSetFilter(space.contactSet, cast(cpHashSetFilterFunc)&contactSetFilterRemovedShape, &context); cpSpaceHashRemove(space.activeShapes, shape, shape.hashid); } void cpSpaceRemoveStaticShape(cpSpace *space, cpShape *shape) { mixin(cpAssertWarn!("cpHashSetFind(space.staticShapes.handleSet, shape.hashid, shape) !is null", "Cannot remove a static or sleeping shape that was never added to the space. (Removed twice maybe?)",__FILE__,__LINE__)); cpAssertSpaceUnlocked(space); removalContext context = {space, shape}; cpHashSetFilter(space.contactSet, cast(cpHashSetFilterFunc)&contactSetFilterRemovedShape, &context); cpSpaceHashRemove(space.staticShapes, shape, shape.hashid); activateShapesTouchingShape(space, shape); } void cpSpaceRemoveBody(cpSpace *space, cpBody *_body) { mixin(cpAssertWarn!("_body.space == space", "Cannot remove a body that was never added to the space. (Removed twice maybe?)")); cpAssertSpaceUnlocked(space); cpBodyActivate(_body); cpArrayDeleteObj(space.bodies, _body); _body.space = null; } void cpSpaceRemoveConstraint(cpSpace *space, cpConstraint *constraint) { mixin(cpAssertWarn!("cpArrayContains(space.constraints, constraint)", "Cannot remove a constraint that was never added to the space. (Removed twice maybe?)",__FILE__,__LINE__)); // cpAssertSpaceUnlocked(space); Should be safe as long as its not from a constraint callback. cpBodyActivate(constraint.a); cpBodyActivate(constraint.b); cpArrayDeleteObj(space.constraints, constraint); } //#pragma mark Spatial Hash Management static void updateBBCache(cpShape *shape, void *unused){cpShapeCacheBB(shape);} void cpSpaceResizeStaticHash(cpSpace *space, cpFloat dim, int count) { cpSpaceHashResize(space.staticShapes, dim, count); cpSpaceHashRehash(space.staticShapes); } void cpSpaceResizeActiveHash(cpSpace *space, cpFloat dim, int count) { cpSpaceHashResize(space.activeShapes, dim, count); } void cpSpaceRehashStatic(cpSpace *space) { cpSpaceHashEach(space.staticShapes, cast(cpSpaceHashIterator)&updateBBCache, null); cpSpaceHashRehash(space.staticShapes); } void cpSpaceRehashShape(cpSpace *space, cpShape *shape) { cpShapeCacheBB(shape); // attempt to rehash the shape in both hashes cpSpaceHashRehashObject(space.activeShapes, shape, shape.hashid); cpSpaceHashRehashObject(space.staticShapes, shape, shape.hashid); }