[ODE] patch: auto-disable implemented

[Aras Pranckevicius]

Hi,

Auto-disabling of inactive bodies was supposed to be in StepFast - the docs
and function prototypes are present, but the actual implementation is not.
Some time ago I've implemented auto-disabling in ODE 0.35, now finally I've
got some time and did that on current ODE from CVS.

I guess the 'proper' thing would be to conditionally enable/disable this
feature at compilation time (similar to trimesh) - as it adds several
members to each body, and has some (very small) overhead. For my usual
stuff, auto-disabling is great, don't know about the others :)

Here, what should be done:

-----------------------------------------------------
objects.h (the one internal to ode sources):
    approx. at line 40, add to body flags enum:
        dxBodyAutoDisable = 16 // enable auto-disable on body
    approx. at line 80, add to struct dxBody:
        // auto-disabling
        dReal autodis_threshold;
        int autodis_steps;
        int autodis_counter;

-----------------------------------------------------
ode.cpp
    in processIslands, add to the start of initial
    body visiting loop ["for (bb=world->firstbody; bb;
bb=(dxBody*)bb->next)"]:
     if( (bb->flags&dxBodyAutoDisable) && !(bb->flags&dxBodyDisabled) ) {
      bool disable = true;
      const dReal *lvel = bb->lvel;;
      dReal lspeed = dDOT(lvel,lvel);
      if( lspeed > bb->autodis_threshold ) {
       disable = false;
      } else {
       const dReal *avel = bb->avel;
       dReal aspeed = dDOT(avel,avel);
       if( aspeed > bb->autodis_threshold ) {
        disable = false;
       }
      }
      if( disable )
       ++bb->autodis_counter;
      else
       bb->autodis_counter = 0;

      if( bb->autodis_counter > bb->autodis_steps ) {
       bb->flags |= dxBodyDisabled;
       continue;
      }
     }

    in the same processIslands, in "traverse and tag all body's joints,
add...",
    add to inside of "if (n->body && !n->body->tag) {":
      n->body->flags &= ~dxBodyDisabled;
      n->body->autodis_counter = 0;

    in dBodyCreate(), add initialization:
      b->autodis_threshold = REAL(0.003);
      b->autodis_steps = 10;
      b->autodis_counter = 0;

    add implementation of auto-disable functions:
        void dBodySetAutoDisableThresholdSF1( dBodyID b, dReal threshold )
        {
         dAASSERT(b);
         b->autodis_threshold = threshold;
        }
        dReal dBodyGetAutoDisableThresholdSF1( dBodyID b )
        {
         dAASSERT(b);
         return b->autodis_threshold;
        }
        void dBodySetAutoDisableStepsSF1( dBodyID b, int steps )
        {
         dAASSERT(b);
         b->autodis_steps = steps;
        }
        int dBodyGetAutoDisableStepsSF1( dBodyID b )
        {
         dAASSERT(b);
         return b->autodis_steps;
        }
        void dBodySetAutoDisableSF1( dBodyID b, int doAutoDisable )
        {
         dAASSERT(b);
         if( !doAutoDisable ) b->flags &= ~dxBodyAutoDisable;
         else b->flags |= dxBodyAutoDisable;
        }
        int dBodyGetAutoDisableSF1( dBodyID b )
        {
         dAASSERT(b);
         return ((b->flags & dxBodyAutoDisable) != 0);
        }


-----------------------------------------------------
stepfast.cpp: similar to ode.cpp
    in processIslandsFast, add to the start of initial body
    visiting loop, just after
    [#ifdef TIMING dTimerNow ("Island Processing); #endif]:
      if( (bb->flags&dxBodyAutoDisable) && !(bb->flags&dxBodyDisabled) ) {
       bool disable = true;
       const dReal *lvel = bb->lvel;;
       dReal lspeed = dDOT(lvel,lvel);
       if( lspeed > bb->autodis_threshold ) {
        disable = false;
       } else {
        const dReal *avel = bb->avel;
        dReal aspeed = dDOT(avel,avel);
        if( aspeed > bb->autodis_threshold ) {
         disable = false;
        }
       }
       if( disable )
        ++bb->autodis_counter;
       else
        bb->autodis_counter = 0;

       if( bb->autodis_counter > bb->autodis_steps ) {
        bb->flags |= dxBodyDisabled;
        continue;
       }
      }

    in the same processIslandsFast, in "traverse and tag all body's joints,
    add untagged...", after "n->body->flags &= ~dxBodyDisabled;" add:
        n->body->autodis_counter = 0;


That should be it. Works for me in classic and iterative solvers like a
charm. The code is rather trivial, so it shouldn't contain severe bugs :)


Aras Pranckevicius aka NeARAZ
http://www.gim.ktu.lt/nesnausk/nearaz/