void reset_test()
{
int i;
dMass m,anchor_m;
dReal q[NUM][3], pm[NUM]; // particle positions and masses
dReal pos1[3] = {1,0,1}; // point of reference (POR)
dReal pos2[3] = {-1,0,1}; // point of reference (POR)
// make random particle positions (relative to POR) and masses
for (i=0; i<NUM; i++) {
pm[i] = dRandReal()+0.1;
q[i][0] = dRandReal()-0.5;
q[i][1] = dRandReal()-0.5;
q[i][2] = dRandReal()-0.5;
}
// adjust particle positions so centor of mass = POR
computeMassParams (&m,q,pm);
for (i=0; i<NUM; i++) {
q[i][0] -= m.c[0];
q[i][1] -= m.c[1];
q[i][2] -= m.c[2];
}
if (world) dWorldDestroy (world);
world = dWorldCreate();
anchor_body = dBodyCreate (world);
dBodySetPosition (anchor_body,pos1[0],pos1[1],pos1[2]);
dMassSetBox (&anchor_m,1,SIDE,SIDE,SIDE);
dMassAdjust (&anchor_m,0.1);
dBodySetMass (anchor_body,&anchor_m);
for (i=0; i<NUM; i++) {
particle[i] = dBodyCreate (world);
dBodySetPosition (particle[i],
pos1[0]+q[i][0],pos1[1]+q[i][1],pos1[2]+q[i][2]);
dMassSetBox (&m,1,SIDE,SIDE,SIDE);
dMassAdjust (&m,pm[i]);
dBodySetMass (particle[i],&m);
}
for (i=0; i < NUM; i++) {
particle_joint[i] = dJointCreateBall (world,0);
dJointAttach (particle_joint[i],anchor_body,particle[i]);
const dReal *p = dBodyGetPosition (particle[i]);
dJointSetBallAnchor (particle_joint[i],p[0],p[1],p[2]);
}
// make test_body with the same mass and inertia of the anchor_body plus
// all the particles
test_body = dBodyCreate (world);
dBodySetPosition (test_body,pos2[0],pos2[1],pos2[2]);
computeMassParams (&m,q,pm);
m.mass += anchor_m.mass;
for (i=0; i<12; i++) m.I[i] = m.I[i] + anchor_m.I[i];
dBodySetMass (test_body,&m);
// rotate the test and anchor bodies by a random amount
dQuaternion qrot;
for (i=0; i<4; i++) qrot[i] = dRandReal()-0.5;
dNormalize4 (qrot);
dBodySetQuaternion (anchor_body,qrot);
dBodySetQuaternion (test_body,qrot);
dMatrix3 R;
dQtoR (qrot,R);
for (i=0; i<NUM; i++) {
dVector3 v;
dMultiply0 (v,R,&q[i][0],3,3,1);
dBodySetPosition (particle[i],pos1[0]+v[0],pos1[1]+v[1],pos1[2]+v[2]);
}
// set random torque
for (i=0; i<3; i++) torque[i] = (dRandReal()-0.5) * 0.1;
iteration=0;
}
void testMassFunctions()
{
dMass m;
int i,j;
dReal q[NUMP][3]; // particle positions
dReal pm[NUMP]; // particle masses
dMass m1,m2;
dMatrix3 R;
HEADER;
printf ("\t");
dMassSetZero (&m);
TRAP_MESSAGE (dMassSetParameters (&m,10, 0,0,0, 1,2,3, 4,5,6),
printf (" FAILED (1)\n"), printf (" passed (1)\n"));
printf ("\t");
dMassSetZero (&m);
TRAP_MESSAGE (dMassSetParameters (&m,10, 0.1,0.2,0.15, 3,5,14, 3.1,3.2,4),
printf ("passed (2)\n") , printf (" FAILED (2)\n"));
if (m.mass==10 && m.c[0]==REAL(0.1) && m.c[1]==REAL(0.2) &&
m.c[2]==REAL(0.15) && m._I(0,0)==3 && m._I(1,1)==5 && m._I(2,2)==14 &&
m._I(0,1)==REAL(3.1) && m._I(0,2)==REAL(3.2) && m._I(1,2)==4 &&
m._I(1,0)==REAL(3.1) && m._I(2,0)==REAL(3.2) && m._I(2,1)==4)
printf ("\tpassed (3)\n"); else printf ("\tFAILED (3)\n");
dMassSetZero (&m);
dMassSetSphere (&m,1.4, 0.86);
if (cmp(m.mass,3.73002719949386) && m.c[0]==0 && m.c[1]==0 && m.c[2]==0 &&
cmp(m._I(0,0),1.10349124669826) &&
cmp(m._I(1,1),1.10349124669826) &&
cmp(m._I(2,2),1.10349124669826) &&
m._I(0,1)==0 && m._I(0,2)==0 && m._I(1,2)==0 &&
m._I(1,0)==0 && m._I(2,0)==0 && m._I(2,1)==0)
printf ("\tpassed (4)\n"); else printf ("\tFAILED (4)\n");
dMassSetZero (&m);
dMassSetCapsule (&m,1.3,1,0.76,1.53);
if (cmp(m.mass,5.99961928996029) && m.c[0]==0 && m.c[1]==0 && m.c[2]==0 &&
cmp(m._I(0,0),1.59461986077384) &&
cmp(m._I(1,1),4.21878433864904) &&
cmp(m._I(2,2),4.21878433864904) &&
m._I(0,1)==0 && m._I(0,2)==0 && m._I(1,2)==0 &&
m._I(1,0)==0 && m._I(2,0)==0 && m._I(2,1)==0)
printf ("\tpassed (5)\n"); else printf ("\tFAILED (5)\n");
dMassSetZero (&m);
dMassSetBox (&m,0.27,3,4,5);
if (cmp(m.mass,16.2) && m.c[0]==0 && m.c[1]==0 && m.c[2]==0 &&
cmp(m._I(0,0),55.35) && cmp(m._I(1,1),45.9) && cmp(m._I(2,2),33.75) &&
m._I(0,1)==0 && m._I(0,2)==0 && m._I(1,2)==0 &&
m._I(1,0)==0 && m._I(2,0)==0 && m._I(2,1)==0)
printf ("\tpassed (6)\n"); else printf ("\tFAILED (6)\n");
// test dMassAdjust?
// make random particles and compute the mass, COM and inertia, then
// translate and repeat.
for (i=0; i<NUMP; i++) {
pm[i] = dRandReal()+0.5;
for (j=0; j<3; j++) {
q[i][j] = 2.0*(dRandReal()-0.5);
}
}
computeMassParams (&m1,q,pm);
memcpy (&m2,&m1,sizeof(dMass));
dMassTranslate (&m2,1,2,-3);
for (i=0; i<NUMP; i++) {
q[i][0] += 1;
q[i][1] += 2;
q[i][2] -= 3;
}
computeMassParams (&m1,q,pm);
compareMassParams (&m1,&m2,"7");
// rotate the masses
_R(0,0) = -0.87919618797635;
_R(0,1) = 0.15278881840384;
_R(0,2) = -0.45129772879842;
_R(1,0) = -0.47307856232664;
_R(1,1) = -0.39258064912909;
_R(1,2) = 0.78871864932708;
_R(2,0) = -0.05666336483842;
_R(2,1) = 0.90693771059546;
_R(2,2) = 0.41743652473765;
dMassRotate (&m2,R);
for (i=0; i<NUMP; i++) {
dReal a[3];
dMultiply0 (a,&_R(0,0),&q[i][0],3,3,1);
q[i][0] = a[0];
q[i][1] = a[1];
q[i][2] = a[2];
}
computeMassParams (&m1,q,pm);
compareMassParams (&m1,&m2,"8");
}
