cpShape *
cpSpaceAddStaticShape(cpSpace *space, cpShape *shape)
{
cpAssertSoft(!shape->space, "This shape is already added to a space and cannot be added to another.");
cpAssertSpaceUnlocked(space);
cpBody *body = shape->body;
cpBodyAddShape(body, shape);
cpShapeUpdate(shape, body->p, body->rot);
cpSpatialIndexInsert(space->staticShapes, shape, shape->hashid);
shape->space = space;
return shape;
}
cpShape *
cpSpaceAddStaticShape(cpSpace *space, cpShape *shape)
{
cpAssertHard(shape->space != space, "You have already added this shape to this space. You must not add it a second time.");
cpAssertHard(!shape->space, "You have already added this shape to another space. You cannot add it to a second.");
cpAssertHard(cpBodyIsRogue(shape->body), "You are adding a static shape to a dynamic body. Did you mean to attach it to a static or rogue body? See the documentation for more information.");
cpAssertSpaceUnlocked(space);
cpBody *body = shape->body;
cpBodyAddShape(body, shape);
cpShapeUpdate(shape, body->p, body->rot);
cpSpatialIndexInsert(space->staticShapes, shape, shape->hashid);
shape->space = space;
return shape;
}
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
cpSpaceConvertBodyToStatic(cpSpace *space, cpBody *body)
{
cpAssertHard(!cpBodyIsStatic(body), "Body is already static.");
cpAssertHard(cpBodyIsRogue(body), "Remove the body from the space before calling this function.");
cpAssertSpaceUnlocked(space);
cpBodySetMass(body, INFINITY);
cpBodySetMoment(body, INFINITY);
cpBodySetVel(body, cpvzero);
cpBodySetAngVel(body, 0.0f);
body->node.idleTime = INFINITY;
CP_BODY_FOREACH_SHAPE(body, shape){
cpSpatialIndexRemove(space->activeShapes, shape, shape->hashid);
cpSpatialIndexInsert(space->staticShapes, shape, shape->hashid);
}
cpShape *
cpSpaceAddShape(cpSpace *space, cpShape *shape)
{
cpBody *body = shape->body;
if(cpBodyIsStatic(body)) return cpSpaceAddStaticShape(space, shape);
// TODO change these to check if it was added to a space at all.
cpAssertSoft(!shape->space, "This shape is already added to a space and cannot be added to another.");
cpAssertSpaceUnlocked(space);
cpBodyActivate(body);
cpBodyAddShape(body, shape);
cpShapeUpdate(shape, body->p, body->rot);
cpSpatialIndexInsert(space->activeShapes, shape, shape->hashid);
shape->space = space;
return shape;
}
//MARK: Body, Shape, and Joint Management
cpShape *
cpSpaceAddShape(cpSpace *space, cpShape *shape)
{
cpBody *body = shape->body;
if(cpBodyIsStatic(body)) return cpSpaceAddStaticShape(space, shape);
cpAssertHard(shape->space != space, "You have already added this shape to this space. You must not add it a second time.");
cpAssertHard(!shape->space, "You have already added this shape to another space. You cannot add it to a second.");
cpAssertSpaceUnlocked(space);
cpBodyActivate(body);
cpBodyAddShape(body, shape);
cpShapeUpdate(shape, body->p, body->rot);
cpSpatialIndexInsert(space->activeShapes, shape, shape->hashid);
shape->space = space;
return shape;
}
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);
}
}
//MARK: Body, Shape, and Joint Management
cpShape *
cpSpaceAddShape(cpSpace *space, cpShape *shape)
{
cpBody *body = shape->body;
cpAssertHard(shape->space != space, "You have already added this shape to this space. You must not add it a second time.");
cpAssertHard(!shape->space, "You have already added this shape to another space. You cannot add it to a second.");
// cpAssertHard(body->space == space, "The shape's body must be added to the space before the shape.");
cpAssertSpaceUnlocked(space);
cpBool isStatic = (cpBodyGetType(body) == CP_BODY_TYPE_STATIC);
if(!isStatic) cpBodyActivate(body);
cpBodyAddShape(body, shape);
shape->hashid = space->shapeIDCounter++;
cpShapeUpdate(shape, body->transform);
cpSpatialIndexInsert(isStatic ? space->staticShapes : space->dynamicShapes, shape, shape->hashid);
shape->space = space;
return shape;
}
static void
copyShapes(cpShape *shape, cpSpatialIndex *index)
{
cpSpatialIndexInsert(index, shape, shape->hashid);
}
void SpatialIndex::insert(void *obj,cpHashValue hashid)
{
cpSpatialIndexInsert(index,obj,hashid);
}
