[ODE] Mapping to OpenGL axis system

[Ruud van Gaal]

> no, ODE uses a right handed coord system, in both the dynamics and
> collision. the choice of the 'up' axis is arbitrary, but the
> handedness of the coordinate system is an important distinction.

Ok, I reverted all back to no negations, direct application. For positions, 
this is ok (all is righthanded). For torque, it isn't. I started digging hard 
and here are my results: (view in Courier)
I tested with a car in 2 simple orientations, 1 in the identity matrix 
(dBodyGetRotation()=I), and 1 with the car rotated 90 degrees counterclockwise 
(to the left) in yaw. Then, I applied just 1 torque, a relative torque around X 
(which gives pitch rotation) for both orientations (using 
dBodyAddRelTorque(body,1000,0,0). You'd expect (in OpenGL) the pitch to go 
down.
Orientation 1 (I): after some integrations, the rotation matrix (ODE's) changes 
from:
1 0 0  =>  1    0     0
0 1 0      0    0.91  -0.4
0 0 1      0    0.41  0.9

Especially take a look at the Z vector (0,-0.4,0.9). The Y is down; the car 
pitches downward. Now for orientation 2 (rotated 90 degrees in yaw):
0  0 1  =>  0 -0.27  0.96
0  1 0      0  0.96  0.27
-1 0 0      -1 0     0

Here, although the same dBodyAddRelTorque(id,1000,0,0) was given, the car's Z 
vector indicates the car is now pointing UPward (Y=0.27 which is >0).

I didn't think this could be right, no matter the orientation of the axes. So I 
viewed the source code, and dBodyAddRel*() does a multiplication with the 
body's R (rotation) member. But as I see it, this would convert World 
coordinates into Body coordinates, right? Instead, it seems to be used to 
calculate World coordinates *from* Body coordinates (which isn't right if I 
understand correctly). As I see it, ODE stores all forces/torque in world 
coordinates, probably because that makes a global contact resolve possible 
(which needs all coordinates in the same reference frame). Wouldn't this 
dBodyAddRel*() function need the inverse of R?
Not getting all this working for arbitrary orientations (and geez I tried), I 
thus modified dBodyAddRelTorque() to multiple with the INVERTED R matrix 
instead (where inverted is just transposed since we're dealing with rotations 
only).

Here's my revised function:
void dBobyAddRelTorque(dBodyID b,dReal Fx,dReal Fy,dReal Fz)
{
  dAASSERT(b);
  dReal Rt[12];
  Rt[0]=b->R[0];
  Rt[1]=b->R[4];
  Rt[2]=b->R[8];
  Rt[3]=0;
  Rt[4]=b->R[1];
  Rt[5]=b->R[5];
  Rt[6]=b->R[9];
  Rt[7]=0;
  Rt[8]=b->R[2];
  Rt[9]=b->R[6];
  Rt[10]=b->R[10];
  Rt[11]=1;

  dVector3 t1,t2;
  t1[0]=fx;
  t1[1]=fy;
  t1[2]=fz;
  dMULTIPLY1_331(t2,Rt,t1);
  b->tacc[0]+=t2[0];
  b->tacc[1]+=t2[1];
  b->tacc[2]+=t2[2];
}

Now, both orientations mentioned above gave the right result; when applying 
 dBodyAddRelTorque(id,1000,0,0) for a body-relative torque around X, the pitch 
went downward. I also tried an entirely different (not axis-aligned) 
orientation which still gives me correct behavior.
Now I might be totally misinterpreting 'relative' in the dBodyAdd*() functions, 
so please correct me how I *should* think about it then. I've got it working 
for torque now though, and I'm ofcourse now convinced that the 
transposed/inverted matrix should be used. I should modify dBodyAddRelForce() 
as well ofcourse, but I've gotta sleep because I have to be up early tomorrow. 
:) Just thought I'd run it by you.

Note that ofcourse calculating the transpose of R like that is highly 
inefficient and should be done in some revised form with another 
dMULTIPLY_Transposed() whatever. Couldn't dig through odemath.h hard enough to 
see if dMULTIPLY1_331() isn't already doing a transpose (the code was quite 
thick/recursively defined to trace with the naked eye).

Also note that you may not notice this at all if you're supplying ODE with all 
world coordinates, so that's why everybody else's attempts probably work but 
mine. ;-)

> i don't think you'll have to do any mapping - but remember to
> set the world gravity vector appropriately for your 'down' direction.

I didn't use gravity in the above examples btw. They would be done separately 
from ODE's built in gravity anyway (for the time being, until I get this 
resolved).

Sorry to go on about this in such length, but I found it weird that I could 
solve it this way. Still don't understand why it works for forces though 
(dBodyAddRelForce()).

Cheers,
Ruud