void
cpSpaceRemoveConstraint(cpSpace *space, cpConstraint *constraint)
{
assert(cpArrayContains(space->constraints, constraint));
cpArrayDeleteObj(space->constraints, constraint);
}
void
cpSpaceRemoveBody(cpSpace *space, cpBody *body)
{
assert(cpArrayContains(space->bodies, body));
cpArrayDeleteObj(space->bodies, body);
}
void
cpSpaceRemoveConstraint(cpSpace *space, cpConstraint *constraint)
{
cpAssert(cpArrayContains(space->constraints, constraint), "Cannot remove a constraint that was never added to the space.");
// cpAssertSpaceUnlocked(space); Should be safe as long as its not from a constraint callback.
cpArrayDeleteObj(space->constraints, constraint);
}
void
cpSpaceRemoveBody(cpSpace *space, cpBody *body)
{
cpAssert(cpArrayContains(space->bodies, body), "Cannot remove a body that was never added to the space.");
cpAssertSpaceUnlocked(space);
cpArrayDeleteObj(space->bodies, body);
}
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;
}
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;
}
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;
}
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
cpSpaceRemoveConstraint(cpSpace *space, cpConstraint *constraint)
{
cpAssertWarn(cpArrayContains(space->constraints, constraint),
"Cannot remove a constraint that was not added to the space. (Removed twice maybe?)");
// cpAssertSpaceUnlocked(space); Should be safe as long as its not from a constraint callback.
cpBodyActivate(constraint->a);
cpBodyActivate(constraint->b);
cpArrayDeleteObj(space->constraints, constraint);
}
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;
}
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);
}
}
