From f205de7847da7ae1c10212d82e7042d0100b4ce0 Mon Sep 17 00:00:00 2001 From: dan miller Date: Fri, 19 Oct 2007 05:24:38 +0000 Subject: from the start... checking in ode-0.9 --- libraries/ode-0.9/ode/demo/demo_buggy.cpp | 325 ++++++++++++++++++++++++++++++ 1 file changed, 325 insertions(+) create mode 100644 libraries/ode-0.9/ode/demo/demo_buggy.cpp (limited to 'libraries/ode-0.9/ode/demo/demo_buggy.cpp') diff --git a/libraries/ode-0.9/ode/demo/demo_buggy.cpp b/libraries/ode-0.9/ode/demo/demo_buggy.cpp new file mode 100644 index 0000000..a482e02 --- /dev/null +++ b/libraries/ode-0.9/ode/demo/demo_buggy.cpp @@ -0,0 +1,325 @@ +/************************************************************************* + * * + * Open Dynamics Engine, Copyright (C) 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 +#include + +#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 0.7 // chassis length +#define WIDTH 0.5 // chassis width +#define HEIGHT 0.2 // chassis height +#define RADIUS 0.18 // wheel radius +#define STARTZ 0.5 // starting height of chassis +#define CMASS 1 // chassis mass +#define WMASS 0.2 // wheel mass + + +// dynamics and collision objects (chassis, 3 wheels, environment) + +static dWorldID world; +static dSpaceID space; +static dBodyID body[4]; +static dJointID joint[3]; // joint[0] is the front wheel +static dJointGroupID contactgroup; +static dGeomID ground; +static dSpaceID car_space; +static dGeomID box[1]; +static dGeomID sphere[3]; +static dGeomID ground_box; + + +// things that the user controls + +static dReal speed=0,steer=0; // user commands + + + +// 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 == ground || o1 == ground_box); + int g2 = (o2 == ground || o2 == ground_box); + if (!(g1 ^ g2)) return; + + const int N = 10; + dContact contact[N]; + n = dCollide (o1,o2,N,&contact[0].geom,sizeof(dContact)); + if (n > 0) { + for (i=0; i 0.1) v = 0.1; + if (v < -0.1) v = -0.1; + v *= 10.0; + dJointSetHinge2Param (joint[0],dParamVel,v); + dJointSetHinge2Param (joint[0],dParamFMax,0.2); + dJointSetHinge2Param (joint[0],dParamLoStop,-0.75); + dJointSetHinge2Param (joint[0],dParamHiStop,0.75); + dJointSetHinge2Param (joint[0],dParamFudgeFactor,0.1); + + 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(body[0]),dBodyGetRotation(body[0]),sides); + dsSetColor (1,1,1); + for (i=1; i<=3; i++) dsDrawCylinder (dBodyGetPosition(body[i]), + dBodyGetRotation(body[i]),0.02f,RADIUS); + + dVector3 ss; + dGeomBoxGetLengths (ground_box,ss); + dsDrawBox (dGeomGetPosition(ground_box),dGeomGetRotation(ground_box),ss); + + /* + printf ("%.10f %.10f %.10f %.10f\n", + dJointGetHingeAngle (joint[1]), + dJointGetHingeAngle (joint[2]), + dJointGetHingeAngleRate (joint[1]), + dJointGetHingeAngleRate (joint[2])); + */ +} + + +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/textures"; + if(argc==2) + { + fn.path_to_textures = argv[1]; + } + + // create world + dInitODE(); + world = dWorldCreate(); + space = dHashSpaceCreate (0); + contactgroup = dJointGroupCreate (0); + dWorldSetGravity (world,0,0,-0.5); + ground = dCreatePlane (space,0,0,1,0); + + // chassis body + body[0] = dBodyCreate (world); + dBodySetPosition (body[0],0,0,STARTZ); + dMassSetBox (&m,1,LENGTH,WIDTH,HEIGHT); + dMassAdjust (&m,CMASS); + dBodySetMass (body[0],&m); + box[0] = dCreateBox (0,LENGTH,WIDTH,HEIGHT); + dGeomSetBody (box[0],body[0]); + + // wheel bodies + for (i=1; i<=3; i++) { + body[i] = dBodyCreate (world); + dQuaternion q; + dQFromAxisAndAngle (q,1,0,0,M_PI*0.5); + dBodySetQuaternion (body[i],q); + dMassSetSphere (&m,1,RADIUS); + dMassAdjust (&m,WMASS); + dBodySetMass (body[i],&m); + sphere[i-1] = dCreateSphere (0,RADIUS); + dGeomSetBody (sphere[i-1],body[i]); + } + dBodySetPosition (body[1],0.5*LENGTH,0,STARTZ-HEIGHT*0.5); + dBodySetPosition (body[2],-0.5*LENGTH, WIDTH*0.5,STARTZ-HEIGHT*0.5); + dBodySetPosition (body[3],-0.5*LENGTH,-WIDTH*0.5,STARTZ-HEIGHT*0.5); + + // front wheel hinge + /* + joint[0] = dJointCreateHinge2 (world,0); + dJointAttach (joint[0],body[0],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<3; i++) { + joint[i] = dJointCreateHinge2 (world,0); + dJointAttach (joint[i],body[0],body[i+1]); + const dReal *a = dBodyGetPosition (body[i+1]); + dJointSetHinge2Anchor (joint[i],a[0],a[1],a[2]); + dJointSetHinge2Axis1 (joint[i],0,0,1); + dJointSetHinge2Axis2 (joint[i],0,1,0); + } + + // set joint suspension + for (i=0; i<3; i++) { + dJointSetHinge2Param (joint[i],dParamSuspensionERP,0.4); + dJointSetHinge2Param (joint[i],dParamSuspensionCFM,0.8); + } + + // lock back wheels along the steering axis + for (i=1; i<3; i++) { + // set stops to make sure wheels always stay in alignment + dJointSetHinge2Param (joint[i],dParamLoStop,0); + dJointSetHinge2Param (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,box[0]); + dSpaceAdd (car_space,sphere[0]); + dSpaceAdd (car_space,sphere[1]); + dSpaceAdd (car_space,sphere[2]); + + // environment + ground_box = dCreateBox (space,2,1.5,1); + dMatrix3 R; + dRFromAxisAndAngle (R,0,1,0,-0.15); + dGeomSetPosition (ground_box,2,0,-0.34); + dGeomSetRotation (ground_box,R); + + // run simulation + dsSimulationLoop (argc,argv,352,288,&fn); + + dGeomDestroy (box[0]); + dGeomDestroy (sphere[0]); + dGeomDestroy (sphere[1]); + dGeomDestroy (sphere[2]); + dJointGroupDestroy (contactgroup); + dSpaceDestroy (space); + dWorldDestroy (world); + dCloseODE(); + return 0; +} -- cgit v1.1