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