static size_t BuildIslandsAndEstimateStepperMemoryRequirements(
dxWorldProcessIslandsInfo &islandsinfo, dxWorldProcessMemArena *memarena,
dxWorld *world, dReal stepsize, dmemestimate_fn_t stepperestimate)
{
size_t maxreq = 0;
// handle auto-disabling of bodies
dInternalHandleAutoDisabling (world,stepsize);
unsigned int nb = world->nb, nj = world->nj;
// Make array for island body/joint counts
unsigned int *islandsizes = memarena->AllocateArray<unsigned int>(2 * (size_t)nb);
unsigned int *sizescurr;
// make arrays for body and joint lists (for a single island) to go into
dxBody **body = memarena->AllocateArray<dxBody *>(nb);
dxJoint **joint = memarena->AllocateArray<dxJoint *>(nj);
BEGIN_STATE_SAVE(memarena, stackstate) {
// 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!
unsigned int stackalloc = (nj < nb) ? nj : nb;
dxBody **stack = memarena->AllocateArray<dxBody *>(stackalloc);
{
// set all body/joint tags to 0
for (dxBody *b=world->firstbody; b; b=(dxBody*)b->next) b->tag = 0;
for (dxJoint *j=world->firstjoint; j; j=(dxJoint*)j->next) j->tag = 0;
}
sizescurr = islandsizes;
dxBody **bodystart = body;
dxJoint **jointstart = joint;
for (dxBody *bb=world->firstbody; bb; bb=(dxBody*)bb->next) {
// get bb = the next enabled, untagged body, and tag it
if (!bb->tag) {
if (!(bb->flags & dxBodyDisabled)) {
bb->tag = 1;
dxBody **bodycurr = bodystart;
dxJoint **jointcurr = jointstart;
// tag all bodies and joints starting from bb.
*bodycurr++ = bb;
unsigned int stacksize = 0;
dxBody *b = bb;
while (true) {
// traverse and tag all body's joints, add untagged connected bodies
// to stack
for (dxJointNode *n=b->firstjoint; n; n=n->next) {
dxJoint *njoint = n->joint;
if (!njoint->tag) {
if (njoint->isEnabled()) {
njoint->tag = 1;
*jointcurr++ = njoint;
dxBody *nbody = n->body;
// Body disabled flag is not checked here. This is how auto-enable works.
if (nbody && nbody->tag <= 0) {
nbody->tag = 1;
// Make sure all bodies are in the enabled state.
nbody->flags &= ~dxBodyDisabled;
stack[stacksize++] = nbody;
}
} else {
njoint->tag = -1; // Used in Step to prevent search over disabled joints (not needed for QuickStep so far)
}
}
}
dIASSERT(stacksize <= (unsigned int)world->nb);
dIASSERT(stacksize <= (unsigned int)world->nj);
if (stacksize == 0) {
break;
}
b = stack[--stacksize]; // pop body off stack
*bodycurr++ = b; // put body on body list
}
unsigned int bcount = (unsigned int)(bodycurr - bodystart);
unsigned int jcount = (unsigned int)(jointcurr - jointstart);
dIASSERT((size_t)(bodycurr - bodystart) <= (size_t)UINT_MAX);
dIASSERT((size_t)(jointcurr - jointstart) <= (size_t)UINT_MAX);
sizescurr[dxISE_BODIES_COUNT] = bcount;
sizescurr[dxISE_JOINTS_COUNT] = jcount;
sizescurr += dxISE__MAX;
size_t islandreq = stepperestimate(bodystart, bcount, jointstart, jcount);
maxreq = (maxreq > islandreq) ? maxreq : islandreq;
bodystart = bodycurr;
jointstart = jointcurr;
} else {
bb->tag = -1; // Not used so far (assigned to retain consistency with joints)
}
}
}
} END_STATE_SAVE(memarena, stackstate);
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
}
static void
processIslandsFast (dxWorld * world, dReal stepsize, int maxiterations)
{
dxBody *b, *bb, **body;
dxJoint *j, **joint;
// nothing to do if no bodies
if (world->nb <= 0)
return;
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'
int tbcount = 0;
int tjcount = 0;
// 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 *));
int *autostack = (int *) ALLOCA (stackalloc * sizeof (int));
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;
int firsttime = 1;
int autoDepth = GetGlobalData()->autoEnableDepth;
b = bb;
body[0] = bb;
bcount = 1;
jcount = 0;
while (stacksize > 0 || firsttime)
{
if (!firsttime)
{
b = stack[--stacksize]; // pop body off stack
autoDepth = autostack[stacksize];
body[bcount++] = b; // put body on body list
}
else
{
firsttime = 0;
}
// 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)
{
int thisDepth = GetGlobalData()->autoEnableDepth;
n->joint->tag = 1;
joint[jcount++] = n->joint;
if (n->body && !n->body->tag)
{
if (n->body->flags & dxBodyDisabled)
thisDepth = autoDepth - 1;
if (thisDepth < 0)
continue;
n->body->flags &= ~dxBodyDisabled;
n->body->tag = 1;
autostack[stacksize] = thisDepth;
stack[stacksize++] = n->body;
}
}
}
}
// now do something with body and joint lists
dInternalStepIslandFast (world, body, bcount, joint, jcount, stepsize, maxiterations);
// 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;
tbcount += bcount;
tjcount += jcount;
}
}
// sorts out islands,
// cllocates array for island information into arrays: body[nj], joint[nb], islandsizes[2*nb]
// context->SavePreallocations(islandcount, islandsizes, body, joint,islandreqs);
// and put into context
//
static size_t BuildIslandsAndEstimateStepperMemoryRequirements(dxWorldProcessContext *context,
dxWorld *world, dReal stepsize, dmemestimate_fn_t stepperestimate)
{
const int sizeelements = 2;
size_t maxreq = 0;
// handle auto-disabling of bodies
dInternalHandleAutoDisabling (world,stepsize);
int nb = world->nb, nj = world->nj;
// Make array for island body/joint counts
int *islandsizes = context->AllocateArray<int>(2 * nb);
int *sizescurr;
// an array to save all islandreqs for each thread
size_t *islandreqs = context->AllocateArray<size_t>(nb);
size_t *islandreqscurr;
// make arrays for body and joint lists (for a single island) to go into
dxBody **body = context->AllocateArray<dxBody *>(nb); // allocates a block of pointers and get back a pointer to first element
dxJoint **joint = context->AllocateArray<dxJoint *>(nj); // allocates a block of pointers and get back a pointer to first element
BEGIN_STATE_SAVE(context, stackstate) {
// stack is used to hold untagged bodies when traversing through all the joint-linked bodies.
// at the end, all the bodies in the stack are popped back out into the island.
//
// 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 = (nj < nb) ? nj : nb;
dxBody **stack = context->AllocateArray<dxBody *>(stackalloc); // a body stack
{
// set all body/joint island_tags to 0
for (dxBody *b=world->firstbody; b; b=(dxBody*)b->next) b->island_tag = 0;
for (dxJoint *j=world->firstjoint; j; j=(dxJoint*)j->next) j->island_tag = 0;
}
//int island_count = 0;
sizescurr = islandsizes;
islandreqscurr = islandreqs;
dxBody **bodystart = body;
dxJoint **jointstart = joint;
// loop through all body, tag each one as it is processed
// every step in this for loop is one island
for (dxBody *bb=world->firstbody; bb; bb=(dxBody*)bb->next) {
// get bb = the next enabled, untagged body, and tag it
if (!bb->island_tag) {
if (!(bb->flags & dxBodyDisabled)) {
bb->island_tag = 1;
dxBody **bodycurr = bodystart;
dxJoint **jointcurr = jointstart;
// tag all bodies and joints starting from bb.
*bodycurr++ = bb;
int stacksize = 0;
dxBody *b = bb;
while (true) {
// traverse and island_tag all body's joints, add untagged connected bodies
// to stack
for (dxJointNode *n=b->firstjoint; n; n=n->next) {
dxJoint *njoint = n->joint;
if (!njoint->island_tag) {
if (njoint->isEnabled()) {
njoint->island_tag = 1;
*jointcurr++ = njoint;
dxBody *nbody = n->body;
// Body disabled flag is not checked here. This is how auto-enable works.
if (nbody && nbody->island_tag <= 0) {
nbody->island_tag = 1;
// Make sure all bodies are in the enabled state.
nbody->flags &= ~dxBodyDisabled;
stack[stacksize++] = nbody;
}
} else {
njoint->tag = -1; // Used in Step to prevent search over disabled joints (not needed for QuickStep so far)
}
}
}
dIASSERT(stacksize <= world->nb);
dIASSERT(stacksize <= world->nj);
if (stacksize == 0) {
break;
}
b = stack[--stacksize]; // pop body off stack
*bodycurr++ = b; // put body on body list
}
int bcount = bodycurr - bodystart;
int jcount = jointcurr - jointstart;
sizescurr[0] = bcount;
sizescurr[1] = jcount;
sizescurr += sizeelements;
// save individual islandreq for each island separately
*islandreqscurr = stepperestimate(bodystart, bcount, jointstart, jcount);
maxreq = (maxreq > *islandreqscurr) ? maxreq : *islandreqscurr;
//printf("island %d complete, stepper %d maxreq %d \n",island_count++,*islandreqscurr, maxreq);
islandreqscurr += 1;
bodystart = bodycurr;
jointstart = jointcurr;
} else {
bb->island_tag = -1; // Not used so far (assigned to retain consistency with joints)
}
}
}
} END_STATE_SAVE(context, stackstate); // restores contex pointer m_pAllocCurrent back to what it was before this block
