[ODE] Trimesh-trimesh and dGeomTriMeshDataSet

[Peter Kyme]

Hi All,

I'm having some issues with trimesh-trimesh collisions and I'd 
appreciate it if someone could point me in the right direction.

The problem manifests itself as reversed contact normals being returned 
from dCollide for half of the collisions. I have a collection of simple 
tests, and half of these will fail (interpenetration of geoms). If I 
manually reverse the contact normals, the other half will fail.

For example, the following system works fine:

   +--+
   |  |
   +--+
+------+
|      |
|      |
+------+

Here the larger box is static, and the smaller box collides with it as 
expected. However if the situation is reversed:

+------+
|      |
|      |
+------+
   +--+
   |  |
   +--+

The larger box completely penetrates the smaller (static) box, unless I 
reverse contact normals.

I am of the impression this could be related to incorrectly calling the
dGeomTriMeshDataSet( TriMeshData, TRIMESH_LAST_TRANSFORMATION ...
function.

Currently my main simulation loop looks like this:

loop {
    space->collide()
    for all bodies{ setTrimeshLastTransform() }
    dWorldStep()
    space->cleanupCollisions()
}

and setTrimeshLastTransform() looks like:

const dReal *rot = dBodyGetRotation(m_body);
const dReal *pos = dBodyGetPosition(m_body);
const double trans[16] = {rot[0], rot[1], rot[2], pos[0],
                           rot[4], rot[5], rot[6], pos[1],
                           rot[8], rot[9], rot[10], pos[2],
                           0, 0, 0, 1.0 };
dGeomTriMeshDataSet( meshId, TRIMESH_LAST_TRANSFORMATION, (void *) trans );

I'm using a stepsize of 0.005, maximum contacts of 80, and I'm letting 
ODE calculate the trimesh normals.

Does anyone have any ideas what might be going wrong?