[ODE] Re: center of mass

Thu Oct 27 19:00:59 MST 2005

Hey my dear it seems you've sent your answer to ode but not to me, I 
didn't get it. Fortunately Geoff message made me discover your message :)
And my previous message is not already on the mailing list because of 
the file, which has to be moderated :/
But for those who want to read it, you have included it in your message ! ;)

I found difficult to put my code as it is a lot of different functions. 
I will try it now.

> having a 'standard' version would make life simpler for many people. 
Indeed. Sure lots of ppl are just stuck like me for a while, asking themself
"why my object does not move" ?
Getting body reference isnt that useful except in particular cases.

> I've also added a new flag to the geoms to show if it's inside a 
>geomtransform.

In your ode core ? Where ? How ?
I am particularly interested to provide a patch for that. Any code is 
welcome.
Geoff, can you give me sthg on cvs ?

>I used to use one of the geoms as a reference (the axis for example); it's 
>pretty straightforward to calculate the necessary pos and R for a new geom 
>or for the body, as we know that the pos and R of this reference geom is 
>relative to the body.

Sure but I meant concerning another object, with another rigidbody. Or with
respect to the floor.

Now the ugly part :oD

build of the object
***********************
void createWheel(MyObject *obj)
{	
	int k;
	dQuaternion q;dMatrix3 Rtx;
	dMass m;

	obj->body = dBodyCreate (world);
	dBodySetPosition (obj->body,0,0,0.1);
	dGeomID g2[GPB];		// encapsulated geometries

	// start accumulating masses for the encapsulated geometries
	dMass m2;
	dMassSetZero (&m);

	/*-- axe --*/
	k=0;
	obj->geom[k] = dCreateGeomTransform (space);
	dGeomTransformSetCleanup(obj->geom[k],1);
	g2[k] = dCreateCCylinder(0,WHEEL_AXE_RADIUS,WHEEL_AXE_LENGTH);
	dMassSetCappedCylinder(&m2,DENSITY,3,WHEEL_AXE_RADIUS,WHEEL_AXE_LENGTH);
	dGeomTransformSetGeom(obj->geom[k],g2[k]);
	// set the transformation (adjust the mass too)
	dGeomSetPosition(g2[k],0,0,WHEEL_RADIUS);
	dMassTranslate(&m2,0,0,WHEEL_RADIUS);
	dQFromAxisAndAngle(q,1,0,0,M_PI*0.5);
	dGeomSetQuaternion(g2[k], q);
	dQtoR(q, Rtx);
	dMassRotate(&m2,Rtx);
	// add to the total mass
	dMassAdd(&m,&m2);

	/*-- wheel --*/
	k=1;
	obj->geom[k] = dCreateGeomTransform (space);
	dGeomTransformSetCleanup(obj->geom[k],1);	
	g2[k] = dCreateSphere(0,WHEEL_RADIUS);
	dMassSetSphere(&m,1,WHEEL_RADIUS);
	dMassAdjust(&m,0.1);
	dGeomTransformSetGeom(obj->geom[k],g2[k]);
	// set the transformation (adjust the mass too)
	dGeomSetPosition(g2[k],0,-WHEEL_SPACING/2,WHEEL_RADIUS);
	dMassTranslate(&m2,0,-WHEEL_SPACING/2,WHEEL_RADIUS);
	dQFromAxisAndAngle(q,1,0,0,M_PI*0.5);
	dGeomSetQuaternion(g2[k],q);
	dQtoR(q, Rtx);
	dMassRotate(&m2,Rtx);
	// add to the total mass
	dMassAdd(&m,&m2);

	/*-- wheel 2 --*/
	k=2;
	obj->geom[k] = dCreateGeomTransform(space);
	dGeomTransformSetCleanup(obj->geom[k],1);
	g2[k] = dCreateSphere(0,WHEEL_RADIUS);
	dMassSetSphere(&m,1,WHEEL_RADIUS);
	dMassAdjust(&m,0.1);
	dGeomTransformSetGeom(obj->geom[k],g2[k]);
	// set the transformation (adjust the mass too)
	dGeomSetPosition(g2[k],0,WHEEL_SPACING/2,WHEEL_RADIUS);
	dMassTranslate(&m2,0,WHEEL_SPACING/2,WHEEL_RADIUS);
	dQFromAxisAndAngle(q,1,0,0,M_PI*0.5);
	dGeomSetQuaternion(g2[k],q);
	dQtoR(q, Rtx);
	dMassRotate (&m2,Rtx);
	// add to the total mass
	dMassAdd (&m,&m2);

	// move all encapsulated objects so that the center of mass is (0,0,0)
	dGeomSetPosition (g2[0],-m.c[0],-m.c[1],WHEEL_RADIUS-m.c[2]);
	dGeomSetPosition (g2[1],-m.c[0],-WHEEL_SPACING/2-m.c[1],WHEEL_RADIUS-m.c[2]);
	dGeomSetPosition (g2[2],-m.c[0],WHEEL_SPACING/2-m.c[1],WHEEL_RADIUS-m.c[2]);
	dMassTranslate (&m,-m.c[0],-m.c[1],-m.c[2]);

	for (k=0; k < GPB; k++) 
        if (obj->geom[k]) dGeomSetBody (obj->geom[k],obj->body);
	dBodySetMass (obj->body,&m);

}

geomtransf world coordinates**************************
//worldPos & worldR must be ALREADY allocated dReal pointers
void getGeomTransfInWorld(dGeomID g, dReal *worldPos, dReal *worldR)
{
    if (!g) return;
    const dReal *pos = dGeomGetPosition (g);//pos of GeomTransform
    const dReal *R = dGeomGetRotation (g);

    dGeomID g2 = dGeomTransformGetGeom (g);//pos of Encasulated Geom
    const dReal *pos2 = dGeomGetPosition (g2);
    const dReal *R2 = dGeomGetRotation (g2);

    dVector3 actual_pos;
    dMatrix3 actual_R;
    dMULTIPLY0_331 (actual_pos,R,pos2);//convert to world Coordinates
    actual_pos[0] += pos[0];
    actual_pos[1] += pos[1];
    actual_pos[2] += pos[2];
    dMULTIPLY0_333 (actual_R,R,R2);   
    memcpy(worldPos, actual_pos,sizeof(dVector3));
    memcpy(worldR,actual_R,sizeof(dMatrix3));
}

Here its very ugly but I had serious problems getting the right 
coordinates if I did
a prototype like this
void getGeomTransfInWorld(dGeomID g, dVector3 *worldPos, dMatrix *worldR)
which should be better. I don't really like the dReal stuff, it makes 
things confusing

***********************************
Effective creation of the object + vertical axe creation + joint
    createWheel(&obj[0]);

    obj[1].body = dBodyCreate(world);
    k=0;

dMassSetCappedCylinderTotal(&m,DENSITY-4,3,WHEEL_AXE_RADIUS,WHEEL_AXE_LENGTH);
    dBodySetMass(obj[1].body,&m);
    dQuaternion q;
    dQFromAxisAndAngle(q,0,0,1,M_PI*-0.5);
    dBodySetQuaternion(obj[1].body,q);
    obj[1].geom[k] = dCreateCCylinder(0,WHEEL_AXE_RADIUS,WHEEL_AXE_LENGTH);
    dGeomSetBody(obj[1].geom[k],obj[1].body);
    dBodySetPosition(obj[1].body, 0,0,WHEEL_AXE_LENGTH/2+0.1);
    dSpaceAdd(space,obj[1].geom[0]);

    /* articulation wheel+joint */
    dReal* pos = (dReal*)dAlloc(4*sizeof(dReal));// this is ugly and 
even funny I know but if in the core...
    dReal *R = (dReal*)dAlloc(4*3*sizeof(dReal));// and I haven't the 
time to think about sthg better now
    getGeomTransfInWorld(obj[0].geom[0], pos, R);
    joint[0] = dJointCreateHinge(world,0);
    dJointAttach(joint[0],obj[0].body,obj[1].body);
    dJointSetHingeAnchor(joint[0], pos[0],pos[1],pos[2]);
    dJointSetHingeAxis(joint[0], 0, 1, 0);
    joint[1] = dJointCreateFixed(world,0);
    dJointAttach(joint[1],obj[1].body,0);
    dJointSetFixed(joint[1]);
    dJointSetHingeParam(joint[0], dParamVel, 10.0);
    dJointSetHingeParam (joint[0],dParamFMax,4);
    dJointSetHingeParam (joint[0],dParamLoStop, -dInfinity);
    dJointSetHingeParam (joint[0],dParamHiStop, dInfinity);
    dFree(R,3*3*sizeof(dReal));
    dFree(pos,3*sizeof(dReal));

Here the funny thing is I don't need to add geoms to space for the 
complex object to collide
but for a simple geom+rigidbody, I have. Next:

last thing: drawing***************************************
    dReal* pos = (dReal*)dAlloc(4*sizeof(dReal));
    dReal *R = (dReal*)dAlloc(4*3*sizeof(dReal));
    if(obj[0].geom[0])
    {
        dsSetColor (1,0,0);
        getGeomTransfInWorld(obj[0].geom[0], pos,R);
        dsDrawCylinder(pos,R,WHEEL_AXE_LENGTH,WHEEL_AXE_RADIUS);
        dsSetColor (1,1,0);
        for (int j=1; j < GPB; j++)
        {
            getGeomTransfInWorld(obj[0].geom[j], pos,R);
            dsDrawCylinder(pos,R,WHEEL_THICK,WHEEL_RADIUS);

        }
    }
    dFree(R,4*3*sizeof(dReal));
    dFree(pos,4*sizeof(dReal));
    if(obj[1].body != NULL)
        dsDrawCappedCylinder(dBodyGetPosition(obj[1].body),

dBodyGetRotation(obj[1].body),WHEEL_AXE_LENGTH,WHEEL_AXE_RADIUS);

*************** If needed constants are
#define WHEEL_POSX 0
#define WHEEL_POSY 0
#define WHEEL_RADIUS 0.08   
#define WHEEL_THICK 0.02f
#define WHEEL_AXE_RADIUS WHEEL_RADIUS/4
#define WHEEL_SPACING 0.06
#define WHEEL_AXE_LENGTH (2*WHEEL_THICK)+WHEEL_SPACING+0.02

Don't get a headache please ;o)