[ODE] Falling object

[Joao Pereira]

I have a problem my falling object passes through another box without 
stoping....

this is my code:

typedef struct ic3dPos{
    dReal x;
    dReal y;
    dReal z;
}ic3dPos_t;

typedef struct icBody{
    dBodyID bId;
    dGeomID gId;
    ic3dPos_t pos;
    dMass mass;
}icBody_t;


static dWorldID world;
static dSpaceID space;
icBody_t ball;
icBody_t floor1,floor2;
static dJointGroupID contactgroup;

//With this parameters we simulate 1 secs
#define nCicles 100
#define stepSize 0.015

#define sphereMass 1
#define sphereRadius 1

#define planeRotation 33



static void nearCallback (void *data, dGeomID o1, dGeomID o2)
{
  assert(o1);
  assert(o2);

  if (dGeomIsSpace(o1) || dGeomIsSpace(o2))
  {
    fprintf(stderr,"testing space %p %p\n", o1,o2);
    // colliding a space with something
    dSpaceCollide2(o1,o2,data,&nearCallback);
    // Note we do not want to test intersections within a space,
    // only between spaces.
    return;
  }


  const int N = 32;
  dContact contact[N];
  int n = dCollide (o1,o2,N,&(contact[0].geom),sizeof(dContact));
  if (n > 0)
  {
      printf("colliding in %d points\n",n);
    //fprintf(stderr,"testing geoms %p %p\n", o1, o2);

    for (int i=0; i<n; i++)
    {
      contact[i].surface.slip1 = 0.7;
      contact[i].surface.slip2 = 0.7;
      contact[i].surface.mode = dContactSoftERP | dContactSoftCFM | 
dContactApprox1 | dContactSlip1 | dContactSlip2;
      contact[i].surface.mu = 50.0; // was: dInfinity
      contact[i].surface.soft_erp = 0.96;
      contact[i].surface.soft_cfm = 0.04;
      dJointID c = dJointCreateContact (world,contactgroup,&contact[i]);
      dJointAttach (c,
            dGeomGetBody(contact[i].geom.g1),
            dGeomGetBody(contact[i].geom.g2));
        
//printf("**contact(%d)(%f,%f,%f)**",i,contact[i].geom.pos[0],contact[i].geom.pos[1],contact[i].geom.pos[2]);
        //printf("**%s collide with %s 
**",(char*)dGeomGetData(contact[i].geom.g1),(char*)dGeomGetData(contact[i].geom.g2));
    }
    printf("\n");
  }
}

int main()
{
    ic3dPos_t * pos;
    dMass bodyMass;
    //Creation of the world
    world = dWorldCreate();
    space = dSimpleSpaceCreate( 0 );
    contactgroup = dJointGroupCreate(0);

   
    //Gravity
    dWorldSetGravity( world , 0 , 0 , -9.81 );

    //Ball
    ball.bId = dBodyCreate( world );
    pos = (ic3dPos_t *) dBodyGetPosition( ball.bId );
    memcpy( pos , &ball.pos , sizeof( icBody_t ) );
    ball.gId = dCreateBox( 0 , 3 , 3 , 3 );
    dGeomSetData(ball.gId,(void*)"Falling Box");


    ball.pos.x = 0;
    ball.pos.y = 0;
    ball.pos.z = 10;

    dBodySetPosition( ball.bId , ball.pos.x , ball.pos.y , ball.pos.z );
   
    //set's the mass parameters
    dMassSetBoxTotal( &ball.mass , 5 , 3 , 3 , 3 );
    dBodySetMass( ball.bId , &ball.mass );

    dSpaceAdd (space, ball.gId );
    //floor

    floor1.bId = dBodyCreate( world );
    pos = (ic3dPos_t *) dBodyGetPosition( floor1.bId );
    memcpy( pos , &floor1.pos , sizeof( icBody_t ) );
    floor1.gId = dCreateBox( 0 , 10 , 10 ,1 );
    dGeomSetData(floor1.gId,(void*)"Plane Box");
   
    //set's the mass parameters
    dMassSetBoxTotal( &floor1.mass , 7.0 , 10 , 10 , 1 );
    dMassAdjust(&floor1.mass,sphereMass*10);
    dBodySetMass( floor1.bId , &floor1.mass );

    dQuaternion a;
    dQSetIdentity( a );
    dQFromAxisAndAngle( a , 1 , 0 , 0 , planeRotation );

    dGeomSetBody( floor1.gId , floor1.bId );
    dGeomSetPosition( floor1.gId , 0 , 0 , 1 );

    dBodySetGravityMode( floor1.bId , 0 );

    dSpaceAdd (space, floor1.gId );


    dBodyID pl = dBodyCreate( world );

    dGeomID plane;
dTriMeshDataID triMesh;
    // Plane geometry
    const int indexes[6] = {2, 1, 0, 3, 2, 0};
    const dVector3 triVert[4] = {
        { 10.0,  10.0,  0},
        {-10.0,  10.0,  0},
        {-10.0,  -10.0, 0},
        { 10.0,  -10.0, 0}
    };
 
    triMesh = dGeomTriMeshDataCreate();
    dGeomTriMeshDataBuildSimple(triMesh, (dReal*)triVert, 4, indexes, 6);
    plane = dCreateTriMesh(space, triMesh, NULL, NULL, NULL);
    dGeomTriMeshEnableTC( plane , dBoxClass , 1 );
    dGeomSetData(plane, (void*)"Plane");
    dGeomSetPosition(plane, 0, 0, 0);
    dGeomSetBody( plane , pl );
    dBodySetGravityMode( pl , 0 );

    for( int i = 0 ; i < nCicles ; i++ )
    {
       
        const dReal *pos1;
        dVector3 size1,size2;
        pos1 = dBodyGetPosition( ball.bId );
        dGeomBoxGetLengths( ball.gId , size1 );
        ball.pos.x = pos1[0] - size1[0]/2;
        ball.pos.y = pos1[1] - size1[1]/2;
        ball.pos.z = pos1[2] - size1[2]/2;
        pos1 = dGeomGetPosition( floor1.gId );
        dGeomBoxGetLengths( floor1.gId , size2 );
        floor1.pos.x = pos1[0] - size2[0]/2;
        floor1.pos.y = pos1[1] - size2[1]/2;
        floor1.pos.z = pos1[2] + size2[2]/2;
        printf("%d:ball is in(%f,%f,%f)||floor(%f,%f,%f)\n",i , 
ball.pos.x,ball.pos.y,ball.pos.z , floor1.pos.x,floor1.pos.y,floor1.pos.z);
        dSpaceCollide( space, NULL , &nearCallback );
        dWorldStep( world , stepSize );
        cicle = false;
        dJointGroupEmpty(contactgroup);
    }

    dBodyDestroy( floor1.bId );
    dBodyDestroy( ball.bId );
    dWorldDestroy( world );

    return 0;
}

Thnx for the help

João Pereira