[ODE] Speeding up a chain simulation

[Shree Kumar]

Hi,

I am new to ODE & physics simulation. 

I was trying to simulate a chain with ODE (using release 0.03).
Since there is no torus primitive, I modelled a link
in the chain using 4 boxes as below:

           +----------+
          /          /|
         +----------+ | 
	 |          | |
         | +------+ | |
         | | |    | | |
         | | |    | | |
height   | | |    | | |
         | | +----| | |
         | |/     | | |
         | +------+ | |
         |          +/
         +----------+
             length  <-> thichness

Excerpts from the code I used to create the chains are included
at the end of the message - please refer to it if the 
diagram is found wanting [I am sure it is!]

Any number of these links can be positioned one below the other,
rotated by 90 degrees successively, resulting in a chain. 
There is no need to use any joints. Only contact joints 
created for collision resolution are enough.

I had 4 chains, each with 25 links.  One end of each of the 4 
chains was fixed and the other end was attached to a box. The
final effect conceived was a box supported by 4 chains.

After fixing a few problems due to the step-size, I was able
to get a good simulation.  There was only one hitch - it was
terribly slow! [The simulation itself took more than a few
secs on my P4] 

On close examination, I found that at each step, approximately
240 contact joints were being generated and this was the chief
cause of the slowdown.

Can anyone give me some hints on what I can do to improve 
the simulation speed?

Please note:
1. I am already using single precision
2. I need 4 chains with 25 links & the box [can't relax the
requirement]

BTW, I tried to add a torus class. I have implemented torus-
sphere collisions but am yet to implement torus-torus
collisions

Thanks in advance for any info.

- Shree Kumar

Code follows:
---------------------------------------------
ChainLink::ChainLink(float length, float height, float thickness, 
                     dSpaceID space, dWorldID world)
{
	// create the 4 boxes & the rigid body for them
	int i;
	dGeomID b[4];

	// save the world & space info
	m_world = world;
	m_space = space;
	m_jointid = 0;
	m_length = length;
	m_height = height;
	m_thickness = thickness;

	dMass m, m2;
	dMassSetZero(&m);
	m_body = dBodyCreate(world);
	dReal dimensions[4][3] = {
		// top segment
		{ length, thickness, thickness },
		// bottom segment
		{ length, thickness, thickness },
		// left segment
		{ thickness, height - 2* thickness, thickness },
		// right segment
		{ thickness, height - 2* thickness, thickness }
	};
	dReal position[4][3] = {
		// top seg
		{ 0, height/2 -thickness/2, 0 },
		// bottom seg
		{ 0, -height/2 + thickness/2, 0 },
		// left seg
		{-length/2+thickness/2,0,0},
		// right seg
		{length/2-thickness/2,0,0}
	};
	// for each part of the link
	for(i=0;i<4;i++)
	{
		// Create a geom transform
		m_box[i]=dCreateGeomTransform(space);
		// and add a box to it
		b[i]=dCreateBox(0,dimensions[i][0],dimensions[i][1],dimensions[i][2]);
		// set the mass properties of the box
		dMassSetBox(&m2,1,dimensions[i][0],dimensions[i][1],dimensions[i][2]);
		dGeomTransformSetGeom(m_box[i], b[i]);
		dGeomSetPosition(b[i],position[i][0],position[i][1],position[i][2]);
		dMassTranslate(&m2,position[i][0],position[i][1],position[i][2]);
		dMassAdd(&m,&m2);
	}

	// move all objects so that center of mass is (0,0,0)	
	for(i=0;i<4;i++)
	{
	
dGeomSetPosition(b[i],position[i][0]-m.c[0],position[i][1]-m.c[1],position[i][2]-m.c[2]);
	}
	dMassTranslate(&m,-m.c[0],-m.c[1],-m.c[2]);
	for(i=0;i<4;i++)
	{
		dGeomSetBody(m_box[i],m_body);
	}
	// Set the final mass of the entire link
	dBodySetMass(m_body,&m);
}

Chain::Chain(float sx, float sy, float sz,  // start position of first
link
             float length, float height, float thickness, // dimensions
             int nLinks, 
             dSpaceID space, dWorldID world,
             float st_angle, float end_angle) // rotation angles 
                                              // for the first link &
last link
{
	int i;
	m_nLinks = nLinks;

	// convert degrees to radians
	st_angle = st_angle*M_PI/180.0;
	end_angle = end_angle*M_PI/180.0;

	float cur_angle = st_angle;
	float incr_angle = (end_angle-st_angle)/((float)nLinks-1);
	float link_angle = 0.0;

	// create links & position them
	for(i=0;i<nLinks;i++)
	{
		ChainLink * link = new ChainLink(length, height, thickness, space,
world);
		// rotate even links by 90 degrees
		if ((i%2)==1)
		{
			link_angle = 0.5*M_PI;
		}
		else
		{
			link_angle = 0.0;
		}

		// set the rotation of the chain link
		dQuaternion q;
		dQFromAxisAndAngle(q,0,1,0,cur_angle+link_angle);
		dBodySetQuaternion(link->GetBodyID(),q);

		// update rotation for next link
		cur_angle = cur_angle + incr_angle;	

		dBodySetPosition(link->GetBodyID(),sx,sy,sz);
		// save the ptr to this link
		m_link.push_back(link);
		// next link will be below this one
		// links are positioned so they touch
		// this avoids needless oscillation 
		// in the simulation
		sy = sy-height-thickness;
	}
}