[ODE] Problem with rotating spheres

[Bjoern Mahn]

Hi all,

okay, it seems that I'm not that good in describing my problem :-). So here 
is a small demo program which shows you what I mean.

Thanks for your help!

Bjoern Mahn.

/*
 * This is a small demonstration program which should show a strange 
 * behavior of ODE with rotating spheres.
 *
 * There are two Spheres which are connected with a "fixed" hinge joint, which
 * means that the joint stop parameters are both 0. The first sphere lies on 
 * the ground, which is in the same space as the Sphere1 (space1). The 
 * second sphere is put into a seperate space (space2) so that it doesn't 
 * collide neither with the ground nor with the sphere1.
 *
 * Now if you start the simulation, you will see that the sphere2's pendular
 * behavior is not physically correct. It gains more and more energy and
 * oscillates higher and higher. After the system has "exploded", ODE gives 
 * the following Message:
 * 
 * "ODE Message 2: vector has zero size in dNormalize4()"
 *
 * ... so why?
 */

#include <ode/ode.h>
#include <drawstuff/drawstuff.h>

#ifdef dDOUBLE
#define dsDrawBox dsDrawBoxD
#define dsDrawSphere dsDrawSphereD
#define dsDrawCylinder dsDrawCylinderD
#define dsDrawCappedCylinder dsDrawCappedCylinderD
#endif

#define  RADIUS_SPHERE1 1.0
#define  MASS_SPHERE1   1.0
#define  RADIUS_SPHERE2 0.2
#define  MASS_SPHERE2   0.5

dWorldID       world;
dSpaceID       space1, space2;
dJointGroupID  contactgroup;
dGeomID        ground;

dBodyID        bSphere1, bSphere2;
dGeomID        gSphere1, gSphere2;
dMass          mSphere1, mSphere2;
dJointID       jFix;

void init()
{
  world  = dWorldCreate(0);
  space1 = dSimpleSpaceCreate(0);
  space2 = dSimpleSpaceCreate(0);
  contactgroup = dJointGroupCreate (0);
  dWorldSetGravity (world, 0, 0, -9.81);
  ground = dCreatePlane (space1, 0, 0, 1, 0);
}

void deinit()
{
  dJointGroupDestroy (contactgroup);
  dBodyDestroy  (bSphere1);
  dBodyDestroy  (bSphere2);
  dGeomDestroy  (gSphere1);
  dGeomDestroy  (gSphere2);
  dJointDestroy (jFix);
  dSpaceDestroy (space1);
  dSpaceDestroy (space2);
  dWorldDestroy (world);
}

void createSpheres(int x, int y, int z)
{
  bSphere1 = dBodyCreate(world);
  bSphere2 = dBodyCreate(world);

  // sphere1 body + geom
  dMassSetSphere (&mSphere1, 1, RADIUS_SPHERE1);
  dMassAdjust    (&mSphere1, MASS_SPHERE1);
  dBodySetMass   (bSphere1, &mSphere1);
  gSphere1 = dCreateSphere (space1, RADIUS_SPHERE1);
  dGeomSetBody       (gSphere1, bSphere1);
  dBodySetPosition   (bSphere1, x, y, z + RADIUS_SPHERE1);

  // sphere2 body + geom
  dMassSetSphere (&mSphere2, 1, RADIUS_SPHERE2);
  dMassAdjust    (&mSphere2, MASS_SPHERE2);
  dBodySetMass   (bSphere2, &mSphere2);
  gSphere2 = dCreateSphere (space2, RADIUS_SPHERE2);
  dGeomSetBody       (gSphere2, bSphere2);
  dBodySetPosition   (bSphere2, 
                      x + RADIUS_SPHERE1 + 0.1 + RADIUS_SPHERE2, 
                      y, 
                      z + RADIUS_SPHERE1);
  
  // fix them with a hinge
  jFix = dJointCreateSlider(world, 0);
  dJointAttach (jFix, bSphere1, bSphere2);
  dJointSetSliderAxis  (jFix, 1, 0, 0);
  dJointSetSliderParam (jFix, dParamVel, 0);
  dJointSetSliderParam (jFix, dParamFMax, 100);
  dJointSetSliderParam (jFix, dParamVel, 0);
  dJointSetSliderParam (jFix, dParamLoStop, 0);
  dJointSetSliderParam (jFix, dParamHiStop, 0); 
  
}

void step(dReal delta)
{
  dWorldStep (world,delta);
  dJointGroupEmpty (contactgroup);
}

void nearCallback (void *data, dGeomID o1, dGeomID o2)
{
  int i,n;

  const int N = 10;
  dContact contact[N];
  n = dCollide (o1,o2,N,&contact[0].geom,sizeof(dContact));
  if (n > 0) {
    for (i=0; i<n ; i++) {

      contact[i].surface.mu = dInfinity;
      
      dJointID c = dJointCreateContact (world,contactgroup,&contact[i]);
      dJointAttach (c,
          dGeomGetBody(contact[i].geom.g1),
          dGeomGetBody(contact[i].geom.g2));
    }
  }
}

void draw()
{
  dsDrawSphere(dGeomGetPosition(gSphere1),
      dGeomGetRotation(gSphere1),
      RADIUS_SPHERE1);
  dsDrawSphere(dGeomGetPosition(gSphere2),
      dGeomGetRotation(gSphere2),
      RADIUS_SPHERE2);
}

static void start()
{
  static float xyz[3] = {0.4369,4.0176,0.170};
  static float hpr[3] = {-90.0000,6.5000,0.000};
  dsSetViewpoint (xyz,hpr);
}

static void command (int cmd)
{
  switch (cmd)
  {
    case 'q': case 'Q': exit(0);
  }
}
static void simLoop (int pause)
{
  if (!pause)
  {
    step(0.01);
    dSpaceCollide (space1, 0, &nearCallback);
  }
  draw();
}

int main(int argc, char **argv)
{
  char *path = new char[1024];
  sprintf(path,"%s/simulators/ode/drawstuff/textures",getenv("HOME"));
  dsFunctions fn;
  fn.version = DS_VERSION;
  fn.start = &start;
  fn.step = &simLoop;
  fn.command = &command;
  fn.stop = 0;
  fn.path_to_textures = path;
  
  init();
  
  createSpheres(0, 0, 0);
  dsSimulationLoop (argc, argv, 300, 300, &fn);

  deinit();
  dCloseODE();  
  return 0;
}