static inline void mergeBodies(cpSpace *space, cpArray *components, cpArray *rogueBodies, cpBody *a, cpBody *b) { // Don't merge with the static body if(cpBodyIsStatic(a) || cpBodyIsStatic(b)) return; cpBody *a_root = componentNodeRoot(a); cpBody *b_root = componentNodeRoot(b); cpBool a_sleep = cpBodyIsSleeping(a_root); cpBool b_sleep = cpBodyIsSleeping(b_root); if(a_sleep && b_sleep){ return; } else if(a_sleep || b_sleep){ componentActivate(a_root); componentActivate(b_root); } // Add any rogue bodies (bodies not added to the space) if(!a->space) cpArrayPush(rogueBodies, a); if(!b->space) cpArrayPush(rogueBodies, b); componentNodeMerge(a_root, b_root); }
static inline void mergeBodies(cpSpace *space, cpArray *components, cpArray *rogueBodies, cpBody *a, cpBody *b) { // Ignore connections to static bodies if(cpBodyIsStatic(a) || cpBodyIsStatic(b)) return; cpBody *a_root = componentNodeRoot(a); cpBody *b_root = componentNodeRoot(b); cpBool a_sleep = cpBodyIsSleeping(a_root); cpBool b_sleep = cpBodyIsSleeping(b_root); if(a_sleep && b_sleep){ return; } else if(a_sleep || b_sleep){ componentActivate(a_root); componentActivate(b_root); } // Add any rogue bodies found to the list and reset the idle time of anything they touch. if(cpBodyIsRogue(a)){ cpArrayPush(rogueBodies, a); b->node.idleTime = 0.0f; } if(cpBodyIsRogue(b)){ cpArrayPush(rogueBodies, b); a->node.idleTime = 0.0f; } componentNodeMerge(a_root, b_root); }
void cpSpaceActivateBody(cpSpace *space, cpBody *body) { if(space->locked){ // cpSpaceActivateBody() is called again once the space is unlocked cpArrayPush(space->rousedBodies, body); } else { cpArrayPush(space->bodies, body); for(cpShape *shape=body->shapesList; shape; shape=shape->next){ cpSpaceHashRemove(space->staticShapes, shape, shape->hashid); cpSpaceHashInsert(space->activeShapes, shape, shape->hashid, shape->bb); } } }
void cpBodySleepWithGroup(cpBody *body, cpBody *group){ cpAssert(!cpBodyIsStatic(body) && !cpBodyIsRogue(body), "Rogue and static bodies cannot be put to sleep."); cpSpace *space = body->space; cpAssert(space, "Cannot put a body to sleep that has not been added to a space."); cpAssert(!space->locked, "Bodies can not be put to sleep during a query or a call to cpSpaceSte(). Put these calls into a post-step callback."); cpAssert(!group || cpBodyIsSleeping(group), "Cannot use a non-sleeping body as a group identifier."); if(cpBodyIsSleeping(body)) return; for(cpShape *shape = body->shapesList; shape; shape = shape->next){ cpShapeCacheBB(shape); cpSpaceHashRemove(space->activeShapes, shape, shape->hashid); cpSpaceHashInsert(space->staticShapes, shape, shape->hashid, shape->bb); } if(group){ cpBody *root = componentNodeRoot(group); cpComponentNode node = {root, root->node.next, 0, 0.0f}; body->node = node; root->node.next = body; } else { cpComponentNode node = {NULL, body, 0, 0.0f}; body->node = node; cpArrayPush(space->sleepingComponents, body); } cpArrayDeleteObj(space->bodies, body); }
// Hashset filter func to throw away old arbiters. static cpBool cpSpaceArbiterSetFilter(cpArbiter *arb, cpSpace *space) { cpTimestamp ticks = space->stamp - arb->stamp; cpBody *a = arb->body_a, *b = arb->body_b; // TODO should make an arbiter state for this so it doesn't require filtering arbiters for dangling body pointers on body removal. // Preserve arbiters on sensors and rejected arbiters for sleeping objects. if( (cpBodyIsStatic(a) || cpBodyIsSleeping(a)) && (cpBodyIsStatic(b) || cpBodyIsSleeping(b)) ){ return cpTrue; } // Arbiter was used last frame, but not this one if(ticks >= 1 && arb->state != cpArbiterStateCached){ cpArbiterCallSeparate(arb, space); arb->state = cpArbiterStateCached; } if(ticks >= space->collisionPersistence){ arb->contacts = NULL; arb->numContacts = 0; cpArrayPush(space->pooledArbiters, arb); return cpFalse; } return cpTrue; }
static cpContactBufferHeader * cpSpaceAllocContactBuffer(cpSpace *space) { cpContactBuffer *buffer = (cpContactBuffer *)malloc(sizeof(cpContactBuffer)); cpArrayPush(space->allocatedBuffers, buffer); return (cpContactBufferHeader *)buffer; }
static cpBool cachedArbitersFilter(cpArbiter *arb, struct arbiterFilterContext *context) { cpShape *shape = context->shape; cpBody *body = context->body; // Match on the filter shape, or if it's NULL the filter body if( (body == arb->body_a && (shape == arb->a || shape == NULL)) || (body == arb->body_b && (shape == arb->b || shape == NULL)) ){ // Call separate when removing shapes. if(shape && arb->state != CP_ARBITER_STATE_CACHED){ // Invalidate the arbiter since one of the shapes was removed. arb->state = CP_ARBITER_STATE_INVALIDATED; cpCollisionHandler *handler = arb->handler; handler->separateFunc(arb, context->space, handler->userData); } cpArbiterUnthread(arb); cpArrayDeleteObj(context->space->arbiters, arb); cpArrayPush(context->space->pooledArbiters, arb); return cpFalse; } return cpTrue; }
static Pair * PairFromPool(cpBBTree *tree) { // Share the pool of the master tree. // TODO would be lovely to move the pairs stuff into an external data structure. tree = GetMasterTree(tree); Pair *pair = tree->pooledPairs; if(pair){ tree->pooledPairs = pair->a.next; return pair; } else { // Pool is exhausted, make more int count = CP_BUFFER_BYTES/sizeof(Pair); cpAssertHard(count, "Internal Error: Buffer size is too small."); Pair *buffer = (Pair *)cpcalloc(1, CP_BUFFER_BYTES); cpArrayPush(tree->allocatedBuffers, buffer); // push all but the first one, return the first instead for(int i=1; i<count; i++) PairRecycle(tree, buffer + i); return buffer; } }
// 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/sizeof(cpArbiter); cpAssert(count, "Buffer size too small."); cpArbiter *buffer = (cpArbiter *)cpmalloc(CP_BUFFER_BYTES); cpArrayPush(space->allocatedBuffers, buffer); for(int i=0; i<count; i++) cpArrayPush(space->pooledArbiters, buffer + i); } return cpArbiterInit((cpArbiter *) cpArrayPop(space->pooledArbiters), shapes[0], shapes[1]); }
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; cpCollisionHandler *handler = lookupCollisionHandler(space, a->collision_type, b->collision_type); cpBool sensor = a->sensor || b->sensor; if(sensor && handler == &cpSpaceDefaultHandler) 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((size_t)a, (size_t)b); cpArbiter *arb = (cpArbiter *)cpHashSetInsert(space->contactSet, arbHashID, shape_pair, space); cpArbiterUpdate(arb, contacts, numContacts, handler, a, b); // Call the begin function first if it's the first step if(arb->state == 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 != 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 != cpArbiterStateIgnore) arb->state = cpArbiterStateNormal; } // Time stamp the arbiter so we know it was used recently. arb->stamp = space->stamp; }
cpConstraint * cpSpaceAddConstraint(cpSpace *space, cpConstraint *constraint) { assert(!cpArrayContains(space->constraints, constraint)); cpArrayPush(space->constraints, constraint); return constraint; }
cpBody * cpSpaceAddBody(cpSpace *space, cpBody *body) { assert(!cpArrayContains(space->bodies, body)); cpArrayPush(space->bodies, body); return body; }
cpBody * cpSpaceAddBody(cpSpace *space, cpBody *body) { cpAssert(!cpArrayContains(space->bodies, body), "Cannot add the same body more than once."); // cpAssertSpaceUnlocked(space); This should be safe as long as it's not from an integration callback cpArrayPush(space->bodies, body); return body; }
cpConstraint * cpSpaceAddConstraint(cpSpace *space, cpConstraint *constraint) { cpAssert(!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. cpArrayPush(space->constraints, constraint); return constraint; }
static void * handleSetTrans(void *obj, cpSpaceHash *hash) { if(hash->pooledHandles->num == 0){ // handle pool is exhausted, make more int count = CP_BUFFER_BYTES/sizeof(cpHandle); cpAssertSoft(count, "Buffer size is too small."); cpHandle *buffer = (cpHandle *)cpcalloc(1, CP_BUFFER_BYTES); cpArrayPush(hash->allocatedBuffers, buffer); for(int i=0; i<count; i++) cpArrayPush(hash->pooledHandles, buffer + i); } cpHandle *hand = cpHandleInit((cpHandle *)cpArrayPop(hash->pooledHandles), obj); cpHandleRetain(hand); return hand; }
// Hashset filter func to throw away old arbiters. static int 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 0; } return 1; }
cpBody * cpSpaceAddBody(cpSpace *space, cpBody *body) { cpAssertWarn(body->m != INFINITY, "Did you really mean to add an infinite mass body to the space?"); cpAssert(!cpArrayContains(space->bodies, body), "Cannot add the same body more than once."); // cpAssertSpaceUnlocked(space); This should be safe as long as it's not from an integration callback cpArrayPush(space->bodies, body); return body; }
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); } }
cpBody * cpSpaceAddBody(cpSpace *space, cpBody *body) { cpAssertHard(body->space != space, "You have already added this body to this space. You must not add it a second time."); cpAssertHard(!body->space, "You have already added this body to another space. You cannot add it to a second."); cpAssertSpaceUnlocked(space); cpArrayPush(cpSpaceArrayForBodyType(space, cpBodyGetType(body)), body); body->space = space; return body; }
cpBody * cpSpaceAddBody(cpSpace *space, cpBody *body) { cpAssertHard(!cpBodyIsStatic(body), "Static bodies cannot be added to a space as they are not meant to be simulated."); cpAssertSoft(!body->space, "This body is already added to a space and cannot be added to another."); cpAssertSpaceUnlocked(space); cpArrayPush(space->bodies, body); body->space = space; return body; }
cpBody * cpSpaceAddBody(cpSpace *space, cpBody *body) { cpAssertHard(body->space != space, "You have already added this body to this space. You must not add it a second time."); cpAssertHard(!body->space, "You have already added this body to another space. You cannot add it to a second."); cpAssertSpaceUnlocked(space); cpArrayPush(cpBodyIsDynamic(body) ? space->dynamicBodies : space->otherBodies, body); body->space = space; return body; }
cpBody * cpSpaceAddBody(cpSpace *space, cpBody *body) { cpAssertWarn(!cpBodyIsStatic(body), "Static bodies cannot be added to a space as they are not meant to be simulated."); cpAssert(!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; }
cpBody * cpSpaceAddBody(cpSpace *space, cpBody *body) { cpAssertHard(!cpBodyIsStatic(body), "Do not add static bodies to a space. Static bodies do not move and should not be simulated."); cpAssertHard(body->space != space, "You have already added this body to this space. You must not add it a second time."); cpAssertHard(!body->space, "You have already added this body to another space. You cannot add it to a second."); cpAssertSpaceUnlocked(space); cpArrayPush(space->bodies, body); body->space = space; return body; }
static int begin(cpArbiter *arb, cpSpace *space, void *ignore) { CP_ARBITER_GET_SHAPES(arb, a, b); PlayerStruct *player = a->data; cpVect n = cpvneg(cpArbiterGetNormal(arb, 0)); if(n.y > 0.0f){ cpArrayPush(player->groundShapes, b); } return 1; }
// Hashset filter func to throw away old arbiters. static cpBool contactSetFilterRemovedShape(cpArbiter *arb, removalContext *context) { if(context->shape == arb->a || context->shape == arb->b){ if(arb->state != cpArbiterStateCached){ arb->handler->separate(arb, context->space, arb->handler->data); } cpArrayPush(context->space->pooledArbiters, arb); return cpFalse; } return cpTrue; }
cpConstraint * cpSpaceAddConstraint(cpSpace *space, cpConstraint *constraint) { cpAssert(!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; }
// 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; }
// Hashset filter func to throw away old arbiters. static int contactSetFilter(cpArbiter *arb, cpSpace *space) { int ticks = space->stamp - arb->stamp; // was used last frame, but not this one if(ticks == 1){ arb->handler->separate(arb, space, arb->handler->data); arb->stamp = -1; // mark it as a new pair again. } if(ticks >= cp_contact_persistence){ cpArrayPush(space->pooledArbiters, arb); return 0; } return 1; }
cpConstraint * cpSpaceAddConstraint(cpSpace *space, cpConstraint *constraint) { cpAssertSoft(!constraint->space, "This shape is already added to a space and cannot be added to another."); cpAssertSpaceUnlocked(space); cpBodyActivate(constraint->a); cpBodyActivate(constraint->b); cpArrayPush(space->constraints, constraint); // Push onto the heads of the bodies' constraint lists cpBody *a = constraint->a, *b = constraint->b; constraint->next_a = a->constraintList; a->constraintList = constraint; constraint->next_b = b->constraintList; b->constraintList = constraint; constraint->space = space; return constraint; }