Sign in to follow this  
Electrik Cyclist

demo_buggy question in ODE

Recommended Posts

hello,

I am modifying the demo_buggy sample in ODE to experiment with triangle meshes collision. For the moment, I have replaced the ground plane, (0,0,1,0), with a triangle mesh ( quad with 2 tris ) and noticed the buggy penetrates the mesh ( without falling through it ), unlike the scene with the plane, when it works fine. I guess the normals are fine, but, for checking, I have added the 2 indices arrays to switch the direction of normals. I am new to ODE and I would like to understand the reason behind this. Bellow you can find the modified code ( best way - backup the old file and copy this one over, then run the sample ).


/*************************************************************************
* *
* Open Dynamics Engine, Copyright © 2001,2002 Russell L. Smith. *
* All rights reserved. Email: russ@q12.org Web: www.q12.org *
* *
* This library is free software; you can redistribute it and/or *
* modify it under the terms of EITHER: *
* (1) The GNU Lesser General Public License as published by the Free *
* Software Foundation; either version 2.1 of the License, or (at *
* your option) any later version. The text of the GNU Lesser *
* General Public License is included with this library in the *
* file LICENSE.TXT. *
* (2) The BSD-style license that is included with this library in *
* the file LICENSE-BSD.TXT. *
* *
* This library is distributed in the hope that it will be useful, *
* but WITHOUT ANY WARRANTY; without even the implied warranty of *
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files *
* LICENSE.TXT and LICENSE-BSD.TXT for more details. *
* *
*************************************************************************/

/*

buggy with suspension.
this also shows you how to use geom groups.

*/


#include <ode/ode.h>
#include <drawstuff/drawstuff.h>
#include "texturepath.h"
#include "windows.h"
#include "gl/gl.h"

#ifdef _MSC_VER
#pragma warning(disable:4244 4305) // for VC++, no precision loss complaints
#endif

// select correct drawing functions

#ifdef dDOUBLE
#define dsDrawBox dsDrawBoxD
#define dsDrawSphere dsDrawSphereD
#define dsDrawCylinder dsDrawCylinderD
#define dsDrawCapsule dsDrawCapsuleD
#endif


// some constants

#define LENGTH 1.0 // chassis length
#define WIDTH 0.6 // chassis width
#define HEIGHT 0.1 // chassis height
#define RADIUS 0.1 // wheel radius
#define STARTZ 0.08 // starting height of chassis
#define CMASS 80.0f // chassis mass
#define WMASS 1.0 // wheel mass


// dynamics and collision objects (chassis, 3 wheels, environment)

static dWorldID world;
static dSpaceID space;
static dBodyID buggy_body;
static dBodyID buggy_wheelbody[4];
static dJointID buggy_joint[4]; // joint[0] is the front wheel
static dJointGroupID contactgroup;
static dGeomID buggy_ground;
static dSpaceID car_space;
static dGeomID buggy_geom;
static dGeomID buggy_sphere[4];
static dGeomID ground_box;


// things that the user controls

static dReal speed=0,steer=0; // user commands

#define ELEMENTS_NUMBER(_x) (sizeof(_x) / sizeof( (_x)[0] ))

static void setupContact( dContact * contact );


// this is called by dSpaceCollide when two objects in space are
// potentially colliding.

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

// only collide things with the ground
int g1 = (o1 == buggy_ground || o1 == ground_box);
int g2 = (o2 == buggy_ground || o2 == ground_box);
if (!(g1 ^ g2)) return;
/*if ( dGeomIsSpace(o2) || dGeomIsSpace(o1) )
{
return;
}*/

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

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


// start simulation - set viewpoint

static void start()
{
dAllocateODEDataForThread(dAllocateMaskAll);

static float xyz[3] = {0.8317f,-0.9817f,0.8000f};
static float hpr[3] = {121.0000f,-27.5000f,0.0000f};
dsSetViewpoint (xyz,hpr);
printf ("Press:\t'a' to increase speed.\n"
"\t'z' to decrease speed.\n"
"\t',' to steer left.\n"
"\t'.' to steer right.\n"
"\t' ' to reset speed and steering.\n"
"\t'1' to save the current state to 'state.dif'.\n");
}


// called when a key pressed

static void command (int cmd)
{
switch (cmd)
{
case 'a': case 'A':
speed += 0.3;
break;

case 'z': case 'Z':
speed -= 0.3;
break;

case ',':
if (steer > -1.5f)
{
steer -= 0.5;
}
break;

case '.':
if (steer < 1.5f)
{
steer += 0.5;
}
break;

case ' ':
speed = 0;
steer = 0;
break;

case '1':
{
FILE *f = fopen ("state.dif","wt");
if (f)
{
dWorldExportDIF (world,f,"");
fclose (f);
}
}
}
}

float vBox[6][3] =
{
{ 0, 15, -10 }, { 20, 15, -10 }, { 20, -15, -10 },
{ 0, 15, 10 }, { 20, 15, 10 }, { 20, -15, 10 },
};

dTriIndex vIndices0[12] =
{
0, 3, 1,
1, 3, 4,
1, 4, 2,
2, 4, 5,
};
dTriIndex vIndices1[12] =
{
0, 1, 3,
1, 4, 3,
1, 2, 4,
2, 5, 4,
};

float vBoxPlane[][3] =
{
{ -1000, -1000, 0 }, { -1000, 1000, 0 }, { 1000, 1000, 0 }, { 1000, -1000, 0 }
};

dTriIndex vIndices0P[6] =
{
0, 1, 2,
2, 1, 3,
};
dTriIndex vIndices1P[6] =
{
0, 2, 1,
2, 3, 1
};

static void setJointSteer( dJointID djID, dReal v );
static void setJointSpeed(dJointID djID, dReal speed);

// simulation loop
static void simLoop (int pause)
{
int i;
if (!pause)
{
// motor
setJointSpeed(buggy_joint[0], speed);
setJointSpeed(buggy_joint[1], speed);

// steering
dReal v = steer - dJointGetHinge2Angle1 (buggy_joint[0]);
if (v > 0.1) v = 0.1;
if (v < -0.1) v = -0.1;
v *= 10.0;

setJointSteer(buggy_joint[0], v);
setJointSteer(buggy_joint[1], v);

dSpaceCollide (space,0,&nearCallback);
dWorldStep (world,0.05);

// remove all contact joints
dJointGroupEmpty (contactgroup);
}

dsSetColor (0,1,1);
dsSetTexture (DS_WOOD);
dReal sides[3] = {LENGTH,WIDTH,HEIGHT};
dsDrawBox (dBodyGetPosition(buggy_body),dBodyGetRotation(buggy_body),sides);
dsSetColor (1,1,1);
for (i=0; i<4; i++)
{
//dsDrawCylinder(dBodyGetPosition(body[i]), dBodyGetRotation(body[i]),0.02f,RADIUS);
dsDrawSphere( dBodyGetPosition(buggy_wheelbody[i]), dBodyGetRotation(buggy_wheelbody[i]), RADIUS );
}

//dVector3 ss = {30, 40, 40};
//dVector3 Pos = {0,0,0};
////dGeomBoxGetLengths (ground_box,ss);
//dsDrawBox(Pos /*dGeomGetPosition(ground_box)*/,dGeomGetRotation(ground_box),ss);
glColor3b(0, 0, 255);
glBegin(GL_TRIANGLES);
{
for ( int i = 0; i < sizeof(vIndices1)/sizeof(vIndices1[0]); i++ )
{
glVertex3f( vBox[vIndices1[i]][0], vBox[vIndices1[i]][1], vBox[vIndices1[i]][2] );
}
}
glEnd();

/*
printf ("%.10f %.10f %.10f %.10f\n",
dJointGetHingeAngle (joint[1]),
dJointGetHingeAngle (joint[2]),
dJointGetHingeAngleRate (joint[1]),
dJointGetHingeAngleRate (joint[2]));
*/
}

#define BUGGY_USETRIANGLEMESHBOX
#define BUGGY_USETRIANGLEMESHPLANE

int main (int argc, char **argv)
{
int i;
dMass m;

// setup pointers to drawstuff callback functions
dsFunctions fn;
fn.version = DS_VERSION;
fn.start = &start;
fn.step = &simLoop;
fn.command = &command;
fn.stop = 0;
fn.path_to_textures = DRAWSTUFF_TEXTURE_PATH;

// create world
dInitODE2(0);
world = dWorldCreate();
space = dHashSpaceCreate (0);
contactgroup = dJointGroupCreate (0);
dWorldSetGravity (world,0,0,-9.81f);
#ifndef BUGGY_USETRIANGLEMESHPLANE
buggy_ground = dCreatePlane(space,0,0,1,0);
#else
dTriMeshDataID PlaneTriMeshDataID = dGeomTriMeshDataCreate();
dGeomTriMeshDataBuildSingle(
PlaneTriMeshDataID,
&(vBoxPlane[0][0]),
sizeof(vBoxPlane[0]),
ELEMENTS_NUMBER(vBoxPlane),
&(vIndices0P[0]),
ELEMENTS_NUMBER(vIndices0P),
3*sizeof(vIndices0P[0])
);
buggy_ground = dCreateTriMesh( space, PlaneTriMeshDataID, 0, 0, 0);
#endif

// chassis body
buggy_body = dBodyCreate (world);
dBodySetPosition (buggy_body,0,0,STARTZ);
dMassSetBox (&m,1,LENGTH,WIDTH,HEIGHT);
dMassAdjust (&m,CMASS);
dBodySetMass (buggy_body,&m);
buggy_geom = dCreateBox (0,LENGTH,WIDTH,HEIGHT);
dGeomSetBody (buggy_geom,buggy_body);

// wheel bodies
for (i=0; i<4; i++)
{
buggy_wheelbody[i] = dBodyCreate (world);
dQuaternion q;
dQFromAxisAndAngle (q,1,0,0,M_PI*0.5);
dBodySetQuaternion (buggy_wheelbody[i],q);
dMassSetSphere (&m,1,RADIUS);
dMassAdjust (&m,WMASS);
dBodySetMass (buggy_wheelbody[i],&m);
buggy_sphere[i] = dCreateSphere (0,RADIUS);
dGeomSetBody (buggy_sphere[i],buggy_wheelbody[i]);
}

dBodySetPosition( buggy_wheelbody[0], 0.5*LENGTH, WIDTH*0.5, -(RADIUS - HEIGHT*0.5) );
dBodySetPosition( buggy_wheelbody[1], 0.5*LENGTH, -WIDTH*0.5, -(RADIUS - HEIGHT*0.5) );
dBodySetPosition( buggy_wheelbody[2], -0.5*LENGTH, WIDTH*0.5, -(RADIUS - HEIGHT*0.5) );
dBodySetPosition( buggy_wheelbody[3], -0.5*LENGTH, -WIDTH*0.5, -(RADIUS - HEIGHT*0.5) );

// front wheel hinge
/*
joint[0] = dJointCreateHinge2 (world,0);
dJointAttach (joint[0],buggy_body,body[1]);
const dReal *a = dBodyGetPosition (body[1]);
dJointSetHinge2Anchor (joint[0],a[0],a[1],a[2]);
dJointSetHinge2Axis1 (joint[0],0,0,1);
dJointSetHinge2Axis2 (joint[0],0,1,0);
*/

// front and back wheel hinges
for (i=0; i<4; i++)
{
buggy_joint[i] = dJointCreateHinge2 (world,0);
dJointAttach (buggy_joint[i],buggy_body,buggy_wheelbody[i]);
const dReal *a = dBodyGetPosition (buggy_wheelbody[i]);
dJointSetHinge2Anchor (buggy_joint[i],a[0],a[1],a[2]);
dJointSetHinge2Axis1 (buggy_joint[i],0,0,1);
dJointSetHinge2Axis2 (buggy_joint[i],0,1,0);
}

// set joint suspension
for (i=0; i<4; i++)
{
dJointSetHinge2Param (buggy_joint[i],dParamSuspensionERP,0.14);
dJointSetHinge2Param (buggy_joint[i],dParamSuspensionCFM,0.0002);
}

// lock back wheels along the steering axis
for (i=0; i<4; i++)
{
// set stops to make sure wheels always stay in alignment
dJointSetHinge2Param (buggy_joint[i],dParamLoStop,0);
dJointSetHinge2Param (buggy_joint[i],dParamHiStop,0);
// the following alternative method is no good as the wheels may get out
// of alignment:
// dJointSetHinge2Param (joint[i],dParamVel,0);
// dJointSetHinge2Param (joint[i],dParamFMax,dInfinity);
}

// create car space and add it to the top level space
car_space = dSimpleSpaceCreate (space);
dSpaceSetCleanup (car_space,0);
dSpaceAdd (car_space,buggy_geom);
dSpaceAdd (car_space,buggy_sphere[0]);
dSpaceAdd (car_space,buggy_sphere[1]);
dSpaceAdd (car_space,buggy_sphere[2]);
dSpaceAdd (car_space,buggy_sphere[3]);

// environment
#ifndef BUGGY_USETRIANGLEMESHBOX
ground_box = dCreateBox( space, 20, 15, 10 );
#else
dTriMeshDataID BoxTriMeshDataID = dGeomTriMeshDataCreate();
dGeomTriMeshDataBuildSingle(
BoxTriMeshDataID,
&(vBox[0][0]),
sizeof(vBox[0]),
sizeof(vBox)/sizeof(vBox[0][0]),
&(vIndices1[0]),
sizeof(vIndices1)/sizeof(vIndices1[0]),
sizeof(vIndices1[0])
);
ground_box = dCreateTriMesh( space, BoxTriMeshDataID, 0, 0, 0);
#endif
/*
dMatrix3 RY;
dRFromAxisAndAngle (RY,0,1,0,-0.15);
*/
/*
dMatrix3 RZ;
dRFromAxisAndAngle( RZ, 0, 0, 1, -0.75 );
dGeomSetPosition( ground_box, 20, 0, -3.4 );
dGeomSetRotation( ground_box, RZ );
*/

// run simulation
dsSimulationLoop (argc,argv,352,288,&fn);

dGeomDestroy (buggy_geom);
dGeomDestroy (buggy_sphere[0]);
dGeomDestroy (buggy_sphere[1]);
dGeomDestroy (buggy_sphere[2]);
dGeomDestroy (buggy_sphere[3]);
dJointGroupDestroy (contactgroup);
dSpaceDestroy (space);
dWorldDestroy (world);
dCloseODE();
return 0;
}

void setJointSteer( dJointID djID, dReal v )
{
dJointSetHinge2Param( djID,dParamVel, v );
dJointSetHinge2Param( djID,dParamFMax, 0.2 );
dJointSetHinge2Param( djID,dParamLoStop, -0.75 );
dJointSetHinge2Param( djID,dParamHiStop, 0.75 );
dJointSetHinge2Param( djID,dParamFudgeFactor, 0.1 );
}

void setJointSpeed(dJointID djID, dReal speed)
{
dJointSetHinge2Param (djID,dParamVel2,-speed);
dJointSetHinge2Param (djID,dParamFMax2, 200.1);
}

void setupContact( dContact * contact )
{
contact->surface.mode = dContactSlip1 | dContactSlip2 | dContactSoftERP | dContactSoftCFM;
contact->surface.mu = dInfinity;
contact->surface.slip1 = 0.01f;
contact->surface.slip2 = 0.01f;
contact->surface.soft_erp = 0.95f;
contact->surface.soft_cfm = 0.00001f;
}

Share this post


Link to post
Share on other sites

Create an account or sign in to comment

You need to be a member in order to leave a comment

Create an account

Sign up for a new account in our community. It's easy!

Register a new account

Sign in

Already have an account? Sign in here.

Sign In Now

Sign in to follow this