void
cpSpaceFilterArbiters(cpSpace *space, cpBody *body, cpShape *filter)
{
cpSpaceLock(space); {
struct arbiterFilterContext context = {space, body, filter};
cpHashSetFilter(space->cachedArbiters, (cpHashSetFilterFunc)cachedArbitersFilter, &context);
} cpSpaceUnlock(space, cpTrue);
}
void
cpSpaceRemoveStaticShape(cpSpace *space, cpShape *shape)
{
cpAssert(cpHashSetFind(space->staticShapes->handleSet, shape->hashid, shape), "Cannot remove a static shape that was never added to the space.");
cpAssertSpaceUnlocked(space);
removalContext context = {space, shape};
cpHashSetFilter(space->contactSet, (cpHashSetFilterFunc)contactSetFilterRemovedShape, &context);
cpSpaceHashRemove(space->staticShapes, shape, shape->hashid);
}
void
cpSpaceRemoveStaticShape(cpSpace *space, cpShape *shape)
{
cpAssertWarn(cpHashSetFind(space->staticShapes->handleSet, shape->hashid, shape),
"Cannot remove a static or sleeping shape that was not added to the space. (Removed twice maybe?)");
cpAssertSpaceUnlocked(space);
removalContext context = {space, shape};
cpHashSetFilter(space->contactSet, (cpHashSetFilterFunc)contactSetFilterRemovedShape, &context);
cpSpaceHashRemove(space->staticShapes, shape, shape->hashid);
cpSpaceActivateShapesTouchingShape(space, 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);
}
void
cpSpaceStep(cpSpace *space, cpFloat dt)
{
if(!dt) return; // don't step if the timestep is 0!
cpFloat dt_inv = 1.0f/dt;
cpArray *bodies = space->bodies;
cpArray *constraints = space->constraints;
space->locked = 1;
// Empty the arbiter list.
space->arbiters->num = 0;
// Integrate positions.
for(int i=0; i<bodies->num; i++){
cpBody *body = (cpBody *)bodies->arr[i];
body->position_func(body, dt);
}
// Pre-cache BBoxes and shape data.
cpSpaceHashEach(space->activeShapes, (cpSpaceHashIterator)updateBBCache, NULL);
// Collide!
cpSpacePushNewContactBuffer(space);
cpSpaceHashEach(space->activeShapes, (cpSpaceHashIterator)active2staticIter, space);
cpSpaceHashQueryRehash(space->activeShapes, (cpSpaceHashQueryFunc)queryFunc, space);
// Clear out old cached arbiters and dispatch untouch functions
cpHashSetFilter(space->contactSet, (cpHashSetFilterFunc)contactSetFilter, space);
// Prestep the arbiters.
cpArray *arbiters = space->arbiters;
for(int i=0; i<arbiters->num; i++)
cpArbiterPreStep((cpArbiter *)arbiters->arr[i], dt_inv);
// Prestep the constraints.
for(int i=0; i<constraints->num; i++){
cpConstraint *constraint = (cpConstraint *)constraints->arr[i];
constraint->klass->preStep(constraint, dt, dt_inv);
}
for(int i=0; i<space->elasticIterations; i++){
for(int j=0; j<arbiters->num; j++)
cpArbiterApplyImpulse((cpArbiter *)arbiters->arr[j], 1.0f);
for(int j=0; j<constraints->num; j++){
cpConstraint *constraint = (cpConstraint *)constraints->arr[j];
constraint->klass->applyImpulse(constraint);
}
}
// Integrate velocities.
cpFloat damping = cpfpow(1.0f/space->damping, -dt);
for(int i=0; i<bodies->num; i++){
cpBody *body = (cpBody *)bodies->arr[i];
body->velocity_func(body, space->gravity, damping, dt);
}
for(int i=0; i<arbiters->num; i++)
cpArbiterApplyCachedImpulse((cpArbiter *)arbiters->arr[i]);
// run the old-style elastic solver if elastic iterations are disabled
cpFloat elasticCoef = (space->elasticIterations ? 0.0f : 1.0f);
// 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], elasticCoef);
for(int j=0; j<constraints->num; j++){
cpConstraint *constraint = (cpConstraint *)constraints->arr[j];
constraint->klass->applyImpulse(constraint);
}
}
space->locked = 0;
// run the post solve callbacks
for(int i=0; i<arbiters->num; i++){
cpArbiter *arb = (cpArbiter *) arbiters->arr[i];
cpCollisionHandler *handler = arb->handler;
handler->postSolve(arb, space, handler->data);
arb->state = cpArbiterStateNormal;
}
// Run the post step callbacks
while(space->postStepCallbacks->entries){
cpHashSet *callbacks = space->postStepCallbacks;
space->postStepCallbacks = cpHashSetNew(0, (cpHashSetEqlFunc)postStepFuncSetEql, (cpHashSetTransFunc)postStepFuncSetTrans);
cpHashSetEach(callbacks, (cpHashSetIterFunc)postStepCallbackSetIter, space);
}
// cpFloat dvsq = cpvdot(space->gravity, space->gravity);
// dvsq *= dt*dt * space->damping*space->damping;
// for(int i=0; i<bodies->num; i++)
// cpBodyMarkLowEnergy(bodies->arr[i], dvsq, space->sleepTicks);
// Increment the stamp.
space->stamp++;
}
void
cpSpaceStep(cpSpace *space, cpFloat dt)
{
if(!dt) return; // don't step if the timestep is 0!
cpFloat dt_inv = 1.0f/dt;
cpArray *bodies = space->bodies;
cpArray *constraints = space->constraints;
// Empty the arbiter list.
space->arbiters->num = 0;
//i51AdeOsLog ( i51_LOG_DC , "for" ) ;
// Integrate positions.
for(int i=0; i<bodies->num; i++){
cpBody *body = (cpBody *)bodies->arr[i];
body->position_func(body, dt);
}
// Pre-cache BBoxes and shape data.
//i51AdeOsLog ( i51_LOG_DC , "cpSpaceHashEach" ) ;
cpSpaceHashEach(space->activeShapes, (cpSpaceHashIterator)updateBBCache, NULL);
cpSpaceLock(space);
// Collide!
//i51AdeOsLog ( i51_LOG_DC , "cpSpacePushFreshContactBuffer" ) ;
cpSpacePushFreshContactBuffer(space);
if(space->staticShapes->handleSet->entries)
cpSpaceHashEach(space->activeShapes, (cpSpaceHashIterator)active2staticIter, space);
cpSpaceHashQueryRehash(space->activeShapes, (cpSpaceHashQueryFunc)queryFunc, space);
cpSpaceUnlock(space);
//i51AdeOsLog ( i51_LOG_DC , "sleepTimeThreshold" ) ;
// If body sleeping is enabled, do that now.
if(space->sleepTimeThreshold != INFINITY){
cpSpaceProcessComponents(space, dt);
bodies = space->bodies; // rebuilt by processContactComponents()
}/**/
// Clear out old cached arbiters and dispatch untouch functions
//i51AdeOsLog ( i51_LOG_DC , "cpHashSetFilter" ) ;
cpHashSetFilter(space->contactSet, (cpHashSetFilterFunc)contactSetFilter, space);
// Prestep the arbiters.
//i51AdeOsLog ( i51_LOG_DC , "space->arbiters" ) ;
cpArray *arbiters = space->arbiters;
for(int i=0; i<arbiters->num; i++)
cpArbiterPreStep((cpArbiter *)arbiters->arr[i], dt_inv);
// Prestep the constraints.
//i51AdeOsLog ( i51_LOG_DC , "constraints->num=%d",constraints->num ) ;
for(int i=0; i<constraints->num; i++){
cpConstraint *constraint = (cpConstraint *)constraints->arr[i];
//i51AdeOsLog ( i51_LOG_DC , "i=%d,preStep=%X",i,constraint->klass->preStep) ;
constraint->klass->preStep(constraint, dt, dt_inv);
}
//i51AdeOsLog ( i51_LOG_DC , "space->elasticIterations=%d",space->elasticIterations ) ;
for(int i=0; i<space->elasticIterations; i++){
for(int j=0; j<arbiters->num; j++)
cpArbiterApplyImpulse((cpArbiter *)arbiters->arr[j], 1.0f);
for(int j=0; j<constraints->num; j++){
cpConstraint *constraint = (cpConstraint *)constraints->arr[j];
constraint->klass->applyImpulse(constraint);
}
}
// Integrate velocities.
//i51AdeOsLog ( i51_LOG_DC , "space->damping=%d",space->damping ) ;
cpFloat damping = cpfpow(1.0f/space->damping, -dt);
for(int i=0; i<bodies->num; i++){
cpBody *body = (cpBody *)bodies->arr[i];
body->velocity_func(body, space->gravity, damping, dt);
}
//i51AdeOsLog ( i51_LOG_DC , "arbiters->num=%d",arbiters->num) ;
for(int i=0; i<arbiters->num; i++)
cpArbiterApplyCachedImpulse((cpArbiter *)arbiters->arr[i]);
// run the old-style elastic solver if elastic iterations are disabled
cpFloat elasticCoef = (space->elasticIterations ? 0.0f : 1.0f);
// Run the impulse solver.
//i51AdeOsLog ( i51_LOG_DC , "space->iterations=%d",space->iterations) ;
for(int i=0; i<space->iterations; i++){
for(int j=0; j<arbiters->num; j++)
cpArbiterApplyImpulse((cpArbiter *)arbiters->arr[j], elasticCoef);
for(int j=0; j<constraints->num; j++){
cpConstraint *constraint = (cpConstraint *)constraints->arr[j];
constraint->klass->applyImpulse(constraint);
}
}
cpSpaceLock(space);
// run the post solve callbacks
//i51AdeOsLog ( i51_LOG_DC , "run the post solve callbacks") ;
for(int i=0; i<arbiters->num; i++){
cpArbiter *arb = (cpArbiter *) arbiters->arr[i];
cpCollisionHandler *handler = arb->handler;
handler->postSolve(arb, space, handler->data);
arb->state = cpArbiterStateNormal;
}
cpSpaceUnlock(space);
// Run the post step callbacks
// Loop because post step callbacks may create more post step callbacks
//i51AdeOsLog ( i51_LOG_DC , "space->postStepCallbacks") ;
while(space->postStepCallbacks){
cpHashSet *callbacks = space->postStepCallbacks;
space->postStepCallbacks = NULL;
cpHashSetEach(callbacks, (cpHashSetIterFunc)postStepCallbackSetIter, space);
cpHashSetFree(callbacks);
}
// Increment the stamp.
space->stamp++;
//i51AdeOsLog ( i51_LOG_DC , "_____________OK_______________") ;
}
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);
}
