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);
}
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;
}
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;
}
static void
cpSpaceHashRehashObject(cpSpaceHash *hash, void *obj, cpHashValue hashid)
{
cpHandle *hand = (cpHandle *)cpHashSetRemove(hash->handleSet, hashid, obj);
if(hand){
hand->obj = NULL;
cpHandleRelease(hand, hash->pooledHandles);
cpSpaceHashInsert(hash, obj, hashid);
}
}
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);
}
}
}
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 *
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;
}
// TODO this function needs more commenting.
void
cpSpaceProcessComponents(cpSpace *space, cpFloat dt)
{
cpArray *bodies = space->bodies;
cpArray *newBodies = cpArrayNew(bodies->num);
cpArray *rogueBodies = cpArrayNew(16);
cpArray *arbiters = space->arbiters;
cpArray *constraints = space->constraints;
cpArray *components = cpArrayNew(space->sleepingComponents->num);
cpFloat dv = space->idleSpeedThreshold;
cpFloat dvsq = (dv ? dv*dv : cpvdot(space->gravity, space->gravity)*dt*dt);
// update idling
for(int i=0; i<bodies->num; i++){
cpBody *body = (cpBody*)bodies->arr[i];
cpFloat thresh = (dvsq ? body->m*dvsq : 0.0f);
body->node.idleTime = (cpBodyKineticEnergy(body) > thresh ? 0.0f : body->node.idleTime + dt);
}
// iterate graph edges and build forests
for(int i=0; i<arbiters->num; i++){
cpArbiter *arb = (cpArbiter*)arbiters->arr[i];
mergeBodies(space, components, rogueBodies, arb->a->body, arb->b->body);
}
for(int j=0; j<constraints->num; j++){
cpConstraint *constraint = (cpConstraint *)constraints->arr[j];
mergeBodies(space, components, rogueBodies, constraint->a, constraint->b);
}
// iterate bodies and add them to their components
for(int i=0; i<bodies->num; i++) addToComponent((cpBody*)bodies->arr[i], components);
for(int i=0; i<rogueBodies->num; i++) addToComponent((cpBody*)rogueBodies->arr[i], components);
// iterate components, copy or deactivate
for(int i=0; i<components->num; i++){
cpBody *root = (cpBody*)components->arr[i];
if(componentActive(root, space->sleepTimeThreshold)){
cpBody *body = root, *next;
do {
next = body->node.next;
if(!cpBodyIsRogue(body)) cpArrayPush(newBodies, body);
cpComponentNode node = {NULL, NULL, 0, body->node.idleTime};
body->node = node;
} while((body = next) != root);
} else {
cpBody *body = root, *next;
do {
next = body->node.next;
for(cpShape *shape = body->shapesList; shape; shape = shape->next){
cpSpaceHashRemove(space->activeShapes, shape, shape->hashid);
cpSpaceHashInsert(space->staticShapes, shape, shape->hashid, shape->bb);
}
} while((body = next) != root);
cpArrayPush(space->sleepingComponents, root);
}
}
space->bodies = newBodies;
cpArrayFree(bodies);
cpArrayFree(rogueBodies);
cpArrayFree(components);
}
void
cpSpaceAddStaticShape(cpSpace *space, cpShape *shape)
{
cpSpaceHashInsert(space->staticShapes, shape, shape->id, shape->bb);
}
void
cpSpaceAddShape(cpSpace *space, cpShape *shape)
{
cpSpaceHashInsert(space->activeShapes, shape, shape->id, shape->bb);
}
static inline void
addShapeRaw(cpShape *shape, cpSpaceHash *hash)
{
cpSpaceHashInsert(hash, shape, shape->hashid, shape->bb);
}