// Hashset filter func to call and throw away post step callbacks. static int postStepCallbackSetFilter(postStepCallback *callback, cpSpace *space) { callback->func(space, callback->obj, callback->data); cpfree(callback); return 0; }
void cpArbiterFree(cpArbiter *arb) { if(arb){ cpArbiterDestroy(arb); cpfree(arb); } }
void cpSpaceFree(cpSpace *space) { if(space) { cpSpaceDestroy(space); cpfree(space); } }
void cpSpaceRemoveCollisionHandler(cpSpace *space, cpCollisionType a, cpCollisionType b) { cpAssertSpaceUnlocked(space); struct{cpCollisionType a, b;} ids = {a, b}; cpCollisionHandler *old_handler = (cpCollisionHandler *) cpHashSetRemove(space->collFuncSet, CP_HASH_PAIR(a, b), &ids); cpfree(old_handler); }
// Frees the old table, and allocates a new one. static void cpSpaceHashAllocTable(cpSpaceHash *hash, int numcells) { if(hash->table) cpfree(hash->table); hash->numcells = numcells; hash->table = (cpSpaceHashBin **)cpcalloc(numcells, sizeof(cpSpaceHashBin *)); }
void cpHashSetDestroy(cpHashSet *set) { // Free the table. cpfree(set->table); if(set->allocatedBuffers) cpArrayEach(set->allocatedBuffers, freeWrap, NULL); cpArrayFree(set->allocatedBuffers); }
// Free the recycled hash bins. static void freeBins(cpSpaceHash *hash) { cpSpaceHashBin *bin = hash->bins; while(bin){ cpSpaceHashBin *next = bin->next; cpfree(bin); bin = next; } }
void cpPolylineSetDestroy(cpPolylineSet *set, cpBool freePolylines) { if(freePolylines){ for(int i=0; i<set->count; i++){ cpPolylineFree(set->lines[i]); } } cpfree(set->lines); }
static void cpSpaceHashDestroy(cpSpaceHash *hash) { if(hash->table) clearTable(hash); cpfree(hash->table); cpHashSetFree(hash->handleSet); cpArrayFreeEach(hash->allocatedBuffers, cpfree); cpArrayFree(hash->allocatedBuffers); cpArrayFree(hash->pooledHandles); }
void cpSpaceHashDestroy(cpSpaceHash *hash) { clearHash(hash); freeBins(hash); // Free the handles. cpHashSetEach(hash->handleSet, &handleFreeWrap, NULL); cpHashSetFree(hash->handleSet); cpfree(hash->table); }
void cpSpaceHashDestroy(cpSpaceHash *hash) { clearHash(hash); cpHashSetFree(hash->handleSet); cpArrayEach(hash->allocatedBuffers, freeWrap, NULL); cpArrayFree(hash->allocatedBuffers); cpArrayFree(hash->pooledHandles); cpfree(hash->table); }
void cpSpaceActivateBody(cpSpace *space, cpBody *body) { cpAssertHard(!cpBodyIsRogue(body), "Internal error: Attempting to activate a rogue body."); if(space->locked){ // cpSpaceActivateBody() is called again once the space is unlocked if(!cpArrayContains(space->rousedBodies, body)) cpArrayPush(space->rousedBodies, body); } else { cpAssertSoft(body->node.root == NULL && body->node.next == NULL, "Internal error: Activating body non-NULL node pointers."); cpArrayPush(space->bodies, body); CP_BODY_FOREACH_SHAPE(body, shape){ cpSpatialIndexRemove(space->staticShapes, shape, shape->hashid); cpSpatialIndexInsert(space->activeShapes, shape, shape->hashid); } CP_BODY_FOREACH_ARBITER(body, arb){ cpBody *bodyA = arb->body_a; // Arbiters are shared between two bodies that are always woken up together. // You only want to restore the arbiter once, so bodyA is arbitrarily chosen to own the arbiter. // The edge case is when static bodies are involved as the static bodies never actually sleep. // If the static body is bodyB then all is good. If the static body is bodyA, that can easily be checked. if(body == bodyA || cpBodyIsStatic(bodyA)){ int numContacts = arb->numContacts; cpContact *contacts = arb->contacts; // Restore contact values back to the space's contact buffer memory arb->contacts = cpContactBufferGetArray(space); memcpy(arb->contacts, contacts, numContacts*sizeof(cpContact)); cpSpacePushContacts(space, numContacts); // Reinsert the arbiter into the arbiter cache cpShape *a = arb->a, *b = arb->b; cpShape *shape_pair[] = {a, b}; cpHashValue arbHashID = CP_HASH_PAIR((cpHashValue)a, (cpHashValue)b); cpHashSetInsert(space->cachedArbiters, arbHashID, shape_pair, arb, NULL); // Update the arbiter's state arb->stamp = space->stamp; arb->handler = cpSpaceLookupHandler(space, a->collision_type, b->collision_type); cpArrayPush(space->arbiters, arb); cpfree(contacts); } }
// 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{cpCollisionType a, b;} ids = {a->collision_type, b->collision_type}; cpHashValue collHashID = CP_HASH_PAIR(a->collision_type, b->collision_type); cpCollisionHandler *handler = (cpCollisionHandler *)cpHashSetFind(space->collFuncSet, collHashID, &ids); int 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 = NULL; int numContacts = cpCollideShapes(a, b, &contacts); if(!numContacts) return; // Shapes are not colliding. // 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(a, b); cpArbiter *arb = (cpArbiter *)cpHashSetInsert(space->contactSet, arbHashID, shape_pair, NULL); cpArbiterUpdate(arb, contacts, numContacts, handler, a, b); // retains the contacts array // Call the begin function first if we need to int beginPass = (arb->stamp >= 0) || (handler->begin(arb, space, handler->data)); if(beginPass && handler->preSolve(arb, space, handler->data) && !sensor){ cpArrayPush(space->arbiters, arb); } else { cpfree(arb->contacts); arb->contacts = NULL; } // Time stamp the arbiter so we know it was used recently. arb->stamp = space->stamp; }
void cpSpaceActivateBody(cpSpace *space, cpBody *body) { cpAssertHard(!cpBodyIsRogue(body), "Internal error: Attempting to activate a rouge body."); if(space->locked){ // cpSpaceActivateBody() is called again once the space is unlocked if(!cpArrayContains(space->rousedBodies, body)) cpArrayPush(space->rousedBodies, body); } else { cpArrayPush(space->bodies, body); CP_BODY_FOREACH_SHAPE(body, shape){ cpSpatialIndexRemove(space->staticShapes, shape, shape->hashid); cpSpatialIndexInsert(space->activeShapes, shape, shape->hashid); } CP_BODY_FOREACH_ARBITER(body, arb){ cpBody *bodyA = arb->body_a; if(body == bodyA || cpBodyIsStatic(bodyA)){ int numContacts = arb->numContacts; cpContact *contacts = arb->contacts; // Restore contact values back to the space's contact buffer memory arb->contacts = cpContactBufferGetArray(space); memcpy(arb->contacts, contacts, numContacts*sizeof(cpContact)); cpSpacePushContacts(space, numContacts); // Reinsert the arbiter into the arbiter cache cpShape *a = arb->a, *b = arb->b; cpShape *shape_pair[] = {a, b}; cpHashValue arbHashID = CP_HASH_PAIR((cpHashValue)a, (cpHashValue)b); cpHashSetInsert(space->cachedArbiters, arbHashID, shape_pair, arb, NULL); // Update the arbiter's state arb->stamp = space->stamp; arb->handler = cpSpaceLookupHandler(space, a->collision_type, b->collision_type); cpArrayPush(space->arbiters, arb); cpfree(contacts); } }
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 cpSpaceUnlock(cpSpace *space, cpBool runPostStep) { space->locked--; cpAssertHard(space->locked >= 0, "Internal Error: Space lock underflow."); if(space->locked == 0){ cpArray *waking = space->rousedBodies; for(int i=0, count=waking->num; i<count; i++){ cpSpaceActivateBody(space, (cpBody *)waking->arr[i]); waking->arr[i] = NULL; } waking->num = 0; if(space->locked == 0 && runPostStep && !space->skipPostStep){ space->skipPostStep = cpTrue; cpArray *arr = space->postStepCallbacks; for(int i=0; i<arr->num; i++){ cpPostStepCallback *callback = (cpPostStepCallback *)arr->arr[i]; cpPostStepFunc func = callback->func; // Mark the func as NULL in case calling it calls cpSpaceRunPostStepCallbacks() again. // TODO: need more tests around this case I think. callback->func = NULL; if(func) func(space, callback->key, callback->data); arr->arr[i] = NULL; cpfree(callback); } arr->num = 0; space->skipPostStep = cpFalse; } } }
// The looping and sample caching code is shared between cpMarchHard() and cpMarchSoft(). static void cpMarchCells( cpBB bb, unsigned long x_samples, unsigned long y_samples, cpFloat t, cpMarchSegmentFunc segment, void *segment_data, cpMarchSampleFunc sample, void *sample_data, cpMarchCellFunc cell ){ cpFloat x_denom = 1.0/(cpFloat)(x_samples - 1); cpFloat y_denom = 1.0/(cpFloat)(y_samples - 1); // TODO range assertions and short circuit for 0 sized windows. // Keep a copy of the previous row to avoid double lookups. cpFloat *buffer = (cpFloat *)cpcalloc(x_samples, sizeof(cpFloat)); for(unsigned long i=0; i<x_samples; i++) buffer[i] = sample(cpv(cpflerp(bb.l, bb.r, i*x_denom), bb.b), sample_data); for(unsigned long j=0; j<y_samples-1; j++){ cpFloat y0 = cpflerp(bb.b, bb.t, (j+0)*y_denom); cpFloat y1 = cpflerp(bb.b, bb.t, (j+1)*y_denom); cpFloat a, b = buffer[0]; cpFloat c, d = sample(cpv(bb.l, y1), sample_data); buffer[0] = d; for(unsigned long i=0; i<x_samples-1; i++){ cpFloat x0 = cpflerp(bb.l, bb.r, (i+0)*x_denom); cpFloat x1 = cpflerp(bb.l, bb.r, (i+1)*x_denom); a = b, b = buffer[i + 1]; c = d, d = sample(cpv(x1, y1), sample_data); buffer[i + 1] = d; cell(t, a, b, c, d, x0, x1, y0, y1, segment, segment_data); } } cpfree(buffer); }
void cpArbiterUpdate(cpArbiter *arb, cpContact *contacts, int numContacts, cpCollisionHandler *handler, cpShape *a, cpShape *b) { // Arbiters without contact data may exist if a collision function rejected the collision. if(arb->contacts){ // Iterate over the possible pairs to look for hash value matches. for(int i=0; i<arb->numContacts; i++){ cpContact *old = &arb->contacts[i]; for(int j=0; j<numContacts; j++){ cpContact *new_contact = &contacts[j]; // This could trigger false positives, but is fairly unlikely nor serious if it does. if(new_contact->hash == old->hash){ // Copy the persistant contact information. new_contact->jnAcc = old->jnAcc; new_contact->jtAcc = old->jtAcc; } } } cpfree(arb->contacts); } arb->contacts = contacts; arb->numContacts = numContacts; arb->handler = handler; arb->swappedColl = (a->collision_type != handler->a); arb->e = a->e * b->e; arb->u = a->u * b->u; arb->surface_vr = cpvsub(a->surface_v, b->surface_v); // For collisions between two similar primitive types, the order could have been swapped. arb->a = a; arb->b = b; }
void cpHashSetReject(cpHashSet *set, cpHashSetRejectFunc func, void *data) { // Iterate over all the chains. for(int i=0; i<set->size; i++){ // The rest works similarly to cpHashSetRemove() above. cpHashSetBin **prev_ptr = &set->table[i]; cpHashSetBin *bin = set->table[i]; while(bin){ cpHashSetBin *next = bin->next; if(func(bin->elt, data)){ prev_ptr = &bin->next; } else { (*prev_ptr) = next; set->entries--; cpfree(bin); } bin = next; } } }
// 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); }
// Iterator functions for destructors. static void freeWrap(void *ptr, void *unused){ cpfree(ptr);}
void cpArbiterDestroy(cpArbiter *arb) { cpfree(arb->contacts); }
void cpBodyFree(cpBody *body) { if(body) cpBodyDestroy(body); cpfree(body); }
static void cpSweep1DDestroy(cpSweep1D *sweep) { cpfree(sweep->table); sweep->table = NULL; }
void cpPolylineFree(cpPolyline *line) { cpfree(line); }
static inline void cpHandleFree(cpHandle *hand) { cpfree(hand); }
static Node * partitionNodes(cpBBTree *tree, Node **nodes, int count) { if(count == 1){ return nodes[0]; } else if(count == 2) { return NodeNew(tree, nodes[0], nodes[1]); } // Find the AABB for these nodes cpBB bb = nodes[0]->bb; for(int i=1; i<count; i++) bb = cpBBMerge(bb, nodes[i]->bb); // Split it on it's longest axis cpBool splitWidth = (bb.r - bb.l > bb.t - bb.b); // Sort the bounds and use the median as the splitting point cpFloat *bounds = (cpFloat *)cpcalloc(count*2, sizeof(cpFloat)); if(splitWidth){ for(int i=0; i<count; i++){ bounds[2*i + 0] = nodes[i]->bb.l; bounds[2*i + 1] = nodes[i]->bb.r; } } else { for(int i=0; i<count; i++){ bounds[2*i + 0] = nodes[i]->bb.b; bounds[2*i + 1] = nodes[i]->bb.t; } } qsort(bounds, count*2, sizeof(cpFloat), (int (*)(const void *, const void *))cpfcompare); cpFloat split = (bounds[count - 1] + bounds[count])*0.5f; // use the medain as the split cpfree(bounds); // Generate the child BBs cpBB a = bb, b = bb; if(splitWidth) a.r = b.l = split; else a.t = b.b = split; // Partition the nodes int right = count; for(int left=0; left < right;){ Node *node = nodes[left]; if(cpBBMergedArea(node->bb, b) < cpBBMergedArea(node->bb, a)){ // if(cpBBProximity(node->bb, b) < cpBBProximity(node->bb, a)){ right--; nodes[left] = nodes[right]; nodes[right] = node; } else { left++; } } if(right == count){ Node *node = NULL; for(int i=0; i<count; i++) node = SubtreeInsert(node, nodes[i], tree); return node; } // Recurse and build the node! return NodeNew(tree, partitionNodes(tree, nodes, right), partitionNodes(tree, nodes + right, count - right) ); }
static void cpPolyShapeDestroy(cpPolyShape *poly) { cpfree(poly->verts); cpfree(poly->planes); }
void cpArrayDestroy(cpArray *arr) { cpfree(arr->arr); }
void cpHashSetFree(cpHashSet *set) { if(set) cpHashSetDestroy(set); cpfree(set); }