void
cpSpaceDestroy(cpSpace *space)
{
cpSpatialIndexFree(space->staticShapes);
cpSpatialIndexFree(space->activeShapes);
cpArrayFree(space->bodies);
cpArrayFree(space->sleepingComponents);
cpArrayFree(space->rousedBodies);
cpArrayFree(space->constraints);
cpHashSetFree(space->cachedArbiters);
cpArrayFree(space->arbiters);
cpArrayFree(space->pooledArbiters);
if(space->allocatedBuffers){
cpArrayFreeEach(space->allocatedBuffers, cpfree);
cpArrayFree(space->allocatedBuffers);
}
if(space->postStepCallbacks) cpHashSetEach(space->postStepCallbacks, freeWrap, NULL);
cpHashSetFree(space->postStepCallbacks);
if(space->collisionHandlers) cpHashSetEach(space->collisionHandlers, freeWrap, NULL);
cpHashSetFree(space->collisionHandlers);
}
void
cpSpaceDestroy(cpSpace *space)
{
cpSpaceHashFree(space->staticShapes);
cpSpaceHashFree(space->activeShapes);
cpArrayFree(space->bodies);
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
cpSpaceDestroy(cpSpace *space)
{
cpSpaceEachBody(space, (cpSpaceBodyIteratorFunc)cpBodyActivateWrap, NULL);
cpSpatialIndexFree(space->staticShapes);
cpSpatialIndexFree(space->dynamicShapes);
cpArrayFree(space->dynamicBodies);
cpArrayFree(space->otherBodies);
cpArrayFree(space->sleepingComponents);
cpArrayFree(space->rousedBodies);
cpArrayFree(space->constraints);
cpHashSetFree(space->cachedArbiters);
cpArrayFree(space->arbiters);
cpArrayFree(space->pooledArbiters);
if(space->allocatedBuffers){
cpArrayFreeEach(space->allocatedBuffers, cpfree);
cpArrayFree(space->allocatedBuffers);
}
if(space->postStepCallbacks){
cpArrayFreeEach(space->postStepCallbacks, cpfree);
cpArrayFree(space->postStepCallbacks);
}
if(space->collisionHandlers) cpHashSetEach(space->collisionHandlers, FreeWrap, NULL);
cpHashSetFree(space->collisionHandlers);
}
void
cpSpaceHashQueryRehash(cpSpaceHash *hash, cpSpaceHashQueryFunc func, void *data)
{
clearHash(hash);
queryRehashPair pair = {hash, func, data};
cpHashSetEach(hash->handleSet, &handleQueryRehashHelper, &pair);
}
// Iterate over the objects in the spatial hash.
void
cpSpaceHashEach(cpSpaceHash *hash, cpSpaceHashIterator func, void *data)
{
// Bundle the callback up to send to the hashset iterator.
eachPair pair = {func, data};
cpHashSetEach(hash->handleSet, &eachHelper, &pair);
}
void
cpSpaceHashRehash(cpSpaceHash *hash)
{
clearHash(hash);
// Rehash all of the handles.
cpHashSetEach(hash->handleSet, &handleRehashHelper, hash);
}
static void
cpSpaceHashReindexQuery(cpSpaceHash *hash, cpSpatialIndexQueryFunc func, void *data)
{
clearTable(hash);
queryRehashContext context = {hash, func, data};
cpHashSetEach(hash->handleSet, (cpHashSetIteratorFunc)queryRehash_helper, &context);
cpSpatialIndexCollideStatic((cpSpatialIndex *)hash, hash->spatialIndex.staticIndex, func, data);
}
void
cpSpaceDestroy(cpSpace *space)
{
cpSpaceHashFree(space->staticShapes);
cpSpaceHashFree(space->activeShapes);
cpArrayFree(space->bodies);
cpArrayFree(space->joints);
if(space->contactSet)
cpHashSetEach(space->contactSet, &arbiterFreeWrap, NULL);
cpHashSetFree(space->contactSet);
cpArrayFree(space->arbiters);
if(space->collFuncSet)
cpHashSetEach(space->collFuncSet, &freeWrap, NULL);
cpHashSetFree(space->collFuncSet);
}
void
cpSpaceHashDestroy(cpSpaceHash *hash)
{
clearHash(hash);
freeBins(hash);
// Free the handles.
cpHashSetEach(hash->handleSet, &handleFreeWrap, NULL);
cpHashSetFree(hash->handleSet);
cpfree(hash->table);
}
static void
cpSpaceRunPostStepCallbacks(cpSpace *space)
{
// Loop because post step callbacks may add more post step callbacks directly or indirectly.
while(space->postStepCallbacks){
cpHashSet *callbacks = space->postStepCallbacks;
space->postStepCallbacks = NULL;
cpHashSetEach(callbacks, (cpHashSetIteratorFunc)cpSpacePostStepCallbackSetIter, space);
cpHashSetFree(callbacks);
}
}
static void
cpBBTreeReindexQuery(cpBBTree *tree, cpSpatialIndexQueryFunc func, void *data)
{
if(!tree->root) return;
// LeafUpdate() may modify tree->root. Don't cache it.
cpHashSetEach(tree->leaves, (cpHashSetIteratorFunc)LeafUpdate, tree);
cpSpatialIndex *staticIndex = tree->spatialIndex.staticIndex;
Node *staticRoot = (staticIndex && staticIndex->klass == Klass() ? ((cpBBTree *)staticIndex)->root : NULL);
MarkContext context = {tree, staticRoot, func, data};
MarkSubtree(tree->root, &context);
if(staticIndex && !staticRoot) cpSpatialIndexCollideStatic((cpSpatialIndex *)tree, staticIndex, func, data);
IncrementStamp(tree);
}
void
cpBBTreeOptimize(cpSpatialIndex *index)
{
if(index->klass != &klass){
cpAssertWarn(cpFalse, "Ignoring cpBBTreeOptimize() call to non-tree spatial index.");
return;
}
cpBBTree *tree = (cpBBTree *)index;
Node *root = tree->root;
if(!root) return;
int count = cpBBTreeCount(tree);
Node **nodes = (Node **)cpcalloc(count, sizeof(Node *));
Node **cursor = nodes;
cpHashSetEach(tree->leaves, (cpHashSetIteratorFunc)fillNodeArray, &cursor);
SubtreeRecycle(tree, root);
tree->root = partitionNodes(tree, nodes, count);
cpfree(nodes);
}
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;
// Empty the arbiter list.
space->arbiters->num = 0;
//i51AdeOsLog ( i51_LOG_DC , "for" ) ;
// 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.
//i51AdeOsLog ( i51_LOG_DC , "cpSpaceHashEach" ) ;
cpSpaceHashEach(space->activeShapes, (cpSpaceHashIterator)updateBBCache, NULL);
cpSpaceLock(space);
// Collide!
//i51AdeOsLog ( i51_LOG_DC , "cpSpacePushFreshContactBuffer" ) ;
cpSpacePushFreshContactBuffer(space);
if(space->staticShapes->handleSet->entries)
cpSpaceHashEach(space->activeShapes, (cpSpaceHashIterator)active2staticIter, space);
cpSpaceHashQueryRehash(space->activeShapes, (cpSpaceHashQueryFunc)queryFunc, space);
cpSpaceUnlock(space);
//i51AdeOsLog ( i51_LOG_DC , "sleepTimeThreshold" ) ;
// 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
//i51AdeOsLog ( i51_LOG_DC , "cpHashSetFilter" ) ;
cpHashSetFilter(space->contactSet, (cpHashSetFilterFunc)contactSetFilter, space);
// Prestep the arbiters.
//i51AdeOsLog ( i51_LOG_DC , "space->arbiters" ) ;
cpArray *arbiters = space->arbiters;
for(int i=0; i<arbiters->num; i++)
cpArbiterPreStep((cpArbiter *)arbiters->arr[i], dt_inv);
// Prestep the constraints.
//i51AdeOsLog ( i51_LOG_DC , "constraints->num=%d",constraints->num ) ;
for(int i=0; i<constraints->num; i++){
cpConstraint *constraint = (cpConstraint *)constraints->arr[i];
//i51AdeOsLog ( i51_LOG_DC , "i=%d,preStep=%X",i,constraint->klass->preStep) ;
constraint->klass->preStep(constraint, dt, dt_inv);
}
//i51AdeOsLog ( i51_LOG_DC , "space->elasticIterations=%d",space->elasticIterations ) ;
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.
//i51AdeOsLog ( i51_LOG_DC , "space->damping=%d",space->damping ) ;
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);
}
//i51AdeOsLog ( i51_LOG_DC , "arbiters->num=%d",arbiters->num) ;
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.
//i51AdeOsLog ( i51_LOG_DC , "space->iterations=%d",space->iterations) ;
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);
}
}
cpSpaceLock(space);
// run the post solve callbacks
//i51AdeOsLog ( i51_LOG_DC , "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;
}
cpSpaceUnlock(space);
// Run the post step callbacks
// Loop because post step callbacks may create more post step callbacks
//i51AdeOsLog ( i51_LOG_DC , "space->postStepCallbacks") ;
while(space->postStepCallbacks){
cpHashSet *callbacks = space->postStepCallbacks;
space->postStepCallbacks = NULL;
cpHashSetEach(callbacks, (cpHashSetIterFunc)postStepCallbackSetIter, space);
cpHashSetFree(callbacks);
}
// Increment the stamp.
space->stamp++;
//i51AdeOsLog ( i51_LOG_DC , "_____________OK_______________") ;
}
static void
cpSpaceHashRehash(cpSpaceHash *hash)
{
clearTable(hash);
cpHashSetEach(hash->handleSet, (cpHashSetIteratorFunc)rehash_helper, hash);
}
static void
cpBBTreeEach(cpBBTree *tree, cpSpatialIndexIteratorFunc func, void *data)
{
eachContext context = {func, data};
cpHashSetEach(tree->leaves, (cpHashSetIteratorFunc)each_helper, &context);
}
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++;
}
static void
cpSpaceHashEach(cpSpaceHash *hash, cpSpatialIndexIteratorFunc func, void *data)
{
eachContext context = {func, data};
cpHashSetEach(hash->handleSet, (cpHashSetIteratorFunc)eachHelper, &context);
}
