[ODE] [Fwd: Transforming functions]

[Martin C. Martin]

Here are some functions I submitted a while ago, that are less
cryptically named that the matrix multiply functions.

The Transform/Rotate functions convert from body coords to world coords;
the Inverse ones go the opposite direction.  (Maybe they should be
renamed to reflect that?  Hmm....)

To convert velocities (including angular velocities), you want the
rotate functions, not the transform.

Enjoy,
Martin

-------- Original Message --------
From: "Martin C. Martin" <martin@metahuman.org>
Subject: Transforming functions
To: ode@q12.org

Hi all,

I sent this 6 hours ago but I haven't seen it come back, so I thought
I'd send it again.  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);
}