[ODE] Re: center of mass

[Nagymathe Denes]

Hi,

I didn't have the time to find the bug in your code, but a working version 
to place the given geom into the desired pos and R looks like this:
(It's directly inherited from dxGeomTransform::computeFinalTx())

//test:
if (obj[i].geom[1])
if (dGeomGetClass(obj[i].geom[1]) == dGeomTransformClass)
{
    dVector3 final_pos = {0,0,1}, body_pos;
    dMatrix3 final_R, body_R;
    dRFromAxisAndAngle(final_R,0,0,1,0.5);
    dGeomID g = dGeomTransformGetGeom(obj[i].geom[1]);
    const dReal *g_pos = dGeomGetPosition (g);
    const dReal *g_R = dGeomGetRotation (g);
    dMULTIPLY2_333 (body_R,final_R,g_R);
    dMULTIPLY0_331 (body_pos,body_R,g_pos);
    body_pos[0] = final_pos[0] - body_pos[0];
    body_pos[1] = final_pos[1] - body_pos[1];
    body_pos[2] = final_pos[2] - body_pos[2];
    dBodySetPosition(obj[i].body, body_pos[0], body_pos[1], body_pos[2]);
    dBodySetRotation(obj[i].body, body_R);
}

I used it in the test_boxstack example and it works fine. I was thinking 
about making it a support function of geomtransform, but seemed to be a bad 
idea; it would need too many params and/or direct access to its body. :o(

Denes