[ODE] Angular-velocity autodisabling considered broken

[John Miles]

There's at least one serious drawback to the way that angular-velocity
thresholds are implemented in ODE.  The problem I'm running into arises from
the fact that the autodisable code in util.cpp uses the object's
instantaneous angular velocity, rather than a time-averaged value, for its
threshold comparison.

In ODE, the angular velocity for objects that should be -- but aren't -- at
rest on a surface is very spiky.  When an object lands on a surface and
"jitters", that jitter is, in my experience, characterized by a long series
of very small (0.01 or less) angular velocity readings at successive
timesteps, punctuated by occasional lurches in the 1.0-5.0 range that
correspond to the actual jitter effect that the user sees.

These lurches last only a single timestep, but because a single outlying
value is enough to prevent the auto-disable check in
dInternalHandleAutoDisabling() from putting an object (and therefore, an
entire island) to sleep, I've had to set the angular auto-disable threshold
as much as 100x higher than what's appropriate given the average velocity
magnitudes I'm working with.

For example, if I raise my angular-velocity disable threshold to eliminate
visible jitter on resting objects, objects that tip over slowly are
prematurely autodisabled in mid-tip.  Back the threshold down to let objects
tip over completely, and jittery islands never settle down.

One of the biggest reasons that other engines like Novodex are ahead of ODE
in the performance department is that they are smarter about autodisabling
islands.  So this issue deserves some serious attention.  I've modified my
ODE build to use a moving average of bb->avel over the last 5 timesteps for
autodisable testing, and that helps a lot.  With the moving average in
place, it's possible to find a compromise threshold value that suppresses
both tipping problems and jitter.

I'm not saying that a moving average is the Right Way to address the
problem, but *some* form of spike-filtering needs to be added to the
angular-velocity check, or its usefulness is severely limited.

-- jm


-------------------------
Modified dInternalHandleAutoDisabling() from util.cpp appears below.  To use
it, you need to #define N_AVEL 5 (or whatever) in objects.h; add a field
"dVector3 avel_int[N_AVEL]" to the dxBody struct; and init the array to 0 in
dBodyCreate().  Again, this was just a quick hack for testing, but it's
definitely a step in the right direction.

void dInternalHandleAutoDisabling (dxWorld *world, dReal stepsize)
{
	dxBody *bb;
	for (bb=world->firstbody; bb; bb=(dxBody*)bb->next) {

      for (int i=N_AVEL-2; i >= 0; i--)
         {
         bb->avel_int[i+1][0] = bb->avel_int[i][0];
         bb->avel_int[i+1][1] = bb->avel_int[i][1];
         bb->avel_int[i+1][2] = bb->avel_int[i][2];
         }

      bb->avel_int[0][0] = bb->avel[0];
      bb->avel_int[0][1] = bb->avel[1];
      bb->avel_int[0][2] = bb->avel[2];

		// nothing to do unless this body is currently enabled and has
		// the auto-disable flag set
		if ((bb->flags & (dxBodyAutoDisable|dxBodyDisabled)) != dxBodyAutoDisable)
continue;

		// see if the body is idle
		int idle = 1;			// initial assumption
		dReal lspeed2 = dDOT(bb->lvel,bb->lvel);
		if (lspeed2 > bb->adis.linear_threshold) {
			idle = 0;		// moving fast - not idle
		}
		else {
         dVector3 avg;
         dSetZero(avg,4);

         for (int i=0; i < N_AVEL; i++)
            {
            avg[0] = avg[0] + bb->avel_int[i][0];
            avg[1] = avg[1] + bb->avel_int[i][1];
            avg[2] = avg[2] + bb->avel_int[i][2];
            }

         avg[0] = avg[0] / N_AVEL;
         avg[1] = avg[1] / N_AVEL;
         avg[2] = avg[2] / N_AVEL;

			dReal aspeed = dDOT(avg,avg);
			if (aspeed > bb->adis.angular_threshold) {
				idle = 0;	// turning fast - not 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 {
			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;
		}
	}
}