[ODE] RE: Angular acceleration from torque

[Ruud van Gaal]

Hi,

Following a couple of messages lately I've formed this function to calculate
angular acceleration from input torque. There's something wrong with it, but
who can tell what?

void dBodyAngAccelFromTorque(const dBodyID body,dReal *angAccel,
  const dReal *torque)
// Calculate angular acceleration from the torque
{
  dMass m;
  dMatrix3 invI,invI1,local_invI;
  const dReal *R;

  // Get inverted inertia matrix
  dBodyGetMass(body,&m);
  dInvertPDMatrix(m.I,invI,3);

  // Convert global into local coordinates
  R=dBodyGetRotation(body);
  dMULTIPLY2_333(invI1,invI,R);
  dMULTIPLY1_333(local_invI,R,invI1);

  // Not right!
  //dMULTIPLY1_333(angAccel,local_invI,torque);
}

Problem is I don't know what all the dMULTIPLY's do exactly; somebody
suggest the upper half, then stated that the invI matrix is still in global
coords, to be converted into local ones. Not sure how to put the pieces
together.

Thanks for any help,
Ruud van Gaal