[ODE] Drifting of Geom Transforms

[Abheek Bose]

Greetings All:

I am having some funny problems with Geoms and Transforms which i
cannot really solve...
I am making a composite body which contains 7 elements. What i
initially do is the following:

1. I calculate the CG (Center of Gravity) of the main body by 1st Principles
2. I set the Body (roverBody) at this point and set the geoms
accordingly by shifting these geoms by the CG vector.
3. All the mass adjust and translation is done as given in the
test_box.cpp example.

NOTE: I DO NOT Rotate the masses (i have strange -ve inertia
complaints during runtime)... also since i calculate the CG i do NOT
shift the masses back so that the CG is (0,0,0) as given in the
example
 
now the problem is when i draw the Centers of all these composite
geoms, they seem to shift when the body has movements in the Z
axis.... an image of this can be found in:

http://home.inf.fh-brs.de/~abose2s/ODE_Problem_arrows.JPG

the red arrows connect the ODE calculated positions and the drawn geoms....

the code which calculates these geom positions is also given below:

Please let me know what i am doing wrong.... thanx in advance

cheers:

Abheek

Code Sniplet_
void Rover::updateCG()
{
	gotoxy(1,1);
	const dReal *BPos = dBodyGetPosition(roverBody); // Position of Main Mass
	printf("\nBodyPosition: (%f,%f,%f)", BPos[0], BPos[1], BPos[2]);

	for(int i=0; i<NUMBER_OF_ROVER_ELEMENTS; i++)
	{
		const dReal *tPos = dGeomGetPosition(roverGeom[i]);
		const dReal *ePos = dGeomGetPosition(encapseGeom[i]);
		
		dVector3 enPos;
		enPos[0] = tPos[0] + ePos[0];
		enPos[1] = tPos[1] + ePos[1];
		enPos[2] = tPos[2] + ePos[2];
		drawCG(enPos, i/10.0f, i/20.0f, i/30.0f);

		printf("\nEncapse %d Position: (%f,%f,%f)", i, ePos[0], ePos[1], ePos[2]);
		printf("\nTransfm %d Position: (%f,%f,%f)", i, tPos[0], tPos[1], tPos[2]);

	}

	dVector3 cgPos;
	cgPos[0] = BPos[0];
	cgPos[1] = BPos[1];
	cgPos[2] = BPos[2];

	drawCG(cgPos, 0.2f, 0.2f, 0.9f);
}

void Rover::drawCG(dVector3 pos, double r, double b, double g)
{
	dsSetColor(r,b,g);
	dMatrix3 R;
	dRFromAxisAndAngle(R,1,0,0,M_PI/2);
	dsDrawCylinder(pos, R, 0.7f, 0.01f);
}