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);
}
void
cpSpaceRemoveConstraint(cpSpace *space, cpConstraint *constraint)
{
cpAssertHard(cpSpaceContainsConstraint(space, constraint), "Cannot remove a constraint that was not added to the space. (Removed twice maybe?)");
cpAssertSpaceUnlocked(space);
cpBodyActivate(constraint->a);
cpBodyActivate(constraint->b);
cpArrayDeleteObj(space->constraints, constraint);
cpBodyRemoveConstraint(constraint->a, constraint);
cpBodyRemoveConstraint(constraint->b, constraint);
constraint->space = NULL;
}
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;
}
static void internalBodySetMass(cpBody *body, cpFloat mass)
{
cpBodyActivate(body);
body->m = mass;
body->m_inv = 1.0f/mass;
//cpAssertSaneBody(body);
}
void
cpShapeSetElasticity(cpShape *shape, cpFloat elasticity)
{
cpAssertHard(elasticity >= 0.0f, "Elasticity must be positive and non-zero.");
cpBodyActivate(shape->body);
shape->e = elasticity;
}
void
cpBodySetMass(cpBody *body, cpFloat mass)
{
cpBodyActivate(body);
body->m = mass;
body->m_inv = 1.0f/mass;
}
void
cpBodySetMoment(cpBody *body, cpFloat moment)
{
cpBodyActivate(body);
body->i = moment;
body->i_inv = 1.0f/moment;
}
void
cpBodyApplyForce(cpBody *body, cpVect force, cpVect r)
{
cpBodyActivate(body);
body->f = cpvadd(body->f, force);
body->t += cpvcross(r, force);
}
void
cpShapeSetFriction(cpShape *shape, cpFloat friction)
{
cpAssertHard(friction >= 0.0f, "Friction must be postive and non-zero.");
cpBodyActivate(shape->body);
shape->u = friction;
}
void
cpBodyResetForces(cpBody *body)
{
cpBodyActivate(body);
body->f = cpvzero;
body->t = 0.0f;
}
void
cpBodySetPos(cpBody *body, cpVect pos)
{
cpBodyActivate(body);
cpBodyAssertSane(body);
body->p = pos;
}
void
cpBodySetAngle(cpBody *body, cpFloat angle)
{
cpBodyActivate(body);
cpBodyAssertSane(body);
setAngle(body, angle);
}
void
cpShapeSetMass(cpShape *shape, cpFloat mass){
cpBody *body = shape->body;
cpBodyActivate(body);
shape->massInfo.m = mass;
cpBodyAccumulateMassFromShapes(body);
}
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;
}
void PhysicsBody::setResting(bool rest) const
{
if (rest && !isResting())
{
cpBodySleep(_info->getBody());
}else if(!rest && isResting())
{
cpBodyActivate(_info->getBody());
}
}
void
cpSpaceFreeChildren(cpSpace *space)
{
cpArray *components = space->sleepingComponents;
while(components->num) cpBodyActivate((cpBody *)components->arr[0]);
cpSpaceHashEach(space->staticShapes, (cpSpaceHashIterator)&shapeFreeWrap, NULL);
cpSpaceHashEach(space->activeShapes, (cpSpaceHashIterator)&shapeFreeWrap, NULL);
cpArrayEach(space->bodies, (cpArrayIter)&bodyFreeWrap, NULL);
cpArrayEach(space->constraints, (cpArrayIter)&constraintFreeWrap, NULL);
}
void
cpSpaceRemoveBody(cpSpace *space, cpBody *body)
{
cpAssertWarn(body->space == space,
"Cannot remove a body that was not added to the space. (Removed twice maybe?)");
cpAssertSpaceUnlocked(space);
cpBodyActivate(body);
cpArrayDeleteObj(space->bodies, body);
body->space = NULL;
}
void
cpSpaceRemoveBody(cpSpace *space, cpBody *body)
{
cpAssertHard(cpSpaceContainsBody(space, body), "Cannot remove a body that was not added to the space. (Removed twice maybe?)");
cpAssertSpaceUnlocked(space);
cpBodyActivate(body);
// cpSpaceFilterArbiters(space, body, NULL);
cpArrayDeleteObj(space->bodies, body);
body->space = NULL;
}
void
cpSpaceSetGravity(cpSpace *space, cpVect gravity)
{
space->gravity = gravity;
// Wake up all of the bodies since the gravity changed.
cpArray *components = space->sleepingComponents;
for(int i=0; i<components->num; i++){
cpBodyActivate((cpBody *)components->arr[i]);
}
}
cpConstraint *
cpSpaceAddConstraint(cpSpace *space, cpConstraint *constraint)
{
cpAssertHard(constraint->space != space, "You have already added this constraint to this space. You must not add it a second time.");
cpAssertHard(!constraint->space, "You have already added this constraint to another space. You cannot add it to a second.");
cpAssertHard(constraint->a && constraint->b, "Constraint is attached to a NULL body.");
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;
}
void
cpSpaceRemoveBody(cpSpace *space, cpBody *body)
{
cpAssertHard(body != cpSpaceGetStaticBody(space), "Cannot remove the designated static body for the space.");
cpAssertHard(cpSpaceContainsBody(space, body), "Cannot remove a body that was not added to the space. (Removed twice maybe?)");
// cpAssertHard(body->shapeList == NULL, "Cannot remove a body from the space before removing the bodies attached to it.");
// cpAssertHard(body->constraintList == NULL, "Cannot remove a body from the space before removing the constraints attached to it.");
cpAssertSpaceUnlocked(space);
cpBodyActivate(body);
// cpSpaceFilterArbiters(space, body, NULL);
cpArrayDeleteObj(cpSpaceArrayForBodyType(space, cpBodyGetType(body)), body);
body->space = NULL;
}
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;
}
void
cpSpaceRemoveShape(cpSpace *space, cpShape *shape)
{
cpBody *body = shape->body;
if(cpBodyIsStatic(body)){
cpSpaceRemoveStaticShape(space, shape);
} else {
cpAssertHard(cpSpaceContainsShape(space, shape), "Cannot remove a shape that was not added to the space. (Removed twice maybe?)");
cpAssertSpaceUnlocked(space);
cpBodyActivate(body);
cpBodyRemoveShape(body, shape);
cpSpaceFilterArbiters(space, body, shape);
cpSpatialIndexRemove(space->activeShapes, shape, shape->hashid);
shape->space = NULL;
}
}
//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, "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;
}
ETERM *body_activate(ETERM *fromp, ETERM *argp) {
// get the args
ETERM *space_refp = erl_element(1, argp);
ETERM *idp = erl_element(2, argp);
erlmunk_space *s;
int space_id = ERL_REF_NUMBER(space_refp);
HASH_FIND_INT(erlmunk_spaces, &space_id, s);
int body_id = ERL_INT_VALUE(idp);
erlmunk_body *b;
HASH_FIND_INT(s->bodies, &body_id, b);
cpBodyActivate(b->body);
return NULL;
}
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;
}
//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
cpSpaceRemoveShape(cpSpace *space, cpShape *shape)
{
cpBody *body = shape->body;
cpAssertHard(cpSpaceContainsShape(space, shape), "Cannot remove a shape that was not added to the space. (Removed twice maybe?)");
cpAssertSpaceUnlocked(space);
cpBool isStatic = (cpBodyGetType(body) == CP_BODY_TYPE_STATIC);
if(isStatic){
cpBodyActivateStatic(body, shape);
} else {
cpBodyActivate(body);
}
cpBodyRemoveShape(body, shape);
cpSpaceFilterArbiters(space, body, shape);
cpSpatialIndexRemove(isStatic ? space->staticShapes : space->dynamicShapes, shape, shape->hashid);
shape->space = NULL;
shape->hashid = 0;
}
//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;
}
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);
}
