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 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); }
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 cpSpaceStep(cpSpace *space, cpFloat dt) { if(!dt) return; // prevents div by zero. cpFloat dt_inv = 1.0f/dt; cpArray *bodies = space->bodies; cpArray *arbiters = space->arbiters; cpArray *joints = space->joints; // Empty the arbiter list. cpHashSetReject(space->contactSet, &contactSetReject, space); space->arbiters->num = 0; // Integrate velocities. cpFloat damping = pow(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); } // Pre-cache BBoxes and shape data. cpSpaceHashEach(space->activeShapes, &updateBBCache, NULL); // Collide! cpSpaceHashEach(space->activeShapes, &active2staticIter, space); cpSpaceHashQueryRehash(space->activeShapes, &queryFunc, space); // Prestep the arbiters. for(int i=0; i<arbiters->num; i++) cpArbiterPreStep((cpArbiter *)arbiters->arr[i], dt_inv); // Prestep the joints. for(int i=0; i<joints->num; i++){ cpJoint *joint = (cpJoint *)joints->arr[i]; joint->preStep(joint, dt_inv); } // 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<joints->num; j++){ cpJoint *joint = (cpJoint *)joints->arr[j]; joint->applyImpulse(joint); } } // 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); // Integrate positions. for(int i=0; i<bodies->num; i++){ cpBody *body = (cpBody *)bodies->arr[i]; body->position_func(body, dt); } // Increment the stamp. space->stamp++; }