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);
}
void
cpSpaceRemoveStaticShape(cpSpace *space, cpShape *shape)
{
assert(cpHashSetFind(space->staticShapes->handleSet, shape->hashid, shape));
cpSpaceHashRemove(space->staticShapes, shape, shape->hashid);
}
static void
cpBBTreeReindexObject(cpBBTree *tree, void *obj, cpHashValue hashid)
{
Node *leaf = (Node *)cpHashSetFind(tree->leaves, hashid, obj);
if(leaf){
if(LeafUpdate(leaf, tree)) LeafAddPairs(leaf, tree);
IncrementStamp(tree);
}
}
cpShape *
cpSpaceAddShape(cpSpace *space, cpShape *shape)
{
assert(shape->body);
assert(!cpHashSetFind(space->activeShapes->handleSet, shape->hashid, shape));
cpSpaceHashInsert(space->activeShapes, shape, shape->hashid, shape->bb);
return shape;
}
cpShape *
cpSpaceAddShape(cpSpace *space, cpShape *shape)
{
cpAssert(shape->body, "Cannot add a shape with a NULL body.");
cpAssert(!cpHashSetFind(space->activeShapes->handleSet, shape->hashid, shape), "Cannot add the same shape more than once.");
cpAssertSpaceUnlocked(space);
cpSpaceHashInsert(space->activeShapes, shape, shape->hashid, shape->bb);
return shape;
}
void
cpSpaceRemoveStaticShape(cpSpace *space, cpShape *shape)
{
cpAssert(cpHashSetFind(space->staticShapes->handleSet, shape->hashid, shape), "Cannot remove a static shape that was never added to the space.");
cpAssertSpaceUnlocked(space);
removalContext context = {space, shape};
cpHashSetFilter(space->contactSet, (cpHashSetFilterFunc)contactSetFilterRemovedShape, &context);
cpSpaceHashRemove(space->staticShapes, shape, shape->hashid);
}
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));
}
void *
cpSpaceGetPostStepData(cpSpace *space, void *obj)
{
if(space->postStepCallbacks){
PostStepCallback query = {NULL, obj, NULL};
PostStepCallback *callback = (PostStepCallback *)cpHashSetFind(space->postStepCallbacks, (cpHashValue)(size_t)obj, &query);
return (callback ? callback->data : NULL);
} else {
return NULL;
}
}
cpShape *
cpSpaceAddStaticShape(cpSpace *space, cpShape *shape)
{
assert(shape->body);
assert(!cpHashSetFind(space->staticShapes->handleSet, shape->hashid, shape));
cpShapeCacheBB(shape);
cpSpaceHashInsert(space->staticShapes, shape, shape->hashid, shape->bb);
return shape;
}
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
cpSpaceRemoveStaticShape(cpSpace *space, cpShape *shape)
{
cpAssertWarn(cpHashSetFind(space->staticShapes->handleSet, shape->hashid, shape),
"Cannot remove a static or sleeping shape that was not added to the space. (Removed twice maybe?)");
cpAssertSpaceUnlocked(space);
removalContext context = {space, shape};
cpHashSetFilter(space->contactSet, (cpHashSetFilterFunc)contactSetFilterRemovedShape, &context);
cpSpaceHashRemove(space->staticShapes, shape, shape->hashid);
cpSpaceActivateShapesTouchingShape(space, shape);
}
cpShape *
cpSpaceAddStaticShape(cpSpace *space, cpShape *shape)
{
// cpAssert(shape->body, "Cannot add a static shape with a NULL body.");
if(!shape->body) shape->body = &space->staticBody;
cpAssert(!cpHashSetFind(space->staticShapes->handleSet, shape->hashid, shape),
"Cannot add the same static shape more than once.");
cpAssertSpaceUnlocked(space);
cpShapeCacheBB(shape);
cpSpaceHashInsert(space->staticShapes, shape, shape->hashid, shape->bb);
return shape;
}
cpShape *
cpSpaceAddStaticShape(cpSpace *space, cpShape *shape)
{
cpAssert(!cpHashSetFind(space->staticShapes->handleSet, shape->hashid, shape),
"Cannot add the same static shape more than once.");
cpAssertSpaceUnlocked(space);
if(!shape->body) shape->body = &space->staticBody;
cpShapeCacheBB(shape);
activateShapesTouchingShape(space, shape);
addShapeRaw(shape, space->staticShapes);
return shape;
}
cpShape *
cpSpaceAddShape(cpSpace *space, cpShape *shape)
{
cpBody *body = shape->body;
if(!body || body == &space->staticBody) return cpSpaceAddStaticShape(space, shape);
cpAssert(!cpHashSetFind(space->activeShapes->handleSet, shape->hashid, shape),
"Cannot add the same shape more than once.");
cpAssertSpaceUnlocked(space);
cpBodyActivate(body);
cpBodyAddShape(body, shape);
addShapeRaw(shape, space->activeShapes);
return shape;
}
// 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;
}
cpShape *
cpSpaceAddShape(cpSpace *space, cpShape *shape)
{
cpBody *body = shape->body;
if(!body || cpBodyIsStatic(body)) return cpSpaceAddStaticShape(space, shape);
cpAssert(!cpHashSetFind(space->activeShapes->handleSet, shape->hashid, shape),
"Cannot add the same shape more than once.");
cpAssertSpaceUnlocked(space);
cpBodyActivate(body);
cpBodyAddShape(body, shape);
cpShapeCacheBB(shape);
cpSpaceHashInsert(space->activeShapes, shape, shape->hashid, shape->bb);
return shape;
}
void
cpSpaceRemoveShape(cpSpace *space, cpShape *shape)
{
cpBody *body = shape->body;
if(cpBodyIsStatic(body)){
cpSpaceRemoveStaticShape(space, shape);
return;
}
cpBodyActivate(body);
cpAssertSpaceUnlocked(space);
cpAssertWarn(cpHashSetFind(space->activeShapes->handleSet, shape->hashid, shape),
"Cannot remove a shape that was not added to the space. (Removed twice maybe?)");
cpBodyRemoveShape(body, shape);
removalContext context = {space, shape};
cpHashSetFilter(space->contactSet, (cpHashSetFilterFunc)contactSetFilterRemovedShape, &context);
cpSpaceHashRemove(space->activeShapes, shape, shape->hashid);
}
static int
cpSpaceHashContains(cpSpaceHash *hash, void *obj, cpHashValue hashid)
{
return cpHashSetFind(hash->handleSet, hashid, obj) != NULL;
}
void
cpSpaceHashRehashObject(cpSpaceHash *hash, void *obj, cpHashValue hashid)
{
cpHandle *hand = (cpHandle *)cpHashSetFind(hash->handleSet, hashid, obj);
hashHandle(hash, hand, hash->bbfunc(obj));
}
static cpBool
cpBBTreeContains(cpBBTree *tree, void *obj, cpHashValue hashid)
{
return (cpHashSetFind(tree->leaves, hashid, obj) != 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;
}
}
void
cpSpaceHashRehashObject(cpSpaceHash *hash, void *obj, unsigned int id)
{
cpHandle *hand = (cpHandle *)cpHashSetFind(hash->handleSet, id, obj);
hashHandle(hash, hand, hash->bbfunc(obj));
}
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);
}
// 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;
}
// 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);
cpBool 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 = 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;
}
