/* void dxProcessIslands (dxWorld *world, dReal stepsize, dstepper_fn_t stepper) { dxBody *b,*bb,**body; dxJoint *j,**joint; // nothing to do if no bodies if (world->nb <= 0) return; // handle auto-disabling of bodies dInternalHandleAutoDisabling (world,stepsize); // make arrays for body and joint lists (for a single island) to go into body = (dxBody**) ALLOCA (world->nb * sizeof(dxBody*)); joint = (dxJoint**) ALLOCA (world->nj * sizeof(dxJoint*)); int bcount = 0; // number of bodies in `body' int jcount = 0; // number of joints in `joint' // set all body/joint tags to 0 for (b=world->firstbody; b; b=(dxBody*)b->next) b->tag = 0; for (j=world->firstjoint; j; j=(dxJoint*)j->next) j->tag = 0; // allocate a stack of unvisited bodies in the island. the maximum size of // the stack can be the lesser of the number of bodies or joints, because // new bodies are only ever added to the stack by going through untagged // joints. all the bodies in the stack must be tagged! int stackalloc = (world->nj < world->nb) ? world->nj : world->nb; dxBody **stack = (dxBody**) ALLOCA (stackalloc * sizeof(dxBody*)); for (bb=world->firstbody; bb; bb=(dxBody*)bb->next) { // get bb = the next enabled, untagged body, and tag it if (bb->tag || (bb->flags & dxBodyDisabled)) continue; bb->tag = 1; // tag all bodies and joints starting from bb. int stacksize = 0; b = bb; body[0] = bb; bcount = 1; jcount = 0; goto quickstart; while (stacksize > 0) { b = stack[--stacksize]; // pop body off stack body[bcount++] = b; // put body on body list quickstart: // traverse and tag all body's joints, add untagged connected bodies // to stack for (dxJointNode *n=b->firstjoint; n; n=n->next) { if (!n->joint->tag) { n->joint->tag = 1; joint[jcount++] = n->joint; if (n->body && !n->body->tag) { n->body->tag = 1; stack[stacksize++] = n->body; } } } dIASSERT(stacksize <= world->nb); dIASSERT(stacksize <= world->nj); } // now do something with body and joint lists stepper (world,body,bcount,joint,jcount,stepsize); // what we've just done may have altered the body/joint tag values. // we must make sure that these tags are nonzero. // also make sure all bodies are in the enabled state. int i; for (i=0; i<bcount; i++) { body[i]->tag = 1; body[i]->flags &= ~dxBodyDisabled; } for (i=0; i<jcount; i++) joint[i]->tag = 1; } // if debugging, check that all objects (except for disabled bodies, // unconnected joints, and joints that are connected to disabled bodies) // were tagged. # ifndef dNODEBUG for (b=world->firstbody; b; b=(dxBody*)b->next) { if (b->flags & dxBodyDisabled) { if (b->tag) dDebug (0,"disabled body tagged"); } else { if (!b->tag) dDebug (0,"enabled body not tagged"); } } for (j=world->firstjoint; j; j=(dxJoint*)j->next) { if ((j->node[0].body && (j->node[0].body->flags & dxBodyDisabled)==0) || (j->node[1].body && (j->node[1].body->flags & dxBodyDisabled)==0)) { if (!j->tag) dDebug (0,"attached enabled joint not tagged"); } else { if (j->tag) dDebug (0,"unattached or disabled joint tagged"); } } # endif } */ void dxProcessIslands (dxWorld *world, dReal stepsize, dstepper_fn_t stepper) { // nothing to do if no bodies if (world->nb <= 0) return; # ifdef TIMING dTimerStart ("creating joint and body arrays"); # endif dxBody **bodies, *body; dxJoint **joints, *joint; joints = (dxJoint **) ALLOCA (world->nj * sizeof (dxJoint *)); bodies = (dxBody **) ALLOCA (world->nb * sizeof (dxBody *)); int nj = 0; for (joint = world->firstjoint; joint; joint = (dxJoint *) joint->next) joints[nj++] = joint; int nb = 0; for (body = world->firstbody; body; body = (dxBody *) body->next) { body->flags &= ~dxBodyDisabled; bodies[nb++] = body; } // now do something with body and joint lists stepper (world,bodies,nb,joints,nj,stepsize); //stepper (world,body,bcount,joint,jcount,stepsize); # ifdef TIMING dTimerEnd (); dTimerReport (stdout, 1); # endif }
void dxQuickStepper (dxWorld *world, dxBody * const *body, int nb, dxJoint * const *_joint, int nj, dReal stepsize) { int i,j; IFTIMING(dTimerStart("preprocessing");)