Jump to content

  • Log In with Google      Sign In   
  • Create Account

We're offering banner ads on our site from just $5!

1. Details HERE. 2. GDNet+ Subscriptions HERE. 3. Ad upload HERE.


demo_buggy question in ODE


Old topic!
Guest, the last post of this topic is over 60 days old and at this point you may not reply in this topic. If you wish to continue this conversation start a new topic.

  • You cannot reply to this topic
No replies to this topic

#1 ecyclist   Members   -  Reputation: 105

Like
0Likes
Like

Posted 12 June 2012 - 10:11 AM

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;
}

Sponsor:



Old topic!
Guest, the last post of this topic is over 60 days old and at this point you may not reply in this topic. If you wish to continue this conversation start a new topic.



PARTNERS