void
cpSpaceEachShape(cpSpace *space, cpSpaceShapeIteratorFunc func, void *data)
{
cpSpaceLock(space); {
spaceShapeContext context = {func, data};
cpSpatialIndexEach(space->dynamicShapes, (cpSpatialIndexIteratorFunc)spaceEachShapeIterator, &context);
cpSpatialIndexEach(space->staticShapes, (cpSpatialIndexIteratorFunc)spaceEachShapeIterator, &context);
} cpSpaceUnlock(space, cpTrue);
}
void
cpSpaceUseSpatialHash(cpSpace *space, cpFloat dim, int count)
{
cpSpatialIndex *staticShapes = cpSpaceHashNew(dim, count, (cpSpatialIndexBBFunc)cpShapeGetBB, NULL);
cpSpatialIndex *dynamicShapes = cpSpaceHashNew(dim, count, (cpSpatialIndexBBFunc)cpShapeGetBB, staticShapes);
cpSpatialIndexEach(space->staticShapes, (cpSpatialIndexIteratorFunc)copyShapes, staticShapes);
cpSpatialIndexEach(space->dynamicShapes, (cpSpatialIndexIteratorFunc)copyShapes, dynamicShapes);
cpSpatialIndexFree(space->staticShapes);
cpSpatialIndexFree(space->dynamicShapes);
space->staticShapes = staticShapes;
space->dynamicShapes = dynamicShapes;
}
void
cpSpaceReindexStatic(cpSpace *space)
{
cpAssertHard(!space->locked, "You cannot manually reindex objects while the space is locked. Wait until the current query or step is complete.");
cpSpatialIndexEach(space->staticShapes, (cpSpatialIndexIteratorFunc)&cpShapeUpdateFunc, NULL);
cpSpatialIndexReindex(space->staticShapes);
}
void
cpSpatialIndexCollideStatic(cpSpatialIndex *dynamicIndex, cpSpatialIndex *staticIndex, cpSpatialIndexQueryFunc func, void *data)
{
if(staticIndex && cpSpatialIndexCount(staticIndex) > 0){
dynamicToStaticContext context = {dynamicIndex->bbfunc, staticIndex, func, data};
cpSpatialIndexEach(dynamicIndex, (cpSpatialIndexIteratorFunc)dynamicToStaticIter, &context);
}
}
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);
}
void
cpSpaceReindexStatic(cpSpace *space)
{
cpSpatialIndexEach(space->staticShapes, (cpSpatialIndexIteratorFunc)&updateBBCache, NULL);
cpSpatialIndexReindex(space->staticShapes);
}
void SpatialIndex::each(SpatialIndexIteratorFunc func)
{
cpSpatialIndexEach(index,*SpatialIndexEach,&func);
}
void SpatialIndex::each(cpSpatialIndexIteratorFunc func,void *data)
{
cpSpatialIndexEach(index,func,data);
}
