[ODE] Transforming functions

[Martin C. Martin]

Hi all,

I whipped up those transform/untransform functions I was talking about. 
They convert a description of a point from a coordinate system attached
to a body, to the world coordinate system, and vice versa.  They can
either to just the rotation, or the full on
rotation-plus-transformation.

Russ, can you check that I did them right?  In particular, it's
dMULTIPLY1_331 for the inverse, right?

A similar set could be made for geoms.

Thanks,
Martin


objects.h:

// Object coords to world coords
void dBodyTransformVector(dBodyID, dReal, dReal, dReal, dVector3);
void dBodyRotateVector(dBodyID, dReal, dReal, dReal, dVector3);
// World to object
void dBodyInverseTransformVector(dBodyID, dReal, dReal, dReal,
dVector3);
void dBodyInverseRotateVector(dBodyID, dReal, dReal, dReal, dVector3);


ode.cpp:

void dBodyTransformVector (dBodyID b, dReal px, dReal py, dReal pz,
			  dVector3 result)
{
  dAASSERT (b);
  dVector3 prel,p;
  prel[0] = px;
  prel[1] = py;
  prel[2] = pz;
  prel[3] = 0;
  dMULTIPLY0_331 (p,b->R,prel);
  result[0] = p[0] + b->pos[0];
  result[1] = p[1] + b->pos[1];
  result[2] = p[2] + b->pos[2];
}

void dBodyRotateVector (dBodyID b, dReal px, dReal py, dReal pz,
			  dVector3 result)
{
  dAASSERT (b);
  dVector3 prel;
  prel[0] = px;
  prel[1] = py;
  prel[2] = pz;
  prel[3] = 0;
  dMULTIPLY0_331 (result,b->R,prel);
}

void dBodyInverseTransformVector (dBodyID b, dReal px, dReal py, dReal
pz,
			  dVector3 result)
{
  dAASSERT (b);
  dVector3 prel;
  prel[0] = px - b->pos[0];
  prel[1] = py - b->pos[1];
  prel[2] = pz - b->pos[2];
  prel[3] = 0;
  dMULTIPLY1_331 (result, b->R, prel);
}

void dBodyInverseRotateVector (dBodyID b, dReal px, dReal py, dReal pz,
			  dVector3 result)
{
  dAASSERT (b);
  dVector3 prel;
  prel[0] = px;
  prel[1] = py;
  prel[2] = pz;
  prel[3] = 0;
  dMULTIPLY1_331 (result, b->R, prel);
}