[ODE] Re: Copying/Cloning Composite Objects

[john rieffel]

Hi Again,

Here is my updated code for copying/cloning bodies and their
associated geoms, including geom transforms.   The MyObject class is
the same used in the dropstuff example (which my entire code base grew
out of, if you hadn't noticed).  The biggest "catch" was making sure
that every geom E that is encapsulated by a transform T is removed
from the space.  (I don't know how to *create* geoms without inserting
them into the space-- if there's such a way then maybe the code could
be more elegant).  I'm rather happy with the result, and would welcome
any comments and critique:

Again, my code only cares about box geoms now - but it's pretty
obvious how you'd insert code to copy any other kind of geom.  Enjoy.

MyObject ODEWorld::copyObject(MyObject inobj)
{
  MyObject newobj;
  memset (&newobj,0,sizeof(MyObject));
  newobj.rc = inobj.rc;
  newobj.gc = inobj.gc;
  newobj.bc = inobj.bc;

  //create it
  newobj.body = dBodyCreate(world);


  //copy the mass accordingly
  dMass fromMass;
  dBodyGetMass(inobj.body,&fromMass);
  dMass newmass;
  dMassSetZero(&newmass);
  dMassAdd(&newmass,&fromMass);
  dBodySetMass(newobj.body,&newmass);

  // set the position and rotation accordingly
  const dReal *inpos = dBodyGetPosition (inobj.body);
  const dReal *r2 = dBodyGetRotation(inobj.body);
  dBodySetPosition(newobj.body,inpos[0],inpos[1],inpos[2]);
  dBodySetRotation(newobj.body,r2);


  for (int i = 0; i < GPB; i++)
    {
      
      if (inobj.geom[i] != NULL)
	{
	  //copy each associated geom
	  newobj.geom[i] = copyGeom(inobj.geom[i]);
	  //set its body
	  dGeomSetBody(newobj.geom[i],newobj.body);

	}
      else
	newobj.geom[i] = NULL;
    } 

  return newobj;
  
}

dGeomID ODEWorld::copyGeom(dGeomID ingeom)
{
  dGeomID copiedGeom = NULL;

  if (dGeomGetClass(ingeom) == dGeomTransformClass)
    {
      copiedGeom = dCreateGeomTransform(space);
      dGeomTransformSetCleanup (copiedGeom,1);

      dGeomID subGeom = copyGeom(dGeomTransformGetGeom(ingeom));
      dGeomTransformSetGeom (copiedGeom, subGeom);
      // the manual says that if T is a transform encapsulating geom E
      // then E should NOT be inserted into the space.
      // we fix this by removing it after copying
      // indeed - this resolves most weird problems i had
      dSpaceRemove(space,subGeom);
      
       
    }
  else if (dGeomGetClass(ingeom) == dBoxClass)
    {
          dVector3 sides;
      dGeomBoxGetLengths(ingeom, sides);
      copiedGeom = dCreateBox(space,sides[0],sides[1],sides[2]);

      const dReal *r2 = dGeomGetRotation(ingeom);
      dGeomSetRotation(copiedGeom,r2);
      const dReal *inpos = dGeomGetPosition (ingeom);
      dGeomSetPosition(copiedGeom,inpos[0],inpos[1],inpos[2]);
    }
  else
    {
      printf("ODEWorld::copyGeom - I don't know how to copy geom class
%d\n",dGeomGetClass(ingeom));
    }
 
  return copiedGeom;
  
}