[ODE] Sphere vs trimesh problem again :(

[Peter Libič]

Hi,

I'm trying to write something like test_buggy on the trimesh terrain.
So I copied most of code that creates buggy in the test and tried to 
drop it on trimesh terrain.
But the "wheels" are fallin down through terrain.
Small viedo is here (122K) : http://www.ptus.sk/petko/tst.avi
The small cubes in corners are wheels - their geom is sphere, i only 
don't have something to draw asphere yet.
If I change the spheres to boxes, everything seems to be OK.

I'm using dWorldQuickStep with time 0.01s, but dWorldStep results in 
same behaviour.
Normals of the trimesh are pointing up.

Compiled with ODE from svn trunk with vs2005 and double precision.

Here's body creation code:

    dBodyID bID= dBodyCreate( sim->mODEWorld );
    GLVector3d pos = we->getPosition();
    dBodySetPosition( bID, pos.x, pos.y, pos.z );
    GLQuaterniond rot = we->getOrientation();
    dQuaternion quat;
    quat[0] = rot.getW();
    quat[1] = rot.getV().x;
    quat[2] = rot.getV().y;
    quat[3] = rot.getV().z;
    dBodySetQuaternion( bID, quat );

    dMass m;

    dMassSetBox (&m,1,len,wid,hei);
    dMassAdjust (&m, 1);          dBodySetMass (bID,&m);
    dGeomID gID = dCreateBox( sim->mODEOtherSpace, len, wid, hei );
    dGeomSetBody (gID,bID);


        // wheel bodies
        dBodyID wheel[4];
        dGeomID wgeom[4];
        for ( int i=0; i<4; i++ )
        {
            wheel[i] = dBodyCreate( sim->mODEWorld );
                      dMass mass;
                  dBodySetQuaternion (wheel[i],quat);

            dMassSetSphere (&mass,1,WHEEL_RADIUS);
            dMassAdjust (&mass,1);
            dBodySetMass (wheel[i],&mass);

            wgeom[i] = dCreateSphere ( sim->mODEOtherSpace, WHEEL_RADIUS );
            //wgeom[i] = dCreateBox( sim->mODEOtherSpace, 2,2,2 );
            dGeomSetBody( wgeom[i],wheel[i] );
        }

        dBodySetPosition( wheel[0], pos.x + len / 2 , pos.y + wid / 2, 
pos.z - hei / 2 );
        dBodySetPosition( wheel[1], pos.x + len / 2 , pos.y - wid / 2, 
pos.z - hei / 2 );
        dBodySetPosition( wheel[2], pos.x - len / 2 , pos.y + wid / 2, 
pos.z - hei / 2 );
        dBodySetPosition( wheel[3], pos.x - len / 2 , pos.y - wid / 2, 
pos.z - hei / 2 );


        //wheel hinges
        dJointID joint[4];
        for ( int i=0; i<4; i++ )
        {
            joint[i] = dJointCreateHinge2( sim->mODEWorld, 0 );
            dJointAttach (joint[i], bID, wheel[i] );
            const dReal *a = dBodyGetPosition ( wheel[i] );
            dJointSetHinge2Anchor(joint[i],a[0],a[1],a[2]);
            GLVector3d rot_axis = we->getOrientation().rotateVector( 
GLVector3d( 0, 1, 0 ) );
            GLVector3d susp_axis = we->getOrientation().rotateVector( 
GLVector3d( 0, 0, 1 ) );

            /*
            dJointSetHinge2Axis1(joint[i],0,0,1);
            dJointSetHinge2Axis2(joint[i],0,1,0);
            */

            dJointSetHinge2Axis1( joint[i], susp_axis.x, susp_axis.y, 
susp_axis.z );
            dJointSetHinge2Axis2( joint[i], rot_axis.x, rot_axis.y, 
rot_axis.z );
        }

        // set joint suspension - if uncommented this, nothing improves
        /*
        for (int i=0; i<4; i++)
        {
            dJointSetHinge2Param (joint[i],dParamSuspensionERP,0.4);
            dJointSetHinge2Param (joint[i],dParamSuspensionCFM,0.8);
        }
        */

        // lock wheels along the steering axis
        for (int i=0; i<4; i++)
        {
            // set stops to make sure wheels always stay in alignment
            dJointSetHinge2Param (joint[i],dParamLoStop,0);
            dJointSetHinge2Param (joint[i],dParamHiStop,0);
        }


And here is contact creation code:
            contact[i].surface.mode = /*dContactBounce |*/ 
dContactSoftCFM;   //uncomenting bounce doesn't help
            contact[i].surface.mu = 0005;
            contact[i].surface.mu2 = 0;
            contact[i].surface.bounce = 0.5;
            contact[i].surface.bounce_vel = 0.0;
            contact[i].surface.soft_cfm = 0.01;

            dJointID c = 
dJointCreateContact(sim->mODEWorld,sim->mODEContacts, &contact[i]);
            dJointAttach
            (
                c, 
dGeomGetBody(contact[i].geom.g1),dGeomGetBody(contact[i].geom.g2)
            );

Anyone any suggestions?
Tahnks a lot,

Petko