void
cpArbiterUpdate(cpArbiter *arb, struct cpCollisionInfo *info, cpSpace *space)
{
const cpShape *a = info->a, *b = info->b;
// For collisions between two similar primitive types, the order could have been swapped since the last frame.
arb->a = a; arb->body_a = a->body;
arb->b = b; arb->body_b = b->body;
// Iterate over the possible pairs to look for hash value matches.
for(int i=0; i<info->count; i++){
struct cpContact *con = &info->arr[i];
// r1 and r2 store absolute offsets at init time.
// Need to convert them to relative offsets.
con->r1 = cpvsub(con->r1, a->body->p);
con->r2 = cpvsub(con->r2, b->body->p);
// Cached impulses are not zeroed at init time.
con->jnAcc = con->jtAcc = 0.0f;
for(int j=0; j<arb->count; j++){
struct cpContact *old = &arb->contacts[j];
// This could trigger false positives, but is fairly unlikely nor serious if it does.
if(con->hash == old->hash){
// Copy the persistant contact information.
con->jnAcc = old->jnAcc;
con->jtAcc = old->jtAcc;
}
}
}
arb->contacts = info->arr;
arb->count = info->count;
arb->n = info->n;
arb->e = a->e * b->e;
arb->u = a->u * b->u;
cpVect surface_vr = cpvsub(b->surfaceV, a->surfaceV);
arb->surface_vr = cpvsub(surface_vr, cpvmult(info->n, cpvdot(surface_vr, info->n)));
cpCollisionType typeA = info->a->type, typeB = info->b->type;
cpCollisionHandler *defaultHandler = &space->defaultHandler;
cpCollisionHandler *handler = arb->handler = cpSpaceLookupHandler(space, typeA, typeB, defaultHandler);
// Check if the types match, but don't swap for a default handler which use the wildcard for type A.
cpBool swapped = arb->swapped = (typeA != handler->typeA && handler->typeA != CP_WILDCARD_COLLISION_TYPE);
if(handler != defaultHandler || space->usesWildcards){
// The order of the main handler swaps the wildcard handlers too. Uffda.
arb->handlerA = cpSpaceLookupHandler(space, (swapped ? typeB : typeA), CP_WILDCARD_COLLISION_TYPE, &cpCollisionHandlerDoNothing);
arb->handlerB = cpSpaceLookupHandler(space, (swapped ? typeA : typeB), CP_WILDCARD_COLLISION_TYPE, &cpCollisionHandlerDoNothing);
}
// mark it as new if it's been cached
if(arb->state == CP_ARBITER_STATE_CACHED) arb->state = CP_ARBITER_STATE_FIRST_COLLISION;
}
// Callback from the spatial hash.
static void
collideShapes(cpShape *a, cpShape *b, cpSpace *space)
{
// Reject any of the simple cases
if(queryReject(a,b)) return;
cpCollisionHandler *handler = cpSpaceLookupHandler(space, a->collision_type, b->collision_type);
cpBool sensor = a->sensor || b->sensor;
if(sensor && handler == &cpDefaultCollisionHandler) 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->arbiterSet 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->cachedArbiters, arbHashID, shape_pair, space, (cpHashSetTransFunc)cpSpaceArbiterSetTrans);
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-solve callback.
// However, post-solve 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
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);
}
}
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);
}
}
