static cpBool
cachedArbitersFilter(cpArbiter *arb, struct arbiterFilterContext *context)
{
cpShape *shape = context->shape;
cpBody *body = context->body;
// Match on the filter shape, or if it's NULL the filter body
if(
(body == arb->body_a && (shape == arb->a || shape == NULL)) ||
(body == arb->body_b && (shape == arb->b || shape == NULL))
){
// Call separate when removing shapes.
if(shape && arb->state != CP_ARBITER_STATE_CACHED){
// Invalidate the arbiter since one of the shapes was removed.
arb->state = CP_ARBITER_STATE_INVALIDATED;
cpCollisionHandler *handler = arb->handler;
handler->separateFunc(arb, context->space, handler->userData);
}
cpArbiterUnthread(arb);
cpArrayDeleteObj(context->space->arbiters, arb);
cpArrayPush(context->space->pooledArbiters, arb);
return cpFalse;
}
return cpTrue;
}
static cpBool
cachedArbitersFilter(cpArbiter *arb, struct arbiterFilterContext *context)
{
cpShape *shape = context->shape;
cpBody *body = context->body;
// Match on the filter shape, or if it's NULL the filter body
if(
(body == arb->body_a && (shape == arb->a || shape == NULL)) ||
(body == arb->body_b && (shape == arb->b || shape == NULL))
){
// Call separate when removing shapes.
if(shape && arb->state != cpArbiterStateCached) cpArbiterCallSeparate(arb, context->space);
cpArbiterUnthread(arb);
cpArrayDeleteObj(context->space->arbiters, arb);
cpArrayPush(context->space->pooledArbiters, arb);
return cpFalse;
}
return cpTrue;
}
void
cpSpaceStep(cpSpace *space, cpFloat dt)
{
// don't step if the timestep is 0!
if(dt == 0.0f) return;
space->stamp++;
cpFloat prev_dt = space->curr_dt;
space->curr_dt = dt;
// Reset and empty the arbiter list.
cpArray *arbiters = space->arbiters;
for(int i=0; i<arbiters->num; i++){
cpArbiter *arb = (cpArbiter *)arbiters->arr[i];
arb->state = cpArbiterStateNormal;
// If both bodies are awake, unthread the arbiter from the contact graph.
if(!cpBodyIsSleeping(arb->body_a) && !cpBodyIsSleeping(arb->body_b)){
cpArbiterUnthread(arb);
}
}
arbiters->num = 0;
// Integrate positions
cpArray *bodies = space->bodies;
for(int i=0; i<bodies->num; i++){
cpBody *body = (cpBody *)bodies->arr[i];
body->position_func(body, dt);
}
// Find colliding pairs.
cpSpaceLock(space); {
cpSpacePushFreshContactBuffer(space);
cpSpatialIndexEach(space->activeShapes, (cpSpatialIndexIteratorFunc)cpShapeUpdateFunc, NULL);
cpSpatialIndexReindexQuery(space->activeShapes, (cpSpatialIndexQueryFunc)collideShapes, space);
} cpSpaceUnlock(space, cpFalse);
// If body sleeping is enabled, do that now.
if(space->sleepTimeThreshold != INFINITY || space->enableContactGraph){
cpSpaceProcessComponents(space, dt);
}
// Clear out old cached arbiters and call separate callbacks
cpHashSetFilter(space->cachedArbiters, (cpHashSetFilterFunc)cpSpaceArbiterSetFilter, space);
// Prestep the arbiters and constraints.
cpFloat slop = space->collisionSlop;
cpFloat biasCoef = 1.0f - cpfpow(space->collisionBias, dt);
for(int i=0; i<arbiters->num; i++){
cpArbiterPreStep((cpArbiter *)arbiters->arr[i], dt, slop, biasCoef);
}
cpArray *constraints = space->constraints;
for(int i=0; i<constraints->num; i++){
cpConstraint *constraint = (cpConstraint *)constraints->arr[i];
cpConstraintPreSolveFunc preSolve = constraint->preSolve;
if(preSolve) preSolve(constraint, space);
constraint->klass->preStep(constraint, dt);
}
// Integrate velocities.
cpFloat damping = cpfpow(space->damping, dt);
cpVect gravity = space->gravity;
for(int i=0; i<bodies->num; i++){
cpBody *body = (cpBody *)bodies->arr[i];
body->velocity_func(body, gravity, damping, dt);
}
// Apply cached impulses
cpFloat dt_coef = (prev_dt == 0.0f ? 0.0f : dt/prev_dt);
for(int i=0; i<arbiters->num; i++){
cpArbiterApplyCachedImpulse((cpArbiter *)arbiters->arr[i], dt_coef);
}
for(int i=0; i<constraints->num; i++){
cpConstraint *constraint = (cpConstraint *)constraints->arr[i];
constraint->klass->applyCachedImpulse(constraint, dt_coef);
}
// Run the impulse solver.
for(int i=0; i<space->iterations; i++){
for(int j=0; j<arbiters->num; j++){
cpArbiterApplyImpulse((cpArbiter *)arbiters->arr[j]);
}
for(int j=0; j<constraints->num; j++){
cpConstraint *constraint = (cpConstraint *)constraints->arr[j];
constraint->klass->applyImpulse(constraint);
}
}
// Run the constraint post-solve callbacks
for(int i=0; i<constraints->num; i++){
cpConstraint *constraint = (cpConstraint *)constraints->arr[i];
cpConstraintPostSolveFunc postSolve = constraint->postSolve;
if(postSolve) postSolve(constraint, space);
}
// run the post-solve callbacks
cpSpaceLock(space);
for(int i=0; i<arbiters->num; i++){
cpArbiter *arb = (cpArbiter *) arbiters->arr[i];
cpCollisionHandler *handler = arb->handler;
handler->postSolve(arb, space, handler->data);
}
cpSpaceUnlock(space, cpTrue);
}
void
cpHastySpaceStep(cpSpace *space, cpFloat dt)
{
// don't step if the timestep is 0!
if(dt == 0.0f) return;
space->stamp++;
cpFloat prev_dt = space->curr_dt;
space->curr_dt = dt;
cpArray *bodies = space->dynamicBodies;
cpArray *constraints = space->constraints;
cpArray *arbiters = space->arbiters;
// Reset and empty the arbiter list.
for(int i=0; i<arbiters->num; i++) {
cpArbiter *arb = (cpArbiter *)arbiters->arr[i];
arb->state = CP_ARBITER_STATE_NORMAL;
// If both bodies are awake, unthread the arbiter from the contact graph.
if(!cpBodyIsSleeping(arb->body_a) && !cpBodyIsSleeping(arb->body_b)) {
cpArbiterUnthread(arb);
}
}
arbiters->num = 0;
cpSpaceLock(space);
{
// Integrate positions
for(int i=0; i<bodies->num; i++) {
cpBody *body = (cpBody *)bodies->arr[i];
body->position_func(body, dt);
}
// Find colliding pairs.
cpSpacePushFreshContactBuffer(space);
cpSpatialIndexEach(space->dynamicShapes, (cpSpatialIndexIteratorFunc)cpShapeUpdateFunc, NULL);
cpSpatialIndexReindexQuery(space->dynamicShapes, (cpSpatialIndexQueryFunc)cpSpaceCollideShapes, space);
}
cpSpaceUnlock(space, cpFalse);
// Rebuild the contact graph (and detect sleeping components if sleeping is enabled)
cpSpaceProcessComponents(space, dt);
cpSpaceLock(space);
{
// Clear out old cached arbiters and call separate callbacks
cpHashSetFilter(space->cachedArbiters, (cpHashSetFilterFunc)cpSpaceArbiterSetFilter, space);
// Prestep the arbiters and constraints.
cpFloat slop = space->collisionSlop;
cpFloat biasCoef = 1.0f - cpfpow(space->collisionBias, dt);
for(int i=0; i<arbiters->num; i++) {
cpArbiterPreStep((cpArbiter *)arbiters->arr[i], dt, slop, biasCoef);
}
for(int i=0; i<constraints->num; i++) {
cpConstraint *constraint = (cpConstraint *)constraints->arr[i];
cpConstraintPreSolveFunc preSolve = constraint->preSolve;
if(preSolve) preSolve(constraint, space);
constraint->klass->preStep(constraint, dt);
}
// Integrate velocities.
cpFloat damping = cpfpow(space->damping, dt);
cpVect gravity = space->gravity;
for(int i=0; i<bodies->num; i++) {
cpBody *body = (cpBody *)bodies->arr[i];
body->velocity_func(body, gravity, damping, dt);
}
// Apply cached impulses
cpFloat dt_coef = (prev_dt == 0.0f ? 0.0f : dt/prev_dt);
for(int i=0; i<arbiters->num; i++) {
cpArbiterApplyCachedImpulse((cpArbiter *)arbiters->arr[i], dt_coef);
}
for(int i=0; i<constraints->num; i++) {
cpConstraint *constraint = (cpConstraint *)constraints->arr[i];
constraint->klass->applyCachedImpulse(constraint, dt_coef);
}
// Run the impulse solver.
cpHastySpace *hasty = (cpHastySpace *)space;
if((unsigned long)(arbiters->num + constraints->num) > hasty->constraint_count_threshold) {
RunWorkers(hasty, Solver);
} else {
Solver(space, 0, 1);
}
// Run the constraint post-solve callbacks
for(int i=0; i<constraints->num; i++) {
cpConstraint *constraint = (cpConstraint *)constraints->arr[i];
cpConstraintPostSolveFunc postSolve = constraint->postSolve;
if(postSolve) postSolve(constraint, space);
}
// run the post-solve callbacks
for(int i=0; i<arbiters->num; i++) {
cpArbiter *arb = (cpArbiter *) arbiters->arr[i];
cpCollisionHandler *handler = arb->handler;
handler->postSolveFunc(arb, space, handler->userData);
}
}
cpSpaceUnlock(space, cpTrue);
}
