// This one is complicated and gross. Just don't go there... // TODO: Comment me! static int seg2poly(const cpShape *shape1, const cpShape *shape2, cpContact *arr) { cpSegmentShape *seg = (cpSegmentShape *)shape1; cpPolyShape *poly = (cpPolyShape *)shape2; cpSplittingPlane *planes = poly->tPlanes; cpFloat segD = cpvdot(seg->tn, seg->ta); cpFloat minNorm = cpPolyShapeValueOnAxis(poly, seg->tn, segD) - seg->r; cpFloat minNeg = cpPolyShapeValueOnAxis(poly, cpvneg(seg->tn), -segD) - seg->r; if(minNeg > 0.0f || minNorm > 0.0f) return 0; int mini = 0; cpFloat poly_min = segValueOnAxis(seg, planes->n, planes->d); if(poly_min > 0.0f) return 0; for(int i=0; i<poly->numVerts; i++){ cpFloat dist = segValueOnAxis(seg, planes[i].n, planes[i].d); if(dist > 0.0f){ return 0; } else if(dist > poly_min){ poly_min = dist; mini = i; } } int num = 0; cpVect poly_n = cpvneg(planes[mini].n); cpVect va = cpvadd(seg->ta, cpvmult(poly_n, seg->r)); cpVect vb = cpvadd(seg->tb, cpvmult(poly_n, seg->r)); if(cpPolyShapeContainsVert(poly, va)) cpContactInit(nextContactPoint(arr, &num), va, poly_n, poly_min, CP_HASH_PAIR(seg->shape.hashid, 0)); if(cpPolyShapeContainsVert(poly, vb)) cpContactInit(nextContactPoint(arr, &num), vb, poly_n, poly_min, CP_HASH_PAIR(seg->shape.hashid, 1)); // Floating point precision problems here. // This will have to do for now. // poly_min -= cp_collision_slop; // TODO is this needed anymore? if(minNorm >= poly_min || minNeg >= poly_min) { if(minNorm > minNeg) findPointsBehindSeg(arr, &num, seg, poly, minNorm, 1.0f); else findPointsBehindSeg(arr, &num, seg, poly, minNeg, -1.0f); } // If no other collision points are found, try colliding endpoints. if(num == 0){ cpVect poly_a = poly->tVerts[mini]; cpVect poly_b = poly->tVerts[(mini + 1)%poly->numVerts]; if(circle2circleQuery(seg->ta, poly_a, seg->r, 0.0f, arr)) return 1; if(circle2circleQuery(seg->tb, poly_a, seg->r, 0.0f, arr)) return 1; if(circle2circleQuery(seg->ta, poly_b, seg->r, 0.0f, arr)) return 1; if(circle2circleQuery(seg->tb, poly_b, seg->r, 0.0f, arr)) return 1; } return num; }
static struct Edge SupportEdgeForSegment(const cpSegmentShape *seg, const cpVect n) { cpHashValue hashid = seg->shape.hashid; if(cpvdot(seg->tn, n) > 0.0) { struct Edge edge = {{seg->ta, CP_HASH_PAIR(hashid, 0)}, {seg->tb, CP_HASH_PAIR(hashid, 1)}, seg->r, seg->tn}; return edge; } else { struct Edge edge = {{seg->tb, CP_HASH_PAIR(hashid, 1)}, {seg->ta, CP_HASH_PAIR(hashid, 0)}, seg->r, cpvneg(seg->tn)}; return edge; } }
// This one is complicated and gross. Just don't go there... // TODO: Comment me! static int seg2poly(cpShape *shape1, cpShape *shape2, cpContact **arr) { cpSegmentShape *seg = (cpSegmentShape *)shape1; cpPolyShape *poly = (cpPolyShape *)shape2; cpPolyShapeAxis *axes = poly->tAxes; cpFloat segD = cpvdot(seg->tn, seg->ta); cpFloat minNorm = cpPolyShapeValueOnAxis(poly, seg->tn, segD) - seg->r; cpFloat minNeg = cpPolyShapeValueOnAxis(poly, cpvneg(seg->tn), -segD) - seg->r; if(minNeg > 0.0f || minNorm > 0.0f) return 0; int mini = 0; cpFloat poly_min = segValueOnAxis(seg, axes->n, axes->d); if(poly_min > 0.0f) return 0; for(int i=0; i<poly->numVerts; i++){ cpFloat dist = segValueOnAxis(seg, axes[i].n, axes[i].d); if(dist > 0.0f){ return 0; } else if(dist > poly_min){ poly_min = dist; mini = i; } } int max = 0; int num = 0; cpVect poly_n = cpvneg(axes[mini].n); cpVect va = cpvadd(seg->ta, cpvmult(poly_n, seg->r)); cpVect vb = cpvadd(seg->tb, cpvmult(poly_n, seg->r)); if(cpPolyShapeContainsVert(poly, va)) cpContactInit(addContactPoint(arr, &max, &num), va, poly_n, poly_min, CP_HASH_PAIR(seg, 0)); if(cpPolyShapeContainsVert(poly, vb)) cpContactInit(addContactPoint(arr, &max, &num), vb, poly_n, poly_min, CP_HASH_PAIR(seg, 1)); // Floating point precision problems here. // This will have to do for now. poly_min -= cp_collision_slop; if(minNorm >= poly_min || minNeg >= poly_min) { if(minNorm > minNeg) findPointsBehindSeg(arr, &max, &num, seg, poly, minNorm, 1.0f); else findPointsBehindSeg(arr, &max, &num, seg, poly, minNeg, -1.0f); } return num; }
void cpSpaceRemoveCollisionHandler(cpSpace *space, cpCollisionType a, cpCollisionType b) { struct{cpCollisionType a, b;} ids = {a, b}; cpCollisionHandler *old_handler = cpHashSetRemove(space->collFuncSet, CP_HASH_PAIR(a, b), &ids); cpfree(old_handler); }
void cpSpaceAddCollisionHandler( cpSpace *space, cpCollisionType a, cpCollisionType b, cpCollisionBeginFunc begin, cpCollisionPreSolveFunc preSolve, cpCollisionPostSolveFunc postSolve, cpCollisionSeparateFunc separate, void *data ){ cpAssertSpaceUnlocked(space); // Remove any old function so the new one will get added. cpSpaceRemoveCollisionHandler(space, a, b); cpCollisionHandler handler = { a, b, begin ? begin : alwaysCollide, preSolve ? preSolve : alwaysCollide, postSolve ? postSolve : nothing, separate ? separate : nothing, data }; cpHashSetInsert(space->collFuncSet, CP_HASH_PAIR(a, b), &handler, NULL); }
// 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; }
void cpSpaceRemoveCollisionPairFunc(cpSpace *space, unsigned int a, unsigned int b) { unsigned int ids[] = {a, b}; unsigned int hash = CP_HASH_PAIR(a, b); cpCollPairFunc *old_pair = (cpCollPairFunc *)cpHashSetRemove(space->collFuncSet, hash, ids); free(old_pair); }
// 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; }
cpCollisionHandler * cpSpaceAddWildcardHandler(cpSpace *space, cpCollisionType type) { cpSpaceUseWildcardDefaultHandler(space); cpHashValue hash = CP_HASH_PAIR(type, CP_WILDCARD_COLLISION_TYPE); cpCollisionHandler handler = {type, CP_WILDCARD_COLLISION_TYPE, AlwaysCollide, AlwaysCollide, DoNothing, DoNothing, NULL}; return (cpCollisionHandler*)cpHashSetInsert(space->collisionHandlers, hash, &handler, (cpHashSetTransFunc)handlerSetTrans, NULL); }
// Add contacts for probably penetrating vertexes. // This handles the degenerate case where an overlap was detected, but no vertexes fall inside // the opposing polygon. (like a star of david) static /*inline*/ int findVertsFallback(cpContact *arr, const cpPolyShape *poly1, const cpPolyShape *poly2, const cpVect n, const cpFloat dist) { int num = 0; for(int i=0; i<poly1->numVerts; i++){ cpVect v = poly1->tVerts[i]; if(cpPolyShapeContainsVertPartial(poly2, v, cpvneg(n))) cpContactInit(nextContactPoint(arr, &num), v, n, dist, CP_HASH_PAIR(poly1->shape.hashid, i)); } for(int i=0; i<poly2->numVerts; i++){ cpVect v = poly2->tVerts[i]; if(cpPolyShapeContainsVertPartial(poly1, v, n)) cpContactInit(nextContactPoint(arr, &num), v, n, dist, CP_HASH_PAIR(poly2->shape.hashid, i)); } return num; }
cpCollisionHandler *cpSpaceAddCollisionHandler(cpSpace *space, cpCollisionType a, cpCollisionType b) { cpHashValue hash = CP_HASH_PAIR(a, b); // TODO should use space->defaultHandler values instead? cpCollisionHandler temp = {a, b, DefaultBegin, DefaultPreSolve, DefaultPostSolve, DefaultSeparate, NULL}; cpHashSet *handlers = space->collisionHandlers; cpCollisionHandler *handler = cpHashSetFind(handlers, hash, &temp); return (handler ? handler : cpHashSetInsert(handlers, hash, &temp, (cpHashSetTransFunc)handlerSetTrans, NULL)); }
// Add contacts for penetrating vertexes. static inline int findVerts(cpContact *arr, const cpPolyShape *poly1, const cpPolyShape *poly2, const cpVect n, const cpFloat dist) { int num = 0; for(int i=0; i<poly1->numVerts; i++){ cpVect v = poly1->tVerts[i]; if(cpPolyShapeContainsVert(poly2, v)) cpContactInit(nextContactPoint(arr, &num), v, n, dist, CP_HASH_PAIR(poly1->shape.hashid, i)); } for(int i=0; i<poly2->numVerts; i++){ cpVect v = poly2->tVerts[i]; if(cpPolyShapeContainsVert(poly1, v)) cpContactInit(nextContactPoint(arr, &num), v, n, dist, CP_HASH_PAIR(poly2->shape.hashid, i)); } return (num ? num : findVertsFallback(arr, poly1, poly2, n, dist)); }
static struct Edge SupportEdgeForPoly(const cpPolyShape *poly, const cpVect n) { int count = poly->count; int i1 = PolySupportPointIndex(poly->count, poly->planes, n); // TODO: get rid of mod eventually, very expensive on ARM int i0 = (i1 - 1 + count)%count; int i2 = (i1 + 1)%count; const struct cpSplittingPlane *planes = poly->planes; cpHashValue hashid = poly->shape.hashid; if(cpvdot(n, planes[i1].n) > cpvdot(n, planes[i2].n)) { struct Edge edge = {{planes[i0].v0, CP_HASH_PAIR(hashid, i0)}, {planes[i1].v0, CP_HASH_PAIR(hashid, i1)}, poly->r, planes[i1].n}; return edge; } else { struct Edge edge = {{planes[i1].v0, CP_HASH_PAIR(hashid, i1)}, {planes[i2].v0, CP_HASH_PAIR(hashid, i2)}, poly->r, planes[i2].n}; return edge; } }
cpCollisionHandler * cpSpaceAddWildcardHandler(cpSpace *space, cpCollisionType type) { cpSpaceUseWildcardDefaultHandler(space); cpHashValue hash = CP_HASH_PAIR(type, CP_WILDCARD_COLLISION_TYPE); cpCollisionHandler temp = {type, CP_WILDCARD_COLLISION_TYPE, AlwaysCollide, AlwaysCollide, DoNothing, DoNothing, NULL}; cpHashSet *handlers = space->collisionHandlers; cpCollisionHandler *handler = cpHashSetFind(handlers, hash, &temp); return (handler ? handler : cpHashSetInsert(handlers, hash, &temp, (cpHashSetTransFunc)handlerSetTrans, NULL)); }
void cpSpaceAddCollisionPairFunc(cpSpace *space, unsigned int a, unsigned int b, cpCollFunc func, void *data) { unsigned int ids[] = {a, b}; unsigned int hash = CP_HASH_PAIR(a, b); // Remove any old function so the new one will get added. cpSpaceRemoveCollisionPairFunc(space, a, b); collFuncData funcData = {func, data}; cpHashSetInsert(space->collFuncSet, hash, ids, &funcData); }
// Add contacts for penetrating vertexes. static inline int findVerts(cpContact *arr, cpPolyShape *poly1, cpPolyShape *poly2, cpVect n, cpFloat dist) { int num = 0; for(int i=0; i<poly1->numVerts; i++){ cpVect v = poly1->tVerts[i]; if(cpPolyShapeContainsVertPartial(poly2, v, cpvneg(n))) cpContactInit(nextContactPoint(arr, &num), v, n, dist, CP_HASH_PAIR(poly1->shape.hashid, i)); } for(int i=0; i<poly2->numVerts; i++){ cpVect v = poly2->tVerts[i]; if(cpPolyShapeContainsVertPartial(poly1, v, n)) cpContactInit(nextContactPoint(arr, &num), v, n, dist, CP_HASH_PAIR(poly2->shape.hashid, i)); } // if(!num) // addContactPoint(arr, &size, &num, cpContactNew(shape1->body->p, n, dist, 0)); return num; }
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); } }
// Identify vertexes that have penetrated the segment. static inline void findPointsBehindSeg(cpContact **arr, int *max, int *num, cpSegmentShape *seg, cpPolyShape *poly, cpFloat pDist, cpFloat coef) { cpFloat dta = cpvcross(seg->tn, seg->ta); cpFloat dtb = cpvcross(seg->tn, seg->tb); cpVect n = cpvmult(seg->tn, coef); for(int i=0; i<poly->numVerts; i++){ cpVect v = poly->tVerts[i]; if(cpvdot(v, n) < cpvdot(seg->tn, seg->ta)*coef + seg->r){ cpFloat dt = cpvcross(seg->tn, v); if(dta >= dt && dt >= dtb){ cpContactInit(addContactPoint(arr, max, num), v, n, pDist, CP_HASH_PAIR(poly->shape.hashid, i)); } } } }
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); } }
static inline cpCollisionHandler * cpSpaceLookupHandler(cpSpace *space, cpCollisionType a, cpCollisionType b, cpCollisionHandler *defaultValue) { cpCollisionType types[] = {a, b}; cpCollisionHandler *handler = (cpCollisionHandler *)cpHashSetFind(space->collisionHandlers, CP_HASH_PAIR(a, b), types); return (handler ? handler : defaultValue); }
// 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; } 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. cpSpacePushNewContactBuffer(space); } // Narrow-phase collision detection. cpContact *contacts = ((cpContactBuffer *)(space->contactBuffersHead))->contacts + space->contactBuffersHead->numContacts; int numContacts = cpCollideShapes(a, b, contacts); if(!numContacts) return; // Shapes are not colliding. space->contactBuffersHead->numContacts += 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); // retains the contacts array // Call the begin function first if it's the first step if(arb->stamp == -1 && !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 { // cpfree(arb->contacts); space->contactBuffersHead->numContacts -= numContacts; arb->contacts = NULL; arb->numContacts = 0; } // Time stamp the arbiter so we know it was used recently. arb->stamp = space->stamp; }
void cpSpaceRemoveCollisionHandler(cpSpace *space, cpCollisionType a, cpCollisionType b) { cpAssertSpaceUnlocked(space); struct { cpCollisionType a, b; } ids = {a, b}; cpCollisionHandler *old_handler = (cpCollisionHandler *) cpHashSetRemove(space->collisionHandlers, CP_HASH_PAIR(a, b), &ids); cpfree(old_handler); }
cpCollisionHandler *cpSpaceAddCollisionHandler(cpSpace *space, cpCollisionType a, cpCollisionType b) { cpHashValue hash = CP_HASH_PAIR(a, b); cpCollisionHandler handler = {a, b, DefaultBegin, DefaultPreSolve, DefaultPostSolve, DefaultSeparate, NULL}; return (cpCollisionHandler*)cpHashSetInsert(space->collisionHandlers, hash, &handler, (cpHashSetTransFunc)handlerSetTrans, NULL); }
// Callback from the spatial hash. // TODO: Refactor this into separate functions? static int queryFunc(void *p1, void *p2, void *data) { // Cast the generic pointers from the spatial hash back to usefull types cpShape *a = (cpShape *)p1; cpShape *b = (cpShape *)p2; cpSpace *space = (cpSpace *)data; // Reject any of the simple cases if(queryReject(a,b)) return 0; // 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; } // Find the collision pair function for the shapes. unsigned int ids[] = {a->collision_type, b->collision_type}; unsigned int hash = CP_HASH_PAIR(a->collision_type, b->collision_type); cpCollPairFunc *pairFunc = (cpCollPairFunc *)cpHashSetFind(space->collFuncSet, hash, ids); if(!pairFunc->func) return 0; // A NULL pair function means don't collide at all. // Narrow-phase collision detection. cpContact *contacts = NULL; int numContacts = cpCollideShapes(a, b, &contacts); if(!numContacts) return 0; // Shapes are not colliding. // The collision pair function requires objects to be ordered by their collision types. cpShape *pair_a = a; cpShape *pair_b = b; cpFloat normal_coef = 1.0f; // Swap them if necessary. if(pair_a->collision_type != pairFunc->a){ cpShape *temp = pair_a; pair_a = pair_b; pair_b = temp; normal_coef = -1.0f; } if(pairFunc->func(pair_a, pair_b, contacts, numContacts, normal_coef, pairFunc->data)){ // The collision pair function OKed the collision. Record the contact information. // Get an arbiter from space->contactSet for the two shapes. // This is where the persistant contact magic comes from. cpShape *shape_pair[] = {a, b}; cpArbiter *arb = (cpArbiter *)cpHashSetInsert(space->contactSet, CP_HASH_PAIR(a, b), shape_pair, space); // Timestamp the arbiter. arb->stamp = space->stamp; arb->a = a; arb->b = b; // TODO: Investigate why this is still necessary? // Inject the new contact points into the arbiter. cpArbiterInject(arb, contacts, numContacts); // Add the arbiter to the list of active arbiters. cpArrayPush(space->arbiters, arb); return numContacts; } else { // The collision pair function rejected the collision. free(contacts); return 0; } }
unsigned int bmx_CP_HASH_PAIR(int collTypeA, int collTypeB) { return CP_HASH_PAIR(collTypeA, collTypeB); }
static /*inline*/ cpCollisionHandler * lookupCollisionHandler(cpSpace *space, cpCollisionType a, cpCollisionType b) { cpCollisionType types[] = {a, b}; return (cpCollisionHandler *)cpHashSetFind(space->collFuncSet, CP_HASH_PAIR(a, b), types); }