[ODE] trimesh inertia

[gero.mueller@cloo.de]

Hi!

I successfully integrated the dMassSetTrimesh function. Just add it to  
the mass.cpp file. You need to add the declaration in the mass.h,  
aswell as the trimesh_internal include in the mass.cpp.

It works very well so far.

Gero
-------------- next part --------------
/*
 * Based on Brian Mirtich, "Fast and Accurate Computation of
 * Polyhedral Mass Properties," journal of graphics tools, volume 1,
 * number 2, 1996.
 */
void dMassSetTrimesh( dMass *m, dReal density, dGeomID g )
{
  dAASSERT (m);
  dUASSERT(g && g->type == dTriMeshClass, "argument not a trimesh");
  
  dMassSetZero (m);

  unsigned int triangles = dGeomTriMeshGetTriangleCount( g );
  
  dReal nx, ny, nz;
  unsigned int i, A, B, C;
  // face integrals
  dReal Fa, Fb, Fc, Faa, Fbb, Fcc, Faaa, Fbbb, Fccc, Faab, Fbbc, Fcca;
  
  // projection integrals
  dReal P1, Pa, Pb, Paa, Pab, Pbb, Paaa, Paab, Pabb, Pbbb;

  dReal T0 = 0;
  dReal T1[3] = {0., 0., 0.};
  dReal T2[3] = {0., 0., 0.};
  dReal TP[3] = {0., 0., 0.};

  for( i = 0; i < triangles; i++ )	 	
  {
  	dVector3 v0, v1, v2;	 	  
    dGeomTriMeshGetTriangle( g, i, &v0, &v1, &v2);
    
	dVector3 n, a, b;
	dOP( a, -, v1, v0 ); 
	dOP( b, -, v2, v0 ); 
	dCROSS( n, =, b, a );
	nx = fabs(n[0]);
	ny = fabs(n[1]);
	nz = fabs(n[2]);
		
	if( nx > ny && nx > nz )
		C = 0;
	else
		C = (ny > nz) ? 1 : 2;
	
	A = (C + 1) % 3;
	B = (A + 1) % 3;

	// calculate face integrals
	{
		dReal w;
		dReal k1, k2, k3, k4;
		
		//compProjectionIntegrals(f);
		{
			dReal a0, a1, da;
			dReal b0, b1, db;
			dReal a0_2, a0_3, a0_4, b0_2, b0_3, b0_4;
			dReal a1_2, a1_3, b1_2, b1_3;
			dReal C1, Ca, Caa, Caaa, Cb, Cbb, Cbbb;
			dReal Cab, Kab, Caab, Kaab, Cabb, Kabb;
	
			P1 = Pa = Pb = Paa = Pab = Pbb = Paaa = Paab = Pabb = Pbbb = 0.0;
	
			for( int j = 0; j < 3; j++)
			{
				switch(j)
				{
				case 0:
					a0 = v0[A];
					b0 = v0[B];
					a1 = v1[A];
					b1 = v1[B];
					break;
				case 1:
					a0 = v1[A];
					b0 = v1[B];
					a1 = v2[A];
					b1 = v2[B];
					break;
				case 2:
					a0 = v2[A];
					b0 = v2[B];
					a1 = v0[A];
					b1 = v0[B];
					break;
				}
				da = a1 - a0;
				db = b1 - b0;
				a0_2 = a0 * a0; a0_3 = a0_2 * a0; a0_4 = a0_3 * a0;
				b0_2 = b0 * b0; b0_3 = b0_2 * b0; b0_4 = b0_3 * b0;
				a1_2 = a1 * a1; a1_3 = a1_2 * a1; 
				b1_2 = b1 * b1; b1_3 = b1_2 * b1;
	
	 			C1 = a1 + a0;
				Ca = a1*C1 + a0_2; Caa = a1*Ca + a0_3; Caaa = a1*Caa + a0_4;
				Cb = b1*(b1 + b0) + b0_2; Cbb = b1*Cb + b0_3; Cbbb = b1*Cbb + b0_4;
				Cab = 3*a1_2 + 2*a1*a0 + a0_2; Kab = a1_2 + 2*a1*a0 + 3*a0_2;
				Caab = a0*Cab + 4*a1_3; Kaab = a1*Kab + 4*a0_3;
				Cabb = 4*b1_3 + 3*b1_2*b0 + 2*b1*b0_2 + b0_3;
				Kabb = b1_3 + 2*b1_2*b0 + 3*b1*b0_2 + 4*b0_3;
	
				P1 += db*C1;
				Pa += db*Ca;
				Paa += db*Caa;
				Paaa += db*Caaa;
				Pb += da*Cb;
				Pbb += da*Cbb;
				Pbbb += da*Cbbb;
				Pab += db*(b1*Cab + b0*Kab);
				Paab += db*(b1*Caab + b0*Kaab);
				Pabb += da*(a1*Cabb + a0*Kabb);
			}
	
			P1 /= 2.0;
			Pa /= 6.0;
			Paa /= 12.0;
			Paaa /= 20.0;
			Pb /= -6.0;
			Pbb /= -12.0;
			Pbbb /= -20.0;
			Pab /= 24.0;
			Paab /= 60.0;
			Pabb /= -60.0;
		}			
		
		w = - dDOT(n, v0);

		k1 = 1 / n[C]; k2 = k1 * k1; k3 = k2 * k1; k4 = k3 * k1;
	
		Fa = k1 * Pa;
		Fb = k1 * Pb;
		Fc = -k2 * (n[A]*Pa + n[B]*Pb + w*P1);
	
		Faa = k1 * Paa;
		Fbb = k1 * Pbb;
		Fcc = k3 * (SQR(n[A])*Paa + 2*n[A]*n[B]*Pab + SQR(n[B])*Pbb +
			w*(2*(n[A]*Pa + n[B]*Pb) + w*P1));
	
		Faaa = k1 * Paaa;
		Fbbb = k1 * Pbbb;
		Fccc = -k4 * (CUBE(n[A])*Paaa + 3*SQR(n[A])*n[B]*Paab 
			+ 3*n[A]*SQR(n[B])*Pabb + CUBE(n[B])*Pbbb
			+ 3*w*(SQR(n[A])*Paa + 2*n[A]*n[B]*Pab + SQR(n[B])*Pbb)
			+ w*w*(3*(n[A]*Pa + n[B]*Pb) + w*P1));
	
		Faab = k1 * Paab;
		Fbbc = -k2 * (n[A]*Pabb + n[B]*Pbbb + w*Pbb);
		Fcca = k3 * (SQR(n[A])*Paaa + 2*n[A]*n[B]*Paab + SQR(n[B])*Pabb
			+ w*(2*(n[A]*Paa + n[B]*Pab) + w*Pa));
	}


	T0 += n[0] * ((A == 0) ? Fa : ((B == 0) ? Fb : Fc));

	T1[A] += n[A] * Faa;
	T1[B] += n[B] * Fbb;
	T1[C] += n[C] * Fcc;
	T2[A] += n[A] * Faaa;
	T2[B] += n[B] * Fbbb;
	T2[C] += n[C] * Fccc;
	TP[A] += n[A] * Faab;
	TP[B] += n[B] * Fbbc;
	TP[C] += n[C] * Fcca;
  }

  T1[0] /= 2; T1[1] /= 2; T1[2] /= 2;
  T2[0] /= 3; T2[1] /= 3; T2[2] /= 3;
  TP[0] /= 2; TP[1] /= 2; TP[2] /= 2;

  m->mass = density * T0;
  m->_I(0,0) = density * (T2[1] + T2[2]);
  m->_I(1,1) = density * (T2[2] + T2[0]);
  m->_I(2,2) = density * (T2[0] + T2[1]);
  m->_I(0,1) = - density * TP[0];
  m->_I(1,0) = - density * TP[0];
  m->_I(2,1) = - density * TP[1];
  m->_I(1,2) = - density * TP[1];
  m->_I(2,0) = - density * TP[2];
  m->_I(0,2) = - density * TP[2];

  # ifndef dNODEBUG
  dMassCheck (m);
  # endif
}