void
cpSpaceAddPostStepCallback(cpSpace *space, cpPostStepFunc func, void *obj, void *data)
{
cpAssertWarn(space->locked,
"Adding a post-step callback when the space is not locked is unnecessary. "
"Post-step callbacks will not called until the end of the next call to cpSpaceStep() or the next query.");
if(!space->postStepCallbacks){
space->postStepCallbacks = cpHashSetNew(0, (cpHashSetEqlFunc)postStepFuncSetEql);
}
cpPostStepCallback callback = {func, obj, data};
cpHashSetInsert(space->postStepCallbacks, (cpHashValue)obj, &callback, NULL, (cpHashSetTransFunc)postStepFuncSetTrans);
}
cpSpatialIndex *
cpBBTreeInit(cpBBTree *tree, cpSpatialIndexBBFunc bbfunc, cpSpatialIndex *staticIndex)
{
cpSpatialIndexInit((cpSpatialIndex *)tree, Klass(), bbfunc, staticIndex);
tree->velocityFunc = NULL;
tree->leaves = cpHashSetNew(0, (cpHashSetEqlFunc)leafSetEql);
tree->root = NULL;
tree->pooledNodes = NULL;
tree->allocatedBuffers = cpArrayNew(0);
tree->stamp = 0;
return (cpSpatialIndex *)tree;
}
cpSpaceHash*
cpSpaceHashInit(cpSpaceHash *hash, cpFloat celldim, int numcells, cpSpaceHashBBFunc bbfunc)
{
cpSpaceHashAllocTable(hash, next_prime(numcells));
hash->celldim = celldim;
hash->bbfunc = bbfunc;
hash->handleSet = cpHashSetNew(0, (cpHashSetEqlFunc)handleSetEql, (cpHashSetTransFunc)handleSetTrans);
hash->pooledHandles = cpArrayNew(0);
hash->pooledBins = NULL;
hash->allocatedBuffers = cpArrayNew(0);
hash->stamp = 1;
return hash;
}
cpSpatialIndex *
cpSpaceHashInit(cpSpaceHash *hash, cpFloat celldim, int numcells, cpSpatialIndexBBFunc bbfunc, cpSpatialIndex *staticIndex)
{
cpSpatialIndexInit((cpSpatialIndex *)hash, Klass(), bbfunc, staticIndex);
cpSpaceHashAllocTable(hash, next_prime(numcells));
hash->celldim = celldim;
hash->handleSet = cpHashSetNew(0, (cpHashSetEqlFunc)handleSetEql);
hash->pooledHandles = cpArrayNew(0);
hash->pooledBins = NULL;
hash->allocatedBuffers = cpArrayNew(0);
hash->stamp = 1;
return (cpSpatialIndex *)hash;
}
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 = 1;
// Empty the arbiter list.
space->arbiters->num = 0;
// Integrate positions.
for(int i=0; i<bodies->num; i++){
cpBody *body = (cpBody *)bodies->arr[i];
body->position_func(body, dt);
}
// Pre-cache BBoxes and shape data.
cpSpaceHashEach(space->activeShapes, (cpSpaceHashIterator)updateBBCache, NULL);
// Collide!
cpSpacePushNewContactBuffer(space);
cpSpaceHashEach(space->activeShapes, (cpSpaceHashIterator)active2staticIter, space);
cpSpaceHashQueryRehash(space->activeShapes, (cpSpaceHashQueryFunc)queryFunc, space);
// Clear out old cached arbiters and dispatch untouch functions
cpHashSetFilter(space->contactSet, (cpHashSetFilterFunc)contactSetFilter, space);
// Prestep the arbiters.
cpArray *arbiters = space->arbiters;
for(int i=0; i<arbiters->num; i++)
cpArbiterPreStep((cpArbiter *)arbiters->arr[i], dt_inv);
// Prestep the constraints.
for(int i=0; i<constraints->num; i++){
cpConstraint *constraint = (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((cpArbiter *)arbiters->arr[j], 1.0f);
for(int j=0; j<constraints->num; j++){
cpConstraint *constraint = (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 = (cpBody *)bodies->arr[i];
body->velocity_func(body, space->gravity, damping, dt);
}
for(int i=0; i<arbiters->num; i++)
cpArbiterApplyCachedImpulse((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((cpArbiter *)arbiters->arr[j], elasticCoef);
for(int j=0; j<constraints->num; j++){
cpConstraint *constraint = (cpConstraint *)constraints->arr[j];
constraint->klass->applyImpulse(constraint);
}
}
space->locked = 0;
// run the post solve callbacks
for(int i=0; i<arbiters->num; i++){
cpArbiter *arb = (cpArbiter *) arbiters->arr[i];
cpCollisionHandler *handler = arb->handler;
handler->postSolve(arb, space, handler->data);
arb->state = cpArbiterStateNormal;
}
// Run the post step callbacks
while(space->postStepCallbacks->entries){
cpHashSet *callbacks = space->postStepCallbacks;
space->postStepCallbacks = cpHashSetNew(0, (cpHashSetEqlFunc)postStepFuncSetEql, (cpHashSetTransFunc)postStepFuncSetTrans);
cpHashSetEach(callbacks, (cpHashSetIterFunc)postStepCallbackSetIter, space);
}
// cpFloat dvsq = cpvdot(space->gravity, space->gravity);
// dvsq *= dt*dt * space->damping*space->damping;
// for(int i=0; i<bodies->num; i++)
// cpBodyMarkLowEnergy(bodies->arr[i], dvsq, space->sleepTicks);
// Increment the stamp.
space->stamp++;
}
