[ODE] Composite Bodies Rendering

[Wesley Smith]

Hi,
I have a very simple test case for composite bodies to see if I can
get it to behave properly with just one body and move on from there. 
I'm doing something wrong with the GeomTransform I think, but I can't
quite nail it down.  On the wiki, it says the example is out of date. 
Anyone know exactly what's out of date?  I've tried to follow it as
much as possible although my case is a bit different as I'm taking
existing bodies and compositing them.

Here are my steps:

In this code, I have several vectors for holding GeomTransform
GeomIDs, GeomIDs, BodyIDs, and Masses.  The names are straightforward.
 What I'm seeing is the composite body fall through the floor and then
hang and sway as if attached to the floor by some kind of pendulum
mechanism.  I can't explain it at all.  This is how I attach a body:

const dReal *geomPos;
	const dReal *geomRot;
	dMass geomBodyMass;
	
	//get position and rotation of geom
	geomPos = dGeomGetPosition(geom);
	geomRot = dGeomGetRotation(geom);

	//add BodyID, GeomID, and dMass to appropriate vectors
	m_Geoms.push_back(geom);
	m_Bodies.push_back( dGeomGetBody(geom) );
	
	dBodyGetMass( *(m_Bodies.end() - 1), &geomBodyMass );
	m_Masses.push_back(geomBodyMass);
	
	//create a GeomTransform for making the composite Geom
	m_GeomTransforms.push_back( dCreateGeomTransform(m_Space) );
	dGeomSetBody( *(m_GeomTransforms.end() - 1), m_CompositeBody);
	
	//unhook incoming Geom from its body and disable the body
	dBodyDisable( *(m_Bodies.end() - 1) );
	dGeomSetBody( *(m_Geoms.end() - 1), 0 );
	
	if( dGeomGetSpace(*(m_Geoms.end() - 1)) != 0 )
		dSpaceRemove( dGeomGetSpace(*(m_Geoms.end() - 1)) , *(m_Geoms.end() - 1) );
	
	//put Geom into its GeomTransform
	dGeomTransformSetGeom( *(m_GeomTransforms.end() - 1), *(m_Geoms.end() - 1) );
	
	//if this is the first body added, it will be placed
	//at the center of the composite body
	dGeomSetPosition( *(m_Geoms.end() - 1), geomPos[0], geomPos[1], geomPos[2]);
	
	if(m_Geoms.size() <= 1) {
		//mass should already be rotated, so only rotate the GeomTransform
		dGeomSetRotation( *(m_Geoms.end() - 1), geomRot);
	}
	else {
		dMassTranslate( &(m_Masses.at(m_Masses.size()-1)),
						geomPos[0] - m_CompositeMass.c[0],
						geomPos[1] - m_CompositeMass.c[1],
						geomPos[2] - m_CompositeMass.c[2]);
		
		//mass should already be rotated, so only rotate the GeomTransform
		dGeomSetRotation( *(m_GeomTransforms.end() - 1), geomRot);
		dMassRotate( &(m_Masses.at(m_Masses.size()-1)), geomRot );
	}
	
	dMassAdd(&m_CompositeMass, &(m_Masses.at(m_Masses.size()-1)) );
	
	//shift everything so that the center of mass is (0, 0, 0)
	for(int i=0; i < m_Geoms.size(); i++) {
		geomPos = dGeomGetPosition(m_Geoms.at(i));
		dGeomSetPosition(m_Geoms.at(i),
						geomPos[0] - m_CompositeMass.c[0],
						geomPos[1] - m_CompositeMass.c[1],
						geomPos[2] - m_CompositeMass.c[2]);
	}
	
	//shift mass to center on (0, 0, 0)
	dMassTranslate (&m_CompositeMass,
					-m_CompositeMass.c[0],
					-m_CompositeMass.c[1],
					-m_CompositeMass.c[2]);




Comments are very much appreciated.

thanks,
wes