ode/demo/demo_basket.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 // Basket ball demo.
  24 // Serves as a test for the sphere vs trimesh collider
  25 // By Bram Stolk.
  26 // Press the spacebar to reset the position of the ball.
  27
  28 #include <assert.h>
  29 #ifdef HAVE_UNISTD_H
  30 #include <unistd.h>
  31 #endif
  32 #include <ode/ode.h>
  33 #include <drawstuff/drawstuff.h>
  34 #include "texturepath.h"
  35 #include "basket_geom.h" // this is our world mesh
  36
  37 #ifdef _MSC_VER
  38 #pragma warning(disable:4244 4305)  // for VC++, no precision loss complaints
  39 #endif
  40
  41 // some constants
  42
  43 #define RADIUS 0.14
  44
  45 // dynamics and collision objects (chassis, 3 wheels, environment)
  46
  47 static dWorldID world;
  48 static dSpaceID space;
  49
  50 static dBodyID sphbody;
  51 static dGeomID sphgeom;
  52
  53 static dJointGroupID contactgroup;
  54 static dGeomID world_mesh;
  55
  56
  57 // this is called by dSpaceCollide when two objects in space are
  58 // potentially colliding.
  59
  60 static void nearCallback (void *data, dGeomID o1, dGeomID o2)
  61 {
  62   assert(o1);
  63   assert(o2);
  64
  65   if (dGeomIsSpace(o1) || dGeomIsSpace(o2))
  66   {
  67     fprintf(stderr,"testing space %p %p\n", o1,o2);
  68     // colliding a space with something
  69     dSpaceCollide2(o1,o2,data,&nearCallback);
  70     // Note we do not want to test intersections within a space,
  71     // only between spaces.
  72     return;
  73   }
  74
  75 //  fprintf(stderr,"testing geoms %p %p\n", o1, o2);
  76
  77   const int N = 32;
  78   dContact contact[N];
  79   int n = dCollide (o1,o2,N,&(contact[0].geom),sizeof(dContact));
  80   if (n > 0)
  81   {
  82     for (int i=0; i<n; i++)
  83     {
  84           // Paranoia  <-- not working for some people, temporarily removed for 0.6
  85       //dIASSERT(dVALIDVEC3(contact[i].geom.pos));
  86       //dIASSERT(dVALIDVEC3(contact[i].geom.normal));
  87       //dIASSERT(!dIsNan(contact[i].geom.depth));
  88       contact[i].surface.slip1 = 0.7;
  89       contact[i].surface.slip2 = 0.7;
  90       contact[i].surface.mode = dContactSoftERP | dContactSoftCFM | dContactApprox1 | dContactSlip1 | dContactSlip2;
  91       contact[i].surface.mu = 50.0; // was: dInfinity
  92       contact[i].surface.soft_erp = 0.96;
  93       contact[i].surface.soft_cfm = 0.04;
  94       dJointID c = dJointCreateContact (world,contactgroup,&contact[i]);
  95       dJointAttach (c,
  96                     dGeomGetBody(contact[i].geom.g1),
  97                     dGeomGetBody(contact[i].geom.g2));
  98     }
  99   }
 100 }
 101
 102
 103 // start simulation - set viewpoint
 104
 105 static void start()
 106 {
 107   dAllocateODEDataForThread(dAllocateMaskAll);
 108
 109   static float xyz[3] = {-8,0,5};
 110   static float hpr[3] = {0.0f,-29.5000f,0.0000f};
 111   dsSetViewpoint (xyz,hpr);
 112 }
 113
 114
 115
 116 static void reset_ball(void)
 117 {
 118   float sx=0.0f, sy=3.40f, sz=7.05;
 119
 120 #if defined(_MSC_VER) && defined(dDOUBLE)
 121   sy -= 0.01; // Cheat, to make it score under win32/double
 122 #endif
 123
 124   dQuaternion q;
 125   dQSetIdentity(q);
 126   dBodySetPosition (sphbody, sx, sy, sz);
 127   dBodySetQuaternion(sphbody, q);
 128   dBodySetLinearVel (sphbody, 0,0,0);
 129   dBodySetAngularVel (sphbody, 0,0,0);
 130 }
 131
 132
 133 // called when a key pressed
 134
 135 static void command (int cmd)
 136 {
 137   switch (cmd)
 138   {
 139     case ' ':
 140           reset_ball();
 141       break;
 142   }
 143 }
 144
 145
 146 // simulation loop
 147
 148 static void simLoop (int pause)
 149 {
 150   double simstep = 0.001; // 1ms simulation steps
 151   double dt = dsElapsedTime();
 152
 153   int nrofsteps = (int) ceilf(dt/simstep);
 154 //  fprintf(stderr, "dt=%f, nr of steps = %d\n", dt, nrofsteps);
 155
 156   for (int i=0; i<nrofsteps && !pause; i++)
 157   {
 158     dSpaceCollide (space,0,&nearCallback);
 159     dWorldQuickStep (world, simstep);
 160     dJointGroupEmpty (contactgroup);
 161   }
 162
 163   dsSetColor (1,1,1);
 164   const dReal *SPos = dBodyGetPosition(sphbody);
 165   const dReal *SRot = dBodyGetRotation(sphbody);
 166   float spos[3] = {SPos[0], SPos[1], SPos[2]};
 167   float srot[12] = { SRot[0], SRot[1], SRot[2], SRot[3], SRot[4], SRot[5], SRot[6], SRot[7], SRot[8], SRot[9], SRot[10], SRot[11] };
 168   dsDrawSphere
 169   (
 170     spos,
 171     srot,
 172     RADIUS
 173   );
 174
 175   // draw world trimesh
 176   dsSetColor(0.4,0.7,0.9);
 177   dsSetTexture (DS_NONE);
 178
 179   const dReal* Pos = dGeomGetPosition(world_mesh);
 180   //dIASSERT(dVALIDVEC3(Pos));
 181   float pos[3] = { Pos[0], Pos[1], Pos[2] };
 182
 183   const dReal* Rot = dGeomGetRotation(world_mesh);
 184   //dIASSERT(dVALIDMAT3(Rot));
 185   float rot[12] = { Rot[0], Rot[1], Rot[2], Rot[3], Rot[4], Rot[5], Rot[6], Rot[7], Rot[8], Rot[9], Rot[10], Rot[11] };
 186
 187   int numi = sizeof(world_indices)  / sizeof(dTriIndex);
 188
 189   for (int i=0; i<numi/3; i++)
 190   {
 191     int i0 = world_indices[i*3+0];
 192     int i1 = world_indices[i*3+1];
 193     int i2 = world_indices[i*3+2];
 194     float *v0 = world_vertices+i0*3;
 195     float *v1 = world_vertices+i1*3;
 196     float *v2 = world_vertices+i2*3;
 197     dsDrawTriangle(pos, rot, v0,v1,v2, true); // single precision draw
 198   }
 199 }
 200
 201
 202 int main (int argc, char **argv)
 203 {
 204   dMass m;
 205   dMatrix3 R;
 206
 207   // setup pointers to drawstuff callback functions
 208   dsFunctions fn;
 209   fn.version = DS_VERSION;
 210   fn.start = &start;
 211   fn.step = &simLoop;
 212   fn.command = &command;
 213   fn.stop = 0;
 214   fn.path_to_textures = DRAWSTUFF_TEXTURE_PATH;
 215
 216   // create world
 217   dInitODE2(0);
 218   world = dWorldCreate();
 219   space = dHashSpaceCreate (0);
 220
 221   contactgroup = dJointGroupCreate (0);
 222   dWorldSetGravity (world,0,0,-9.8);
 223   dWorldSetQuickStepNumIterations (world, 64);
 224
 225   // Create a static world using a triangle mesh that we can collide with.
 226   int numv = sizeof(world_vertices)/(3*sizeof(float));
 227   int numi = sizeof(world_indices)/ sizeof(dTriIndex);
 228   printf("numv=%d, numi=%d\n", numv, numi);
 229   dTriMeshDataID Data = dGeomTriMeshDataCreate();
 230
 231 //  fprintf(stderr,"Building Single Precision Mesh\n");
 232
 233   dGeomTriMeshDataBuildSingle
 234   (
 235     Data,
 236     world_vertices,
 237     3 * sizeof(float),
 238     numv,
 239     world_indices,
 240     numi,
 241     3 * sizeof(dTriIndex)
 242   );
 243
 244   world_mesh = dCreateTriMesh(space, Data, 0, 0, 0);
 245   dGeomTriMeshEnableTC(world_mesh, dSphereClass, false);
 246   dGeomTriMeshEnableTC(world_mesh, dBoxClass, false);
 247   dGeomSetPosition(world_mesh, 0, 0, 0.5);
 248   dRSetIdentity(R);
 249   //dIASSERT(dVALIDMAT3(R));
 250
 251   dGeomSetRotation (world_mesh, R);
 252
 253   //float sx=0.0, sy=3.40, sz=6.80;
 254   (void)world_normals; // get rid of compiler warning
 255   sphbody = dBodyCreate (world);
 256   dMassSetSphere (&m,1,RADIUS);
 257   dBodySetMass (sphbody,&m);
 258   sphgeom = dCreateSphere(0, RADIUS);
 259   dGeomSetBody (sphgeom,sphbody);
 260   reset_ball();
 261   dSpaceAdd (space, sphgeom);
 262
 263   // run simulation
 264   dsSimulationLoop (argc,argv,352,288,&fn);
 265
 266   // Causes segm violation? Why?
 267   // (because dWorldDestroy() destroys body connected to geom; must call first!)
 268   dGeomDestroy(sphgeom);
 269   dGeomDestroy (world_mesh);
 270
 271   dJointGroupEmpty (contactgroup);
 272   dJointGroupDestroy (contactgroup);
 273   dSpaceDestroy (space);
 274   dWorldDestroy (world);
 275   dCloseODE();
 276   return 0;
 277 }
 278
 279
 280