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;
}
