[ODE] Trimesh Collision Problem!

[zappone at cli.di.unipi.it]

 Hi,
I have problems with trimesh collision detection!
I have a static ground plane, a box that you can control with keyboard and a
trimesh of a one triangle. The target is to go up the box on the triangle that
is a little tilted,but the things not go well.
That is the code:

#ifndef _MAIN_H_
#include "main.h"
#endif


#define radtodeg	57.295779513			/* 180 / pi */
#define degtorad	0.0174532925			/* pi / 180 */
#define rotmax		1
#define velmax		5

#define VertexCount 3
#define IndexCount 3

int Indices[IndexCount];
dVector3 Vertices[VertexCount];

dGeomID TriMesh;
GLUquadricObj* sp;
dGeomID thegeomID;
dGeomID plane;
dGeomID ground_plane;
dWorldID theDynamicsWorldID;
dBodyID thebodyID;
dSpaceID theCollisionSpaceID;
dJointGroupID  contactgroup;

int speed=0,steer=0,brakes;	// user commands

bool done = false;

float rotplus = 0;

SDL_Event event;

int main(int argc,char** argv)
{
	Init init;

	if(!init.initsdl())
	{
		printf("SDL Loading Failed");
	    return -1;
	}
	else
		printf("  Loaded: SDL\n");

	init.initgl();
	printf("  Loaded: OpenGL\n");


	// 1.  Create a dynamics world.
	theDynamicsWorldID = dWorldCreate();
	dWorldSetGravity(theDynamicsWorldID, 0, -9.81, 0);

	// 2.  Create a collision world.
	theCollisionSpaceID = dSimpleSpaceCreate(0);

	// 3.  Create a joint group to hold the contact joints.
	contactgroup = dJointGroupCreate(0);

	// 4.  Create bodies in the dynamics world.
	thebodyID = dBodyCreate(theDynamicsWorldID);
	dBodySetPosition(thebodyID,0,1.0,10.0);

	// 5.  Set the state (position, etc.) of all bodies.
	dMass mass;
	dMassSetBoxTotal(&mass,1,2,2,4);
	dBodySetMass(thebodyID, &mass);

        // 6.  Create joints in the dynamics world.
        ...nothing...

	// 7.  Create collision geometry objects, as necessary.
	thegeomID = dCreateBox(theCollisionSpaceID,2,2,4);
	dGeomSetCategoryBits(thegeomID,6);

	dGeomSetBody(thegeomID, thebodyID); // join the geom with a body

	ground_plane = dCreatePlane(theCollisionSpaceID,0,1,0,0);
	dGeomSetCategoryBits(ground_plane,8);


	/*****TriMesh**************/
	Vertices[0][0] = -10;
	Vertices[0][1] =   -1;
	Vertices[0][2] = -50;

	Vertices[1][0] =  10;
	Vertices[1][1] =   -1;
	Vertices[1][2] = -50;

	Vertices[2][0] =   0;
	Vertices[2][1] =  10;
	Vertices[2][2] = -30;


	Indices[0] = 2;
	Indices[1] = 0;
	Indices[2] = 1;

	dTriMeshDataID Data = dGeomTriMeshDataCreate();
	dGeomTriMeshDataBuildSimple(Data, (dReal*)Vertices, VertexCount, Indices,
IndexCount);

	TriMesh = dCreateTriMesh(theCollisionSpaceID, Data, 0, 0, 0);
	dGeomSetCategoryBits(TriMesh,7);
	dGeomSetBody(TriMesh,0);

	/* Simulation Loop */
	while ( ! done )
	{
			/* Draw to screen */
			gameDraw();

			/* Process user input */
			gameUserInput();

			/* Update game's physics */
			gamePhysics();
	}
	SDL_Quit();
	return 1;
}


void gameDraw()
{
	glClear(GL_COLOR_BUFFER_BIT|GL_DEPTH_BUFFER_BIT);

	glLoadIdentity();

	setCamera(20);

	// getting position
	const dReal* pos1 = dBodyGetPosition (thebodyID);

	// getting rotation degree
	double deg = testDeg();

	dReal sides1[3];
	dGeomBoxGetLengths (thegeomID, sides1);

	/* Draw Box */
	glPushMatrix();
	glTranslatef(pos1[0],pos1[1],pos1[2]);
	glRotatef(deg + rotplus, 0.0f,1.0f,0.0f);  /*Rotate Around Y-Axis */
	glColor3f(1.0f,0.0f,0.0f);
	drawBox (sides1);
	glPopMatrix();

	/* Draw Ground Plane */
	glColor3f(0.5f,0.5f,0.5f);
	dReal sid[3] = {500,0,500};
	glPushMatrix();
	drawBox(sid);
	glPopMatrix();

	/* Draw Collision Plane */
	glColor3f(0.0f,0.0f,1.0f);
	glBegin(GL_TRIANGLES);
	glVertex3i( -10,-1,-50);
	glVertex3i(  0,10,-30);
	glVertex3i(  10,-1,-50);
	glEnd();

        SDL_GL_SwapBuffers();
}

void gamePhysics()
{
	/* If turn to right */
    if(steer == 1)
	{
      if(rotplus >= -30)
		/* Update the rotplus variable for the rotation of ship around Y-axis */
        rotplus -= 1.5;
    }
	/* If turn to left */
	else if(steer == -1)
	{
      if(rotplus <= 30)

		/* Update the rotplus variable for the rotation of ship around Y-axis */
        rotplus += 1.5;
    }
	/* Otherwise return gradually to straight position */
	else
	{
      if(rotplus > 0)
        rotplus -= 1.5;
      else if(rotplus < 0)
        rotplus += 1.5;
    }
	/* If the player brakes */
	float bLeft;
	float bForward;

	if(brakes)
	{
		bForward = 1.5;
		speed = 0;
	}
	else
		bForward = 0.20;

	bLeft = 2;

	const dReal* vel = dBodyGetLinearVel(thebodyID);

	const dReal* R = dBodyGetRotation(thebodyID);

        // Apply viscosity force
	float vLeft = (float)(R[0]*vel[0] + R[4]*vel[1] + R[8]*vel[2]);
	float vForward = (float)(R[2]*vel[0] + R[6]*vel[1] + R[10]*vel[2]);

	dBodyAddRelForce(thebodyID,-bLeft*vLeft,0,-bForward*vForward);

	/* Linear Velocity Calculations */
	if (speed > 0)
	{
		/* Forward Motion */
		/* Accelerate to Max Speed */
		if(linearVel <= 100)
			dBodyAddRelForce(thebodyID,0,0,-40);
	} else
	if (speed < 0)
	{
		/* Backward Motion */
		if(linearVel <= 100)
			/* Accelerate to Max Speed */
			dBodyAddRelForce(thebodyID,0,0,40);
	}

        /*********************************************/

	const dReal* angV = dBodyGetAngularVel(thebodyID);
	float angVel = angV[1];

	/* Rotational Velocity Calculations */
	if(steer > 0)
	{
		/* Right Rotation */
		if(angVel >= -3)
			/* Accelerate to Max Rotation */
			dBodyAddRelTorque (thebodyID, 0, -2, 0);
	}
	else if(steer < 0)
	{
		/* Left Rotation */
		if(angVel <= 3)
			/* Accelerate to Max Rotation */
			dBodyAddRelTorque(thebodyID,0,2,0);
	}
	else
	{
		if(angVel > 0.1)
			/* Left Decelerate */
			dBodyAddRelTorque (thebodyID, 0, -2, 0);
		else if (angVel < -0.1)
			/* Right Decelerate */
			dBodyAddRelTorque(thebodyID,0,2,0);
		else
			/* Stop Rotating*/
			dBodySetAngularVel(thebodyID,0,0,0);
	}
	/*********************************************/

	// 4. Handle Collision Detection
	dSpaceCollide (theCollisionSpaceID,0,&nearCallback);

	// 5. Take a Simulation step.
	dWorldStep(theDynamicsWorldID,0.01);

	// 6. Remove all joints from the contact group.
	dJointGroupEmpty(contactgroup);
	dGeomTriMeshClearTCCache(TriMesh);
}

void nearCallback (void *data, dGeomID o1, dGeomID o2)
{
	dBodyID b1 = dGeomGetBody(o1);
	dBodyID b2 = dGeomGetBody(o2);

	const int N = 100;
	dContact cnt[N];
	int i,n;
	n = dCollide(o1, o2, N, &cnt[0].geom, sizeof(dContact));
	for (i=0; i<n; i++)
	{
		int cat1 = (int)dGeomGetCategoryBits(o1);
		int cat2 = (int)dGeomGetCategoryBits(o2);

		if((cat1 == 6 && cat2 == 8) || (cat1 == 8 && cat2 == 6))
		{
			/* box <--> ground_plane */

                        //set up contact parameters!
			cnt[i].surface.mu = 0;
			cnt[i].surface.mode = dContactSoftCFM;
		        cnt[i].surface.soft_cfm = 0.01;
		}
		else if((cat1 == 6 && cat2 == 7) || (cat1 == 7 && cat2 == 6))
		{
			/* box <--> collision plane */
			 //set up contact parameters!
			cnt[i].surface.mu = 2;
			cnt[i].surface.mode = dContactSoftCFM;
			cnt[i].surface.soft_cfm = 0.1;
		}
		dJointID c = dJointCreateContact(theDynamicsWorldID, contactgroup, &cnt[i]);
		dJointAttach( c, b1, b2);
	}
}

The problem is that the box has problem to go up the triangle mesh.
What is wrong?
Thanks in advanced
zapnic



----------------------------------------------------------------
This message was sent using IMP, the Internet Messaging Program.