text: /*************************************************************************
   *                                                                       *
   * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith.       *
   * All rights reserved.  Email: russ@q12.org   Web: www.q12.org          *
   *                                                                       *
   * This library is free software; you can redistribute it and/or         *
   * modify it under the terms of EITHER:                                  *
   *   (1) The GNU Lesser General Public License as published by the Free  *
   *       Software Foundation; either version 2.1 of the License, or (at  *
   *       your option) any later version. The text of the GNU Lesser      *
   *       General Public License is included with this library in the     *
   *       file LICENSE.TXT.                                               *
   *   (2) The BSD-style license that is included with this library in     *
   *       the file LICENSE-BSD.TXT.                                       *
   *                                                                       *
   * This library is distributed in the hope that it will be useful,       *
   * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
   * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files    *
   * LICENSE.TXT and LICENSE-BSD.TXT for more details.                     *
   *                                                                       *
   *************************************************************************/
  
  #include <ode/ode.h>
  #include "config.h"
  #include "objects.h"
  #include "joints/joint.h"
  #include "util.h"
  #include <boost/thread/recursive_mutex.hpp>
  #include <boost/bind.hpp>
  #include <ode/timer.h>
  
  #undef REPORT_THREAD_TIMING
  #undef TIMING
  #ifdef TIMING
  #define IFTIMING(x) x
  #else
  #define IFTIMING(x) ((void)0)
  #endif
  
  static void InternalFreeWorldProcessContext (dxWorldProcessContext *context);
  
  //****************************************************************************
  // Malloc based world stepping memory manager
  
  /*extern */dxWorldProcessMemoryManager g_WorldProcessMallocMemoryManager(dAlloc, dRealloc, dFree);
  /*extern */dxWorldProcessMemoryReserveInfo g_WorldProcessDefaultReserveInfo(dWORLDSTEP_RESERVEFACTOR_DEFAULT, dWORLDSTEP_RESERVESIZE_DEFAULT);
  
  
  //****************************************************************************
  // dxWorldProcessContext implementation
  
  void dxWorldProcessContext::CleanupContext()
  {
    ResetState();
    ClearPreallocations();
    FreePreallocationsContext();
  }
  
  void dxWorldProcessContext::SavePreallocations(int islandcount, int const *islandsizes, dxBody *const *bodies, dxJoint *const *joints, size_t const *islandreqs)
  {
    m_IslandCount = islandcount;
    m_pIslandReqs = islandreqs;
    m_pIslandSizes = islandsizes;
    m_pBodies = bodies;
    m_pJoints = joints;
  }
  
  void dxWorldProcessContext::RetrievePreallocations(int &islandcount, int const *&islandsizes, dxBody *const *&bodies, dxJoint *const *&joints, size_t const *&islandreqs)
  {
    islandcount = m_IslandCount;
    islandreqs = m_pIslandReqs;
    islandsizes = m_pIslandSizes;
    bodies = m_pBodies;
    joints = m_pJoints;
  }
  
  void dxWorldProcessContext::OffsetPreallocations(size_t stOffset)
  {
    // m_IslandCount = -- no offset for count
    m_pIslandSizes = m_pIslandSizes ? (int const *)((size_t)m_pIslandSizes + stOffset) : NULL;
    m_pIslandReqs = m_pIslandReqs ? (size_t const *)((size_t)m_pIslandReqs + stOffset) : NULL;
    m_pBodies = m_pBodies ? (dxBody *const *)((size_t)m_pBodies + stOffset) : NULL;
    m_pJoints = m_pJoints ? (dxJoint *const *)((size_t)m_pJoints + stOffset) : NULL;
  }
  
  void dxWorldProcessContext::CopyPreallocations(const dxWorldProcessContext *othercontext)
  {
    m_IslandCount = othercontext->m_IslandCount;
    m_pIslandSizes = othercontext->m_pIslandSizes;
    m_pIslandReqs = othercontext->m_pIslandReqs;
    m_pBodies = othercontext->m_pBodies;
    m_pJoints = othercontext->m_pJoints;
  }
  
  void dxWorldProcessContext::ClearPreallocations()
  {
    m_IslandCount = 0;
    m_pIslandSizes = NULL;
    m_pIslandReqs = NULL;
    m_pBodies = NULL;
    m_pJoints = NULL;
  }
  
  void dxWorldProcessContext::FreePreallocationsContext()
  {
    if (m_pPreallocationcContext) {
      InternalFreeWorldProcessContext(m_pPreallocationcContext);
      m_pPreallocationcContext = NULL;
    }
  }
  
  
  //****************************************************************************
  // Auto disabling
  
  void dInternalHandleAutoDisabling (dxWorld *world, dReal stepsize)
  {
  	dxBody *bb;
  	for ( bb=world->firstbody; bb; bb=(dxBody*)bb->next )
  	{
  		// don't freeze objects mid-air (patch 1586738)
  		if ( bb->firstjoint == NULL ) continue;
  
  		// nothing to do unless this body is currently enabled and has
  		// the auto-disable flag set
  		if ( (bb->flags & (dxBodyAutoDisable|dxBodyDisabled)) != dxBodyAutoDisable ) continue;
  
  		// if sampling / threshold testing is disabled, we can never sleep.
  		if ( bb->adis.average_samples == 0 ) continue;
  
  		//
  		// see if the body is idle
  		//
  		
  #ifndef dNODEBUG
  		// sanity check
  		if ( bb->average_counter >= bb->adis.average_samples )
  		{
  			dUASSERT( bb->average_counter < bb->adis.average_samples, "buffer overflow" );
  
  			// something is going wrong, reset the average-calculations
  			bb->average_ready = 0; // not ready for average calculation
  			bb->average_counter = 0; // reset the buffer index
  		}
  #endif // dNODEBUG
  
  		// sample the linear and angular velocity
  		bb->average_lvel_buffer[bb->average_counter][0] = bb->lvel[0];
  		bb->average_lvel_buffer[bb->average_counter][1] = bb->lvel[1];
  		bb->average_lvel_buffer[bb->average_counter][2] = bb->lvel[2];
  		bb->average_avel_buffer[bb->average_counter][0] = bb->avel[0];
  		bb->average_avel_buffer[bb->average_counter][1] = bb->avel[1];
  		bb->average_avel_buffer[bb->average_counter][2] = bb->avel[2];
  		bb->average_counter++;
  
  		// buffer ready test
  		if ( bb->average_counter >= bb->adis.average_samples )
  		{
  			bb->average_counter = 0; // fill the buffer from the beginning
  			bb->average_ready = 1; // this body is ready now for average calculation
  		}
  
  		int idle = 0; // Assume it's in motion unless we have samples to disprove it.
  
  		// enough samples?
  		if ( bb->average_ready )
  		{
  			idle = 1; // Initial assumption: IDLE
  
  			// the sample buffers are filled and ready for calculation
  			dVector3 average_lvel, average_avel;
  
  			// Store first velocity samples
  			average_lvel[0] = bb->average_lvel_buffer[0][0];
  			average_avel[0] = bb->average_avel_buffer[0][0];
  			average_lvel[1] = bb->average_lvel_buffer[0][1];
  			average_avel[1] = bb->average_avel_buffer[0][1];
  			average_lvel[2] = bb->average_lvel_buffer[0][2];
  			average_avel[2] = bb->average_avel_buffer[0][2];
  			
  			// If we're not in "instantaneous mode"
  			if ( bb->adis.average_samples > 1 )
  			{
  				// add remaining velocities together
  				for ( unsigned int i = 1; i < bb->adis.average_samples; ++i )
  				{
  					average_lvel[0] += bb->average_lvel_buffer[i][0];
  					average_avel[0] += bb->average_avel_buffer[i][0];
  					average_lvel[1] += bb->average_lvel_buffer[i][1];
  					average_avel[1] += bb->average_avel_buffer[i][1];
  					average_lvel[2] += bb->average_lvel_buffer[i][2];
  					average_avel[2] += bb->average_avel_buffer[i][2];
  				}
  
  				// make average
  				dReal r1 = dReal( 1.0 ) / dReal( bb->adis.average_samples );
  
  				average_lvel[0] *= r1;
  				average_avel[0] *= r1;
  				average_lvel[1] *= r1;
  				average_avel[1] *= r1;
  				average_lvel[2] *= r1;
  				average_avel[2] *= r1;
  			}
  
  			// threshold test
  			dReal av_lspeed, av_aspeed;
  			av_lspeed = dCalcVectorDot3( average_lvel, average_lvel );
  			if ( av_lspeed > bb->adis.linear_average_threshold )
  			{
  				idle = 0; // average linear velocity is too high for idle
  			}
  			else
  			{
  				av_aspeed = dCalcVectorDot3( average_avel, average_avel );
  				if ( av_aspeed > bb->adis.angular_average_threshold )
  				{
  					idle = 0; // average angular velocity is too high for idle
  				}
  			}
  		}
  
  		// if it's idle, accumulate steps and time.
  		// these counters won't overflow because this code doesn't run for disabled bodies.
  		if (idle) {
  			bb->adis_stepsleft--;
  			bb->adis_timeleft -= stepsize;
  		}
  		else {
  			// Reset countdowns
  			bb->adis_stepsleft = bb->adis.idle_steps;
  			bb->adis_timeleft = bb->adis.idle_time;
  		}
  
  		// disable the body if it's idle for a long enough time
  		if ( bb->adis_stepsleft <= 0 && bb->adis_timeleft <= 0 )
  		{
  			bb->flags |= dxBodyDisabled; // set the disable flag
        if (bb->disabled_callback)
          bb->disabled_callback(bb);
  
  			// disabling bodies should also include resetting the velocity
  			// should prevent jittering in big "islands"
  			bb->lvel[0] = 0;
  			bb->lvel[1] = 0;
  			bb->lvel[2] = 0;
  			bb->avel[0] = 0;
  			bb->avel[1] = 0;
  			bb->avel[2] = 0;
  		}
  	}
  }
  
  
  //****************************************************************************
  // body rotation
  
  // return sin(x)/x. this has a singularity at 0 so special handling is needed
  // for small arguments.
  
  static inline dReal sinc (dReal x)
  {
    // if |x| < 1e-4 then use a taylor series expansion. this two term expansion
    // is actually accurate to one LS bit within this range if double precision
    // is being used - so don't worry!
    if (dFabs(x) < 1.0e-4) return REAL(1.0) - x*x*REAL(0.166666666666666666667);
    else return dSin(x)/x;
  }
  
  
  // given a body b, apply its linear and angular rotation over the time
  // interval h, thereby adjusting its position and orientation.
  
  void dxStepBody (dxBody *b, dReal h)
  {
    // cap the angular velocity
    if (b->flags & dxBodyMaxAngularSpeed) {
      const dReal max_ang_speed = b->max_angular_speed;
      const dReal aspeed = dCalcVectorDot3( b->avel, b->avel );
      if (aspeed > max_ang_speed*max_ang_speed) {
        const dReal coef = max_ang_speed/dSqrt(aspeed);
        dScaleVector3(b->avel, coef);
      }
    }
    // end of angular velocity cap
  
  
    // handle linear velocity
    for (int j=0; j<3; j++) b->posr.pos[j] += h * b->lvel[j];
  
    if (b->flags & dxBodyFlagFiniteRotation) {
      dVector3 irv;	// infitesimal rotation vector
      dQuaternion q;	// quaternion for finite rotation
  
      if (b->flags & dxBodyFlagFiniteRotationAxis) {
        // split the angular velocity vector into a component along the finite
        // rotation axis, and a component orthogonal to it.
        dVector3 frv;		// finite rotation vector
        dReal k = dCalcVectorDot3 (b->finite_rot_axis,b->avel);
        frv[0] = b->finite_rot_axis[0] * k;
        frv[1] = b->finite_rot_axis[1] * k;
        frv[2] = b->finite_rot_axis[2] * k;
        irv[0] = b->avel[0] - frv[0];
        irv[1] = b->avel[1] - frv[1];
        irv[2] = b->avel[2] - frv[2];
  
        // make a rotation quaternion q that corresponds to frv * h.
        // compare this with the full-finite-rotation case below.
        h *= REAL(0.5);
        dReal theta = k * h;
        q[0] = dCos(theta);
        dReal s = sinc(theta) * h;
        q[1] = frv[0] * s;
        q[2] = frv[1] * s;
        q[3] = frv[2] * s;
      }
      else {
        // make a rotation quaternion q that corresponds to w * h
        dReal wlen = dSqrt (b->avel[0]*b->avel[0] + b->avel[1]*b->avel[1] +
          b->avel[2]*b->avel[2]);
        h *= REAL(0.5);
        dReal theta = wlen * h;
        q[0] = dCos(theta);
        dReal s = sinc(theta) * h;
        q[1] = b->avel[0] * s;
        q[2] = b->avel[1] * s;
        q[3] = b->avel[2] * s;
      }
  
      // do the finite rotation
      dQuaternion q2;
      dQMultiply0 (q2,q,b->q);
      for (int j=0; j<4; j++) b->q[j] = q2[j];
  
      // do the infitesimal rotation if required
      if (b->flags & dxBodyFlagFiniteRotationAxis) {
        dReal dq[4];
        dWtoDQ (irv,b->q,dq);
        for (int j=0; j<4; j++) b->q[j] += h * dq[j];
      }
    }
    else {
      // the normal way - do an infitesimal rotation
      dReal dq[4];
      dWtoDQ (b->avel,b->q,dq);
      for (int j=0; j<4; j++) b->q[j] += h * dq[j];
    }
  
    // normalize the quaternion and convert it to a rotation matrix
    dNormalize4 (b->q);
    dQtoR (b->q,b->posr.R);
  
    // notify all attached geoms that this body has moved
    for (dxGeom *geom = b->geom; geom; geom = dGeomGetBodyNext (geom))
      dGeomMoved (geom);
  
    // notify the user
    if (b->moved_callback)
      b->moved_callback(b);
  
  
    // damping
    if (b->flags & dxBodyLinearDamping) {
      const dReal lin_threshold = b->dampingp.linear_threshold;
      const dReal lin_speed = dCalcVectorDot3( b->lvel, b->lvel );
      if ( lin_speed > lin_threshold) {
        const dReal k = 1 - b->dampingp.linear_scale;
        dScaleVector3(b->lvel, k);
      }
    }
    if (b->flags & dxBodyAngularDamping) {
      const dReal ang_threshold = b->dampingp.angular_threshold;
      const dReal ang_speed = dCalcVectorDot3( b->avel, b->avel );
      if ( ang_speed > ang_threshold) {
        const dReal k = 1 - b->dampingp.angular_scale;
        dScaleVector3(b->avel, k);
      }
    }
  }
  
  
  //****************************************************************************
  // island processing
  
  // This estimates dynamic memory requirements for dxProcessIslands
  static size_t EstimateIslandsProcessingMemoryRequirements(dxWorld *world, size_t &sesize)
  {
    size_t res = 0;
  
    size_t islandcounts = dEFFICIENT_SIZE(world->nb * 2 * sizeof(int));
    res += islandcounts;
    size_t islandreqs = dEFFICIENT_SIZE(world->nb * sizeof(size_t)); // keep separate islandreqs for each island
    res += islandreqs;
  
    size_t bodiessize = dEFFICIENT_SIZE(world->nb * sizeof(dxBody*));
    size_t jointssize = dEFFICIENT_SIZE(world->nj * sizeof(dxJoint*));
    res += bodiessize + jointssize;
  
    sesize = (bodiessize < jointssize) ? bodiessize : jointssize;
    return res;
  }
  
  // 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
  
  # ifndef dNODEBUG
    // if debugging, check that all objects (except for disabled bodies,
    // unconnected joints, and joints that are connected to disabled bodies)
    // were tagged.
    {
      for (dxBody *b=world->firstbody; b; b=(dxBody*)b->next) {
        if (b->flags & dxBodyDisabled) {
          if (b->island_tag > 0) dDebug (0,"disabled body tagged");
        }
        else {
          if (b->island_tag <= 0) dDebug (0,"enabled body not tagged");
        }
      }
      for (dxJoint *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) )
          && 
          j->isEnabled() ) {
            if (j->island_tag <= 0) dDebug (0,"attached enabled joint not tagged");
        }
        else {
          if (j->island_tag > 0) dDebug (0,"unattached or disabled joint tagged");
        }
      }
    }
  # endif
  
    int islandcount = (sizescurr - islandsizes) / sizeelements;
    context->SavePreallocations(islandcount, islandsizes, body, joint, islandreqs);
  
    //printf("total island count: %d and summary below: \n",islandcount);
    //for (int j=0; j<islandcount; j++)
      //printf("island:%d bodycount:%d jointcount:%d islandreqs:%d \n",j,islandsizes[2*j],islandsizes[2*j+1],islandreqs[j]);
  
    return maxreq;
  }
  
  // this groups all joints and bodies in a world into islands. all objects
  // in an island are reachable by going through connected bodies and joints.
  // each island can be simulated separately.
  // note that joints that are not attached to anything will not be included
  // in any island, an so they do not affect the simulation.
  //
  // this function starts new island from unvisited bodies. however, it will
  // never start a new islands from a disabled body. thus islands of disabled
  // bodies will not be included in the simulation. disabled bodies are
  // re-enabled if they are found to be part of an active island.
  
  void dxProcessOneIsland(dxWorldProcessContext *island_context, dxWorld *world, dReal stepsize, dstepper_fn_t stepper,
                          dxBody *const* bodystart,
                          int bcount,
                          dxJoint *const *jointstart,
                          int jcount)
  {
  #ifdef REPORT_THREAD_TIMING
      struct timeval tv;
      double cur_time;
      gettimeofday(&tv,NULL);
      cur_time = (double)tv.tv_sec + (double)tv.tv_usec / 1.e6;
      printf("island thread started time %f\n",cur_time);
  #endif
  
      BEGIN_STATE_SAVE(island_context, island_stepperstate) {
        stepper (island_context,world,bodystart,bcount,jointstart,jcount,stepsize);
      } END_STATE_SAVE(island_context, island_stepperstate);
  
  #ifdef REPORT_THREAD_TIMING
      gettimeofday(&tv,NULL);
      double end_time = (double)tv.tv_sec + (double)tv.tv_usec / 1.e6;
      printf("----- island thread start time %f ended at time %f with duration %f\n",cur_time,end_time,end_time - cur_time);
  #endif
  }
  
  void dxProcessIslands (dxWorld *world, dReal stepsize, dstepper_fn_t stepper)
  {
    const int sizeelements = 2;
  
    dxStepWorkingMemory *wmem = world->wmem;
    dIASSERT(wmem != NULL);
  
    dxWorldProcessContext *context = wmem->GetWorldProcessingContext(); 
  
    int islandcount;
    size_t const *islandreqs;
    int const *islandsizes;
    dxBody *const *body;
    dxJoint *const *joint;
    context->RetrievePreallocations(islandcount, islandsizes, body, joint, islandreqs);
  
    dxBody *const *bodystart = body;
    dxJoint *const *jointstart = joint;
  
    IFTIMING(dTimerStart("preprocessing islands"));
    int island_index = 0;
    int const *const sizesend = islandsizes + islandcount * sizeelements;
  
  #ifdef REPORT_THREAD_TIMING
    struct timeval tv;
    double cur_time;
    gettimeofday(&tv,NULL);
    cur_time = (double)tv.tv_sec + (double)tv.tv_usec / 1.e6;
    printf(">>>>>>>>>>>> start island spawn threads at time %f\n",cur_time);
  #endif
  
    for (int const *sizescurr = islandsizes; sizescurr != sizesend; sizescurr += sizeelements) {
      int bcount = sizescurr[0];
      int jcount = sizescurr[1];
  
      // get working memory for each island
      dxStepWorkingMemory *island_wmem = world->island_wmems[island_index++];
      dIASSERT(island_wmem != NULL);
      dxWorldProcessContext *island_context = island_wmem->GetWorldProcessingContext();
  
  #define USE_TPISLAND
  #ifdef USE_TPISLAND
      IFTIMING(dTimerNow("scheduling island"));
      //printf("debug opende tp %d\n",world->threadpool->size());
      if (world->threadpool && world->threadpool->size() > 0)
        world->threadpool->schedule(boost::bind(dxProcessOneIsland,island_context, world, stepsize, stepper,bodystart, bcount, jointstart, jcount));
      else //automatically skip threadpool if only 1 thread allocated
        dxProcessOneIsland(island_context, world, stepsize, stepper,bodystart, bcount, jointstart, jcount);
  #else
      dxProcessOneIsland(island_context, world, stepsize, stepper,bodystart, bcount, jointstart, jcount);
  #endif
  
      bodystart += bcount;
      jointstart += jcount;
    }
  #ifdef USE_TPISLAND
    IFTIMING(dTimerNow("islands wait"));
    if (world->threadpool && world->threadpool->size() > 0)
      world->threadpool->wait();
  #endif
    IFTIMING(dTimerEnd());
    IFTIMING(dTimerReport (stdout,1));
  
  
  #ifdef REPORT_THREAD_TIMING
    gettimeofday(&tv,NULL);
    double end_time = (double)tv.tv_sec + (double)tv.tv_usec / 1.e6;
    printf("<<<<<<<<<<<< all island threads stopped at time %f with duration %f\n",end_time,end_time - cur_time);
  #endif
  
    for (int jj=0; jj < islandcount; jj++)
      world->island_wmems[jj]->GetWorldProcessingContext()->CleanupContext();
  
    context->CleanupContext();
    dIASSERT(context->IsStructureValid());
  }
  
  //****************************************************************************
  // World processing context management
  
  static size_t AdjustArenaSizeForReserveRequirements(size_t arenareq, float rsrvfactor, unsigned rsrvminimum)
  {
    float scaledarena = arenareq * rsrvfactor;
    size_t adjustedarena = (scaledarena < SIZE_MAX) ? (size_t)scaledarena : SIZE_MAX;
    size_t boundedarena = (adjustedarena > rsrvminimum) ? adjustedarena : (size_t)rsrvminimum;
    return dEFFICIENT_SIZE(boundedarena);
  }
  
  static dxWorldProcessContext *InternalReallocateWorldProcessContext (
    dxWorldProcessContext *oldcontext, size_t memreq, 
    const dxWorldProcessMemoryManager *memmgr, float rsrvfactor, unsigned rsrvminimum)
  {
    dxWorldProcessContext *context = oldcontext;
    bool allocsuccess = false;
  
    size_t oldarenasize; 
    void *pOldArena;
  
    do {
      size_t oldmemsize = oldcontext ? oldcontext->GetMemorySize() : 0;
      if (!oldcontext || oldmemsize < memreq) {
        oldarenasize = oldcontext ? dxWorldProcessContext::MakeArenaSize(oldmemsize) : 0;
        pOldArena = oldcontext ? oldcontext->m_pArenaBegin : NULL;
  
        if (!dxWorldProcessContext::IsArenaPossible(memreq)) {
          break;
        }
  
        size_t arenareq = dxWorldProcessContext::MakeArenaSize(memreq);
        size_t arenareq_with_reserve = AdjustArenaSizeForReserveRequirements(arenareq, rsrvfactor, rsrvminimum);
        size_t memreq_with_reserve = memreq + (arenareq_with_reserve - arenareq);
  
        if (oldcontext) {
  
          if (oldcontext->m_pAllocCurrent != oldcontext->m_pAllocBegin) {
  
            // Save old efficient offset and meaningful data size for the case if 
            // reallocation throws the block at different efficient offset
            size_t oldcontextofs = (size_t)oldcontext - (size_t)pOldArena;
            size_t datasize = (size_t)oldcontext->m_pAllocCurrent - (size_t)oldcontext;
            
            // Extra EFFICIENT_ALIGNMENT bytes might be needed after re-allocation with different alignment
            size_t shrunkarenasize = dEFFICIENT_SIZE(datasize + oldcontextofs) + EFFICIENT_ALIGNMENT;
            if (shrunkarenasize < oldarenasize) {
  
              void *pShrunkOldArena = oldcontext->m_pArenaMemMgr->m_fnShrink(pOldArena, oldarenasize, shrunkarenasize);
              if (!pShrunkOldArena) {
                break;
              }
  
              // In case if shrinking is not supported and memory manager had to allocate-copy-free
              if (pShrunkOldArena != pOldArena) {
                dxWorldProcessContext *shrunkcontext = (dxWorldProcessContext *)dEFFICIENT_PTR(pShrunkOldArena);
  
                // Preform data shift in case if efficient alignment of new block
                // does not match that of old block
                size_t shrunkcontextofs = (size_t)shrunkcontext - (size_t)pShrunkOldArena;
                size_t offsetdiff = oldcontextofs - shrunkcontextofs;
                if (offsetdiff != 0) {
                  memmove(shrunkcontext, (void *)((size_t)shrunkcontext + offsetdiff), datasize);
                }
  
                // Make sure allocation pointers are valid - that is necessary to
                // be able to calculate size and free old arena later
                size_t shrunkdatasize = dxWorldProcessContext::MakeBufferSize(shrunkarenasize);
                void *blockbegin = dEFFICIENT_PTR(shrunkcontext + 1);
                void *blockend = dOFFSET_EFFICIENTLY(blockbegin, shrunkdatasize);
                shrunkcontext->m_pAllocBegin = blockbegin;
                shrunkcontext->m_pAllocEnd = blockend;
                shrunkcontext->m_pAllocCurrent = blockend; // -- set to end to prevent possibility of further allocation
                shrunkcontext->m_pArenaBegin = pShrunkOldArena;
  
                size_t stOffset = ((size_t)pShrunkOldArena - (size_t)pOldArena) - offsetdiff;
                shrunkcontext->OffsetPreallocations(stOffset);
  
                oldcontext = shrunkcontext;
  
                // Reassign to old arena variables for potential freeing at exit
                pOldArena = pShrunkOldArena;
              }
  
              // Reassign to old arena variables for potential freeing at exit
              oldarenasize = shrunkarenasize;
            }
  
          } else {
            oldcontext->m_pArenaMemMgr->m_fnFree(pOldArena, oldarenasize);
            oldcontext = NULL;
            
            // Zero variables to avoid another freeing on exit
            pOldArena = NULL;
            oldarenasize = 0;
          }
        }
  
        // Allocate new arena
        void *pNewArena = memmgr->m_fnAlloc(arenareq_with_reserve);
        if (!pNewArena) {
          break;
        }
  
        context = (dxWorldProcessContext *)dEFFICIENT_PTR(pNewArena);
  
        void *blockbegin = dEFFICIENT_PTR(context + 1);
        void *blockend = dOFFSET_EFFICIENTLY(blockbegin, memreq_with_reserve);
  
        context->m_pAllocBegin = blockbegin;
        context->m_pAllocEnd = blockend;
        context->m_pArenaBegin = pNewArena;
        context->m_pAllocCurrent = blockbegin;
  
        if (oldcontext) {
          context->CopyPreallocations(oldcontext);
        } else {
          context->ClearPreallocations();
        }
  
        context->m_pArenaMemMgr = memmgr;
        context->m_pPreallocationcContext = oldcontext;
      }
  
      allocsuccess = true;
    } while (false);
  
    if (!allocsuccess) {
      if (pOldArena) {
        dIASSERT(oldcontext);
        oldcontext->m_pArenaMemMgr->m_fnFree(pOldArena, oldarenasize);
      }
      context = NULL;
    }
  
    return context;
  }
  
  static void InternalFreeWorldProcessContext (dxWorldProcessContext *context)
  {
    size_t memsize = context->GetMemorySize();
    size_t arenasize = dxWorldProcessContext::MakeArenaSize(memsize);
  
    void *pArenaBegin = context->m_pArenaBegin;
    context->m_pArenaMemMgr->m_fnFree(pArenaBegin, arenasize);
  }
  
  
  bool dxReallocateWorldProcessContext (dxWorld *world, 
    dReal stepsize, dmemestimate_fn_t stepperestimate)
  {
    dxStepWorkingMemory *wmem = AllocateOnDemand(world->wmem);  // this is starting a new instance of dxStepWorkingMemory
    if (!wmem) return false;
  
    dxWorldProcessContext *oldcontext = wmem->GetWorldProcessingContext();
    dIASSERT (!oldcontext || oldcontext->IsStructureValid());
  
    const dxWorldProcessMemoryReserveInfo *reserveinfo = wmem->SureGetMemoryReserveInfo();
    const dxWorldProcessMemoryManager *memmgr = wmem->SureGetMemoryManager();
  
    dxWorldProcessContext *context = oldcontext;
  
    // EstimateIslandsProcessingMemoryRequirements allocates memeory for 3 arrays:
    //    islandsizes: integer arrays, 2*n_islands in size, contains bodycount and jointcount for each island
    //    islandreqs: integer arrays, n_islands in size, contains memory requirements for each island
    //    body: one array with all the 'active' bodies, all indexed by islandsizes
    //    joint: one array with all the 'active' joints, all indexed by islandsizes
    size_t sesize;
    size_t islandsreq = EstimateIslandsProcessingMemoryRequirements(world, sesize);
    dIASSERT(islandsreq == dEFFICIENT_SIZE(islandsreq));
    dIASSERT(sesize == dEFFICIENT_SIZE(sesize));
  
    size_t stepperestimatereq = islandsreq + sesize;
    context = InternalReallocateWorldProcessContext(context, stepperestimatereq, memmgr, 1.0f, reserveinfo->m_uiReserveMinimum);
    
    //
    // above context allocation of the island arrays is successful, then we proceed to allocate more spaces for the actual stepping work
    //
    // we want to start multiple contexts, one for each island.
    //
    if (context)
    {
      /*size_t stepperreq =*/ BuildIslandsAndEstimateStepperMemoryRequirements(context, world, stepsize, stepperestimate);
      dIASSERT(stepperreq == dEFFICIENT_SIZE(stepperreq));
  
      // retrieve results of BuildIslandsAndEstimateStepperMemoryRequirements
      int islandcount;
      size_t const *islandreqs;
      int const *islandsizes;
      dxBody *const *body;
      dxJoint *const *joint;
      context->RetrievePreallocations(islandcount, islandsizes, body, joint, islandreqs);
  
      for (int jj = 0; jj < islandcount; jj++)
      {
        // for individual islands
        dxStepWorkingMemory *island_wmem = AllocateOnDemand(world->island_wmems[jj]);  // this is starting a new instance of dxStepWorkingMemory
        if (!island_wmem) return false;
  
        dxWorldProcessContext *island_oldcontext = island_wmem->GetWorldProcessingContext();
        dIASSERT (!island_oldcontext || island_oldcontext->IsStructureValid());
  
        const dxWorldProcessMemoryReserveInfo *island_reserveinfo = island_wmem->SureGetMemoryReserveInfo();
        const dxWorldProcessMemoryManager *island_memmgr = island_wmem->SureGetMemoryManager();
  
        dxWorldProcessContext *island_context = island_oldcontext;
  
        // this is where islandreqs is used, to MakeArenaSize
        island_context = InternalReallocateWorldProcessContext(island_context, islandreqs[jj], island_memmgr, island_reserveinfo->m_fReserveFactor, island_reserveinfo->m_uiReserveMinimum);
        island_wmem->SetWorldProcessingContext(island_context); // set dxStepWorkingMemory to context
      }
    }
  
    wmem->SetWorldProcessingContext(context); // set dxStepWorkingMemory to context
    return context != NULL;
  }
  
  dxWorldProcessContext *dxReallocateTemporayWorldProcessContext(dxWorldProcessContext *oldcontext, 
    size_t memreq, const dxWorldProcessMemoryManager *memmgr/*=NULL*/, const dxWorldProcessMemoryReserveInfo *reserveinfo/*=NULL*/)
  {
    dxWorldProcessContext *context = oldcontext;
  
    if (context && context->GetMemorySize() < memreq) {
      dIASSERT(!context->IsPreallocationsContextAssigned());
  
      InternalFreeWorldProcessContext(context);
      context = NULL;
    }
  
    if (context == NULL) {
      const dxWorldProcessMemoryManager *surememmgr = memmgr ? memmgr : &g_WorldProcessMallocMemoryManager;
      const dxWorldProcessMemoryReserveInfo *surereserveinfo = reserveinfo ? reserveinfo : &g_WorldProcessDefaultReserveInfo;
      context = InternalReallocateWorldProcessContext(context, memreq, surememmgr, surereserveinfo->m_fReserveFactor, surereserveinfo->m_uiReserveMinimum);
    }
  
    return context;
  }
  
  void dxFreeWorldProcessContext (dxWorldProcessContext *context)
  {
    // Free old arena for the case if context is freed after reallocation without
    // a call to world stepping function
    context->FreePreallocationsContext();
  
    // Assert validity after old arena is freed as validation includes checking for
    // old arena to be absent
    dUASSERT (context->IsStructureValid(), "invalid context structure");
  
    InternalFreeWorldProcessContext(context);
  }
  
  
  
  