Mercurial > projects > chipmunkd
view trunk/chipmunkd/cpSpaceStep.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 | 4ceef5833c8c |
line wrap: on
line source
// written in the D programming language module chipmunkd.cpSpaceStep; 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; // //#pragma mark Post Step Callback Functions struct postStepCallback { cpPostStepFunc func; void *obj; void *data; } static cpBool postStepFuncSetEql(postStepCallback *a, postStepCallback *b){ return a.obj == b.obj; } static void * postStepFuncSetTrans(postStepCallback *callback, void *ignored) { postStepCallback *value = cast(postStepCallback *)cpmalloc(postStepCallback.sizeof); (*value) = (*callback); return value; } void cpSpaceAddPostStepCallback(cpSpace *space, cpPostStepFunc func, void *obj, void *data) { if(!space.postStepCallbacks){ space.postStepCallbacks = cpHashSetNew(0, cast(cpHashSetEqlFunc)&postStepFuncSetEql, cast(cpHashSetTransFunc)&postStepFuncSetTrans); } postStepCallback callback = {func, obj, data}; cpHashSetInsert(space.postStepCallbacks, cast(cpHashValue)cast(size_t)obj, &callback, null); } //#pragma mark Contact Buffer Functions enum CP_CONTACTS_BUFFER_SIZE = ((CP_BUFFER_BYTES - cpContactBufferHeader.sizeof)/cpContact.sizeof); struct cpContactBuffer { cpContactBufferHeader header; cpContact[CP_CONTACTS_BUFFER_SIZE] contacts; } static cpContactBufferHeader * cpSpaceAllocContactBuffer(cpSpace *space) { cpContactBuffer *buffer = cast(cpContactBuffer *)cpmalloc(cpContactBuffer.sizeof); cpArrayPush(space.allocatedBuffers, buffer); return cast(cpContactBufferHeader *)buffer; } static cpContactBufferHeader * cpContactBufferHeaderInit(cpContactBufferHeader *header, cpTimestamp stamp, cpContactBufferHeader *splice) { header.stamp = stamp; header.next = (splice ? splice.next : header); header.numContacts = 0; return header; } static void cpSpacePushFreshContactBuffer(cpSpace *space) { cpTimestamp stamp = space.stamp; cpContactBufferHeader *head = space.contactBuffersHead; if(!head){ // No buffers have been allocated, make one space.contactBuffersHead = cpContactBufferHeaderInit(cpSpaceAllocContactBuffer(space), stamp, null); } else if(stamp - head.next.stamp > cp_contact_persistence){ // The tail buffer is available, rotate the ring cpContactBufferHeader *tail = head.next; space.contactBuffersHead = cpContactBufferHeaderInit(tail, stamp, tail); } else { // Allocate a new buffer and push it into the ring cpContactBufferHeader *buffer = cpContactBufferHeaderInit(cpSpaceAllocContactBuffer(space), stamp, head); space.contactBuffersHead = head.next = buffer; } } static cpContact * cpContactBufferGetArray(cpSpace *space) { 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. cpSpacePushFreshContactBuffer(space); } cpContactBufferHeader *head = space.contactBuffersHead; return &(cast(cpContactBuffer *)head).contacts[head.numContacts]; } static void cpSpacePushContacts(cpSpace *space, int count){ assert(count <= CP_MAX_CONTACTS_PER_ARBITER, "Internal error, too many contact point overflow!"); space.contactBuffersHead.numContacts += count; } static void cpSpacePopContacts(cpSpace *space, int count){ space.contactBuffersHead.numContacts -= count; } //#pragma mark Collision Detection Functions static cpBool 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 && 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 tmp{cpCollisionType a, b;} tmp ids = {a.collision_type, b.collision_type}; cpHashValue collHashID = CP_HASH_PAIR(a.collision_type, b.collision_type); cpCollisionHandler *handler = cast(cpCollisionHandler *)cpHashSetFind(space.collFuncSet, collHashID, &ids); cpBool 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; } // Narrow-phase collision detection. cpContact *contacts = cpContactBufferGetArray(space); int numContacts = cpCollideShapes(a, b, contacts); if(!numContacts) return; // Shapes are not colliding. cpSpacePushContacts(space, 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(cast(size_t)a, cast(size_t)b); cpArbiter *arb = cast(cpArbiter *)cpHashSetInsert(space.contactSet, arbHashID, shape_pair.ptr, 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.state == cpArbiterState.cpArbiterStateFirstColl && !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 != cpArbiterState.cpArbiterStateIgnore) && // Call preSolve handler.preSolve(arb, space, handler.data) && // Process, but don't add collisions for sensors. !sensor ){ cpArrayPush(space.arbiters, arb); } else { cpSpacePopContacts(space, numContacts); arb.contacts = null; arb.numContacts = 0; // Normally arbiters are set as used after calling the post-step callback. // However, post-step callbacks are not called for sensors or arbiters rejected from pre-solve. if(arb.state != cpArbiterState.cpArbiterStateIgnore) arb.state = cpArbiterState.cpArbiterStateNormal; } // 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, cast(cpSpaceHashQueryFunc)&queryFunc, space); } // Hashset filter func to throw away old arbiters. static cpBool contactSetFilter(cpArbiter *arb, cpSpace *space) { if(space.sleepTimeThreshold != INFINITY){ cpBody *a = arb.private_a._body; cpBody *b = arb.private_b._body; // both bodies are either static or sleeping cpBool sleepingNow = (cpBodyIsStatic(a) || cpBodyIsSleeping(a)) && (cpBodyIsStatic(b) || cpBodyIsSleeping(b)); if(sleepingNow){ arb.state = cpArbiterState.cpArbiterStateSleep; return cpTrue; } else if(arb.state == cpArbiterState.cpArbiterStateSleep){ // wake up the arbiter and continue as normal arb.state = cpArbiterState.cpArbiterStateNormal; // TODO is it possible that cpArbiterStateIgnore should be set here instead? } } cpTimestamp ticks = space.stamp - arb.stamp; // was used last frame, but not this one if(ticks >= 1 && arb.state != cpArbiterState.cpArbiterStateCached){ arb.handler.separate(arb, space, arb.handler.data); arb.state = cpArbiterState.cpArbiterStateCached; } if(ticks >= cp_contact_persistence){ arb.contacts = null; arb.numContacts = 0; cpArrayPush(space.pooledArbiters, arb); return cpFalse; } return cpTrue; } // Hashset filter func to call and throw away post step callbacks. static void postStepCallbackSetIter(postStepCallback *callback, cpSpace *space) { callback.func(space, callback.obj, callback.data); cpfree(callback); } //#pragma mark All Important cpSpaceStep() Function //void cpSpaceProcessComponents(cpSpace *space, cpFloat dt); static void updateBBCache(cpShape *shape, void *unused){cpShapeCacheBB(shape);} void cpSpaceStep(cpSpace *space, cpFloat dt) { 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 = cpTrue; // Empty the arbiter list. space.arbiters.num = 0; // Integrate positions. for(int i=0; i<bodies.num; i++){ cpBody *_body = cast(cpBody *)bodies.arr[i]; _body.position_func(_body, dt); } // Pre-cache BBoxes and shape data. cpSpaceHashEach(space.activeShapes, cast(cpSpaceHashIterator)&updateBBCache, null); // Collide! cpSpacePushFreshContactBuffer(space); if(space.staticShapes.handleSet.entries) cpSpaceHashEach(space.activeShapes, cast(cpSpaceHashIterator)&active2staticIter, space); cpSpaceHashQueryRehash(space.activeShapes, cast(cpSpaceHashQueryFunc)&queryFunc, space); // If body sleeping is enabled, do that now. if(space.sleepTimeThreshold != INFINITY){ cpSpaceProcessComponents(space, dt); bodies = space.bodies; // rebuilt by processContactComponents() } // Clear out old cached arbiters and dispatch untouch functions cpHashSetFilter(space.contactSet, cast(cpHashSetFilterFunc)&contactSetFilter, space); // Prestep the arbiters. cpArray *arbiters = space.arbiters; for(int i=0; i<arbiters.num; i++) cpArbiterPreStep(cast(cpArbiter *)arbiters.arr[i], dt_inv); // Prestep the constraints. for(int i=0; i<constraints.num; i++){ cpConstraint *constraint = cast(cpConstraint *)constraints.arr[i]; constraint.klass.preStep(constraint, dt, dt_inv); } for(int i=0; i<space.elasticIterations; i++){ for(int j=0; j<arbiters.num; j++) cpArbiterApplyImpulse(cast(cpArbiter *)arbiters.arr[j], 1.0f); for(int j=0; j<constraints.num; j++){ cpConstraint *constraint = cast(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 = cast(cpBody *)bodies.arr[i]; _body.velocity_func(_body, space.gravity, damping, dt); } for(int i=0; i<arbiters.num; i++) cpArbiterApplyCachedImpulse(cast(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(cast(cpArbiter *)arbiters.arr[j], elasticCoef); for(int j=0; j<constraints.num; j++){ cpConstraint *constraint = cast(cpConstraint *)constraints.arr[j]; constraint.klass.applyImpulse(constraint); } } space.locked = cpFalse; // run the post solve callbacks for(int i=0; i<arbiters.num; i++){ cpArbiter *arb = cast(cpArbiter *) arbiters.arr[i]; cpCollisionHandler *handler = arb.handler; handler.postSolve(arb, space, handler.data); arb.state = cpArbiterState.cpArbiterStateNormal; } // Run the post step callbacks // Loop because post step callbacks may create more post step callbacks while(space.postStepCallbacks){ cpHashSet *callbacks = space.postStepCallbacks; space.postStepCallbacks = null; cpHashSetEach(callbacks, cast(cpHashSetIterFunc)&postStepCallbackSetIter, space); cpHashSetFree(callbacks); } // Increment the stamp. space.stamp++; }