ode/demo/demo_buggy.cpp

   1 /*************************************************************************
   2  *                                                                       *
   3  * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith.       *
   4  * All rights reserved.  Email: russ@q12.org   Web: www.q12.org          *
   5  *                                                                       *
   6  * This library is free software; you can redistribute it and/or         *
   7  * modify it under the terms of EITHER:                                  *
   8  *   (1) The GNU Lesser General Public License as published by the Free  *
   9  *       Software Foundation; either version 2.1 of the License, or (at  *
  10  *       your option) any later version. The text of the GNU Lesser      *
  11  *       General Public License is included with this library in the     *
  12  *       file LICENSE.TXT.                                               *
  13  *   (2) The BSD-style license that is included with this library in     *
  14  *       the file LICENSE-BSD.TXT.                                       *
  15  *                                                                       *
  16  * This library is distributed in the hope that it will be useful,       *
  17  * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
  18  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files    *
  19  * LICENSE.TXT and LICENSE-BSD.TXT for more details.                     *
  20  *                                                                       *
  21  *************************************************************************/
  22
  23 /*
  24
  25 buggy with suspension.
  26 this also shows you how to use geom groups.
  27
  28 */
  29
  30
  31 #include <ode/ode.h>
  32 #include <drawstuff/drawstuff.h>
  33 #include "texturepath.h"
  34
  35 #ifdef _MSC_VER
  36 #pragma warning(disable:4244 4305)  // for VC++, no precision loss complaints
  37 #endif
  38
  39 // select correct drawing functions
  40
  41 #ifdef dDOUBLE
  42 #define dsDrawBox dsDrawBoxD
  43 #define dsDrawSphere dsDrawSphereD
  44 #define dsDrawCylinder dsDrawCylinderD
  45 #define dsDrawCapsule dsDrawCapsuleD
  46 #endif
  47
  48
  49 // some constants
  50
  51 #define LENGTH 0.7      // chassis length
  52 #define WIDTH 0.5       // chassis width
  53 #define HEIGHT 0.2      // chassis height
  54 #define RADIUS 0.18     // wheel radius
  55 #define STARTZ 0.5      // starting height of chassis
  56 #define CMASS 1         // chassis mass
  57 #define WMASS 0.2       // wheel mass
  58
  59
  60 // dynamics and collision objects (chassis, 3 wheels, environment)
  61
  62 static dWorldID world;
  63 static dSpaceID space;
  64 static dBodyID body[4];
  65 static dJointID joint[3];       // joint[0] is the front wheel
  66 static dJointGroupID contactgroup;
  67 static dGeomID ground;
  68 static dSpaceID car_space;
  69 static dGeomID box[1];
  70 static dGeomID sphere[3];
  71 static dGeomID ground_box;
  72
  73
  74 // things that the user controls
  75
  76 static dReal speed=0,steer=0;   // user commands
  77
  78
  79
  80 // this is called by dSpaceCollide when two objects in space are
  81 // potentially colliding.
  82
  83 static void nearCallback (void *data, dGeomID o1, dGeomID o2)
  84 {
  85   int i,n;
  86
  87   // only collide things with the ground
  88   int g1 = (o1 == ground || o1 == ground_box);
  89   int g2 = (o2 == ground || o2 == ground_box);
  90   if (!(g1 ^ g2)) return;
  91
  92   const int N = 10;
  93   dContact contact[N];
  94   n = dCollide (o1,o2,N,&contact[0].geom,sizeof(dContact));
  95   if (n > 0) {
  96     for (i=0; i<n; i++) {
  97       contact[i].surface.mode = dContactSlip1 | dContactSlip2 |
  98         dContactSoftERP | dContactSoftCFM | dContactApprox1;
  99       contact[i].surface.mu = dInfinity;
 100       contact[i].surface.slip1 = 0.1;
 101       contact[i].surface.slip2 = 0.1;
 102       contact[i].surface.soft_erp = 0.5;
 103       contact[i].surface.soft_cfm = 0.3;
 104       dJointID c = dJointCreateContact (world,contactgroup,&contact[i]);
 105       dJointAttach (c,
 106                     dGeomGetBody(contact[i].geom.g1),
 107                     dGeomGetBody(contact[i].geom.g2));
 108     }
 109   }
 110 }
 111
 112
 113 // start simulation - set viewpoint
 114
 115 static void start()
 116 {
 117   dAllocateODEDataForThread(dAllocateMaskAll);
 118
 119   static float xyz[3] = {0.8317f,-0.9817f,0.8000f};
 120   static float hpr[3] = {121.0000f,-27.5000f,0.0000f};
 121   dsSetViewpoint (xyz,hpr);
 122   printf ("Press:\t'a' to increase speed.\n"
 123           "\t'z' to decrease speed.\n"
 124           "\t',' to steer left.\n"
 125           "\t'.' to steer right.\n"
 126           "\t' ' to reset speed and steering.\n"
 127           "\t'1' to save the current state to 'state.dif'.\n");
 128 }
 129
 130
 131 // called when a key pressed
 132
 133 static void command (int cmd)
 134 {
 135   switch (cmd) {
 136   case 'a': case 'A':
 137     speed += 0.3;
 138     break;
 139   case 'z': case 'Z':
 140     speed -= 0.3;
 141     break;
 142   case ',':
 143     steer -= 0.5;
 144     break;
 145   case '.':
 146     steer += 0.5;
 147     break;
 148   case ' ':
 149     speed = 0;
 150     steer = 0;
 151     break;
 152   case '1': {
 153       FILE *f = fopen ("state.dif","wt");
 154       if (f) {
 155         dWorldExportDIF (world,f,"");
 156         fclose (f);
 157       }
 158     }
 159   }
 160 }
 161
 162
 163 // simulation loop
 164
 165 static void simLoop (int pause)
 166 {
 167   int i;
 168   if (!pause) {
 169     // motor
 170     dJointSetHinge2Param (joint[0],dParamVel2,-speed);
 171     dJointSetHinge2Param (joint[0],dParamFMax2,0.1);
 172
 173     // steering
 174     dReal v = steer - dJointGetHinge2Angle1 (joint[0]);
 175     if (v > 0.1) v = 0.1;
 176     if (v < -0.1) v = -0.1;
 177     v *= 10.0;
 178     dJointSetHinge2Param (joint[0],dParamVel,v);
 179     dJointSetHinge2Param (joint[0],dParamFMax,0.2);
 180     dJointSetHinge2Param (joint[0],dParamLoStop,-0.75);
 181     dJointSetHinge2Param (joint[0],dParamHiStop,0.75);
 182     dJointSetHinge2Param (joint[0],dParamFudgeFactor,0.1);
 183
 184     dSpaceCollide (space,0,&nearCallback);
 185     dWorldStep (world,0.05);
 186
 187     // remove all contact joints
 188     dJointGroupEmpty (contactgroup);
 189   }
 190
 191   dsSetColor (0,1,1);
 192   dsSetTexture (DS_WOOD);
 193   dReal sides[3] = {LENGTH,WIDTH,HEIGHT};
 194   dsDrawBox (dBodyGetPosition(body[0]),dBodyGetRotation(body[0]),sides);
 195   dsSetColor (1,1,1);
 196   for (i=1; i<=3; i++) dsDrawCylinder (dBodyGetPosition(body[i]),
 197                                        dBodyGetRotation(body[i]),0.02f,RADIUS);
 198
 199   dVector3 ss;
 200   dGeomBoxGetLengths (ground_box,ss);
 201   dsDrawBox (dGeomGetPosition(ground_box),dGeomGetRotation(ground_box),ss);
 202
 203   /*
 204   printf ("%.10f %.10f %.10f %.10f\n",
 205           dJointGetHingeAngle (joint[1]),
 206           dJointGetHingeAngle (joint[2]),
 207           dJointGetHingeAngleRate (joint[1]),
 208           dJointGetHingeAngleRate (joint[2]));
 209   */
 210 }
 211
 212
 213 int main (int argc, char **argv)
 214 {
 215   int i;
 216   dMass m;
 217
 218   // setup pointers to drawstuff callback functions
 219   dsFunctions fn;
 220   fn.version = DS_VERSION;
 221   fn.start = &start;
 222   fn.step = &simLoop;
 223   fn.command = &command;
 224   fn.stop = 0;
 225   fn.path_to_textures = DRAWSTUFF_TEXTURE_PATH;
 226
 227   // create world
 228   dInitODE2(0);
 229   world = dWorldCreate();
 230   space = dHashSpaceCreate (0);
 231   contactgroup = dJointGroupCreate (0);
 232   dWorldSetGravity (world,0,0,-0.5);
 233   ground = dCreatePlane (space,0,0,1,0);
 234
 235   // chassis body
 236   body[0] = dBodyCreate (world);
 237   dBodySetPosition (body[0],0,0,STARTZ);
 238   dMassSetBox (&m,1,LENGTH,WIDTH,HEIGHT);
 239   dMassAdjust (&m,CMASS);
 240   dBodySetMass (body[0],&m);
 241   box[0] = dCreateBox (0,LENGTH,WIDTH,HEIGHT);
 242   dGeomSetBody (box[0],body[0]);
 243
 244   // wheel bodies
 245   for (i=1; i<=3; i++) {
 246     body[i] = dBodyCreate (world);
 247     dQuaternion q;
 248     dQFromAxisAndAngle (q,1,0,0,M_PI*0.5);
 249     dBodySetQuaternion (body[i],q);
 250     dMassSetSphere (&m,1,RADIUS);
 251     dMassAdjust (&m,WMASS);
 252     dBodySetMass (body[i],&m);
 253     sphere[i-1] = dCreateSphere (0,RADIUS);
 254     dGeomSetBody (sphere[i-1],body[i]);
 255   }
 256   dBodySetPosition (body[1],0.5*LENGTH,0,STARTZ-HEIGHT*0.5);
 257   dBodySetPosition (body[2],-0.5*LENGTH, WIDTH*0.5,STARTZ-HEIGHT*0.5);
 258   dBodySetPosition (body[3],-0.5*LENGTH,-WIDTH*0.5,STARTZ-HEIGHT*0.5);
 259
 260   // front wheel hinge
 261   /*
 262   joint[0] = dJointCreateHinge2 (world,0);
 263   dJointAttach (joint[0],body[0],body[1]);
 264   const dReal *a = dBodyGetPosition (body[1]);
 265   dJointSetHinge2Anchor (joint[0],a[0],a[1],a[2]);
 266   dJointSetHinge2Axis1 (joint[0],0,0,1);
 267   dJointSetHinge2Axis2 (joint[0],0,1,0);
 268   */
 269
 270   // front and back wheel hinges
 271   for (i=0; i<3; i++) {
 272     joint[i] = dJointCreateHinge2 (world,0);
 273     dJointAttach (joint[i],body[0],body[i+1]);
 274     const dReal *a = dBodyGetPosition (body[i+1]);
 275     dJointSetHinge2Anchor (joint[i],a[0],a[1],a[2]);
 276     dJointSetHinge2Axis1 (joint[i],0,0,1);
 277     dJointSetHinge2Axis2 (joint[i],0,1,0);
 278   }
 279
 280   // set joint suspension
 281   for (i=0; i<3; i++) {
 282     dJointSetHinge2Param (joint[i],dParamSuspensionERP,0.4);
 283     dJointSetHinge2Param (joint[i],dParamSuspensionCFM,0.8);
 284   }
 285
 286   // lock back wheels along the steering axis
 287   for (i=1; i<3; i++) {
 288     // set stops to make sure wheels always stay in alignment
 289     dJointSetHinge2Param (joint[i],dParamLoStop,0);
 290     dJointSetHinge2Param (joint[i],dParamHiStop,0);
 291     // the following alternative method is no good as the wheels may get out
 292     // of alignment:
 293     //   dJointSetHinge2Param (joint[i],dParamVel,0);
 294     //   dJointSetHinge2Param (joint[i],dParamFMax,dInfinity);
 295   }
 296
 297   // create car space and add it to the top level space
 298   car_space = dSimpleSpaceCreate (space);
 299   dSpaceSetCleanup (car_space,0);
 300   dSpaceAdd (car_space,box[0]);
 301   dSpaceAdd (car_space,sphere[0]);
 302   dSpaceAdd (car_space,sphere[1]);
 303   dSpaceAdd (car_space,sphere[2]);
 304
 305   // environment
 306   ground_box = dCreateBox (space,2,1.5,1);
 307   dMatrix3 R;
 308   dRFromAxisAndAngle (R,0,1,0,-0.15);
 309   dGeomSetPosition (ground_box,2,0,-0.34);
 310   dGeomSetRotation (ground_box,R);
 311
 312   // run simulation
 313   dsSimulationLoop (argc,argv,352,288,&fn);
 314
 315   dGeomDestroy (box[0]);
 316   dGeomDestroy (sphere[0]);
 317   dGeomDestroy (sphere[1]);
 318   dGeomDestroy (sphere[2]);
 319   dJointGroupDestroy (contactgroup);
 320   dSpaceDestroy (space);
 321   dWorldDestroy (world);
 322   dCloseODE();
 323   return 0;
 324 }