[ODE] dGeomSetBody() patch

[David McClurg]

Consider the following sequence of ODE commands--

dGeomSetBody(g, b);
dGeomSetPosition(g, 1,2,3);
dGeomSetBody(g, NULL);
dReal* p = dGeomGetPosition(g);

at this point, I think expecting p == {1,2,3} is reasonable but instead you get p == UNDEFINED.  here is the current code--

void dGeomSetBody (dxGeom *g, dBodyID b)
{
  dAASSERT (g);
  if (b) {
    if (!g->body) dFree (g->pos,sizeof(dxPosR));
    g->body = b;
    g->pos = b->pos;
    g->R = b->R;
  }
  else {
    if (g->body) {
      g->body = 0;
      dxPosR *pr = (dxPosR*) dAlloc (sizeof(dxPosR));
      g->pos = pr->pos;
      g->R = pr->R;
    }
  }
}

--- my suggested patch is the following ---

void dGeomSetBody (dxGeom *g, dBodyID b)
{
  dAASSERT (g);
  if (b) {
    if (!g->body) dFree (g->pos,sizeof(dxPosR));
    g->body = b;
    g->pos = b->pos;
    g->R = b->R;
  }
  else {
    if (g->body) {
      dxPosR *pr = (dxPosR*) dAlloc (sizeof(dxPosR));
      g->pos = pr->pos;
      g->R = pr->R;
      memcpy (g->pos,g->body->pos,3*sizeof(dReal));
      memcpy (g->R,g->body->R,sizeof(dMatrix3));
      g->body = 0;
    }
  }
}

Ta