[ODE] test_boxstack and geom offset

[Geoff Carlton]

I've had a look at making test_boxstack.cpp use geom offsets, and I 
found a few apparent bugs in the geomTransform part of the test code.  I 
wanted to make sure my math is right, so I'm going to delve into the 
code a little.  My modified version is up at:

  http://www.hotboxgames.com/downloads/test_boxstack.cpp

If I am correct in the reasoning below, we can then take the next step 
of updating the test in SVN.  Now I added a bit of visualisation code to 
draw the body's position (thus its centre of mass), and added an extra 
"create" branch to make a composite offset-geom object, which can be 
seen in the code branch (cmd == 'x' && USE_GEOM_OFFSET).

What I found:
1.) The old creation code has a "<2" loop that looks like it should be a 
"<GPB" loop.  I don't think it is deliberate that the last geom doesn't 
get a dGeomSetPosition call.  I changed the old code to use GPB, since 
it seems an obvious bug.
2.) Setting up geom offsets is a bit awkward because offsets can only be 
set after the body is attached, and current code attaches the body at 
the end.  So I had to add a "setBody" flag since my code sets the body 
early on.
3.) The existing code gets it wrong!  Most obvious when GPB is set to 1, 
but clearly noticible normally.  The geom is offset with dpos[k] and 
Rtx, and so is the mass.  But the dMassRotate rotates dpos[k] by Rtx, so 
the correct geom position is really (dpos[k]*Rtx).  More simply, it can 
be fixed by moving the dMassRotate block above the dMassTranslate 
block.  This means that the mass's offset is really dpos[k].  I left 
this code as is, until we agree that it is in fact wrong.
4.) My new code calls dMassRotate above dMassTranslate, so it gets it right.
5.) To aid testing, I pushed apart the geoms with a *10 offset distance 
in both old and new code.

When the centre of gravity is wrong the objects have a tendancy to flop 
around, so I think this may be the mysterious force bug that was 
reported.  Assuming the above is correct we can easily fix the 
geomTransform and have both code paths co-existing in the test. 

The situation of attaching the body at the end of the function is 
ungainly for the geom offset code, but I don't think this is a 
significant practice elsewhere nor a major problem.  It raises the 
question of whether to allow offsets to be specified independant of a 
body, but in a strange form where they are not used until a body is 
attached.  This behaviour would be strange enough that I don't feel its 
a good approach, and technical reasons prevent us from allowing free 
geoms to have properly working offsets (and offset from what, anyway?).

Thanks,
Geoff