Electrik Cyclist

Members
  • Content count

    1
  • Joined

  • Last visited

Community Reputation

105 Neutral

About Electrik Cyclist

  • Rank
    Newbie
  1. demo_buggy question in ODE

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