void
cpSpaceEachBody(cpSpace *space, cpSpaceBodyIteratorFunc func, void *data)
{
cpSpaceLock(space); {
cpArray *bodies = space->dynamicBodies;
for(int i=0; i<bodies->num; i++){
func((cpBody *)bodies->arr[i], data);
}
cpArray *otherBodies = space->staticBodies;
for(int i=0; i<otherBodies->num; i++){
func((cpBody *)otherBodies->arr[i], data);
}
cpArray *components = space->sleepingComponents;
for(int i=0; i<components->num; i++){
cpBody *root = (cpBody *)components->arr[i];
cpBody *body = root;
while(body){
cpBody *next = body->sleeping.next;
func(body, data);
body = next;
}
}
} cpSpaceUnlock(space, cpTrue);
}
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
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
cpSpacePointQuery(cpSpace *space, cpVect point, cpLayers layers, cpGroup group, cpSpacePointQueryFunc func, void *data)
{
pointQueryContext context = {layers, group, func, data};
cpSpaceLock(space); {
cpSpaceHashPointQuery(space->activeShapes, point, (cpSpaceHashQueryFunc)pointQueryHelper, &context);
cpSpaceHashPointQuery(space->staticShapes, point, (cpSpaceHashQueryFunc)pointQueryHelper, &context);
} cpSpaceUnlock(space);
}
void
cpSpaceBBQuery(cpSpace *space, cpBB bb, cpLayers layers, cpGroup group, cpSpaceBBQueryFunc func, void *data)
{
struct BBQueryContext context = {bb, layers, group, func};
cpSpaceLock(space); {
cpSpatialIndexQuery(space->activeShapes, &context, bb, (cpSpatialIndexQueryFunc)BBQuery, data);
cpSpatialIndexQuery(space->staticShapes, &context, bb, (cpSpatialIndexQueryFunc)BBQuery, data);
} cpSpaceUnlock(space, cpTrue);
}
void
cpSpaceBBQuery(cpSpace *space, cpBB bb, cpLayers layers, cpGroup group, cpSpaceBBQueryFunc func, void *data)
{
bbQueryContext context = {layers, group, func, data};
cpSpaceLock(space); {
cpSpaceHashQuery(space->activeShapes, &bb, bb, (cpSpaceHashQueryFunc)bbQueryHelper, &context);
cpSpaceHashQuery(space->staticShapes, &bb, bb, (cpSpaceHashQueryFunc)bbQueryHelper, &context);
} cpSpaceUnlock(space);
}
void
cpSpaceEachConstraint(cpSpace *space, cpSpaceConstraintIteratorFunc func, void *data)
{
cpSpaceLock(space); {
cpArray *constraints = space->constraints;
for(int i=0; i<constraints->num; i++){
func((cpConstraint *)constraints->arr[i], data);
}
} cpSpaceUnlock(space, cpTrue);
}
void
cpSpacePointQuery(cpSpace *space, cpVect point, cpLayers layers, cpGroup group, cpSpacePointQueryFunc func, void *data)
{
struct PointQueryContext context = {point, layers, group, func, data};
cpBB bb = cpBBNewForCircle(point, 0.0f);
cpSpaceLock(space); {
cpSpatialIndexQuery(space->activeShapes, &context, bb, (cpSpatialIndexQueryFunc)PointQuery, data);
cpSpatialIndexQuery(space->staticShapes, &context, bb, (cpSpatialIndexQueryFunc)PointQuery, data);
} cpSpaceUnlock(space, cpTrue);
}
void
cpSpaceNearestPointQuery(cpSpace *space, cpVect point, cpFloat maxDistance, cpLayers layers, cpGroup group, cpSpaceNearestPointQueryFunc func, void *data)
{
struct NearestPointQueryContext context = {point, maxDistance, layers, group, func};
cpBB bb = cpBBNewForCircle(point, cpfmax(maxDistance, 0.0f));
cpSpaceLock(space); {
cpSpatialIndexQuery(space->activeShapes, &context, bb, (cpSpatialIndexQueryFunc)NearestPointQuery, data);
cpSpatialIndexQuery(space->staticShapes, &context, bb, (cpSpatialIndexQueryFunc)NearestPointQuery, data);
} cpSpaceUnlock(space, cpTrue);
}
cpBool
cpSpaceShapeQuery(cpSpace *space, cpShape *shape, cpSpaceShapeQueryFunc func, void *data)
{
cpBB bb = cpShapeCacheBB(shape);
shapeQueryContext context = {func, data, cpFalse};
cpSpaceLock(space); {
cpSpaceHashQuery(space->activeShapes, shape, bb, (cpSpaceHashQueryFunc)shapeQueryHelper, &context);
cpSpaceHashQuery(space->staticShapes, shape, bb, (cpSpaceHashQueryFunc)shapeQueryHelper, &context);
} cpSpaceUnlock(space);
return context.anyCollision;
}
void
cpSpaceSegmentQuery(cpSpace *space, cpVect start, cpVect end, cpLayers layers, cpGroup group, cpSpaceSegmentQueryFunc func, void *data)
{
segQueryContext context = {
start, end,
layers, group,
func,
};
cpSpaceLock(space); {
cpSpaceHashSegmentQuery(space->staticShapes, &context, start, end, 1.0f, (cpSpaceHashSegmentQueryFunc)segQueryFunc, data);
cpSpaceHashSegmentQuery(space->activeShapes, &context, start, end, 1.0f, (cpSpaceHashSegmentQueryFunc)segQueryFunc, data);
} cpSpaceUnlock(space);
}
cpBool
cpSpaceShapeQuery(cpSpace *space, cpShape *shape, cpSpaceShapeQueryFunc func, void *data)
{
cpBody *body = shape->body;
cpBB bb = (body ? cpShapeUpdate(shape, body->p, body->rot) : shape->bb);
struct ShapeQueryContext context = {func, data, cpFalse};
cpSpaceLock(space); {
cpSpatialIndexQuery(space->activeShapes, shape, bb, (cpSpatialIndexQueryFunc)ShapeQuery, &context);
cpSpatialIndexQuery(space->staticShapes, shape, bb, (cpSpatialIndexQueryFunc)ShapeQuery, &context);
} cpSpaceUnlock(space, cpTrue);
return context.anyCollision;
}
void
cpSpaceEachBody(cpSpace *space, cpSpaceBodyIteratorFunc func, void *data)
{
cpSpaceLock(space); {
cpArray *bodies = space->bodies;
for(int i=0; i<bodies->num; i++){
func((cpBody *)bodies->arr[i], data);
}
cpArray *components = space->sleepingComponents;
for(int i=0; i<components->num; i++){
cpBody *root = (cpBody *)components->arr[i];
CP_BODY_FOREACH_COMPONENT(root, body) func(body, data);
}
} cpSpaceUnlock(space, cpTrue);
}
void
cpSpaceEachBody(cpSpace *space, cpSpaceBodyIterator func, void *data)
{
cpSpaceLock(space); {
cpArray *bodies = space->bodies;
for(int i=0; i<bodies->num; i++){
func((cpBody *)bodies->arr[i], data);
}
cpArray *components = space->sleepingComponents;
for(int i=0; i<components->num; i++){
cpBody *root = (cpBody *)components->arr[i];
cpBody *body = root, *next;
do {
next = body->node.next;
func(body, data);
} while((body = next) != root);
}
} cpSpaceUnlock(space);
}
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);
}
