ode/demo/demo_trimesh.cpp

   1 /*************************************************************************
   2  *                                                                       *
   3  * Open Dynamics Engine, Copyright (C) 2001-2003 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 // TriMesh test by Erwin de Vries
  24
  25 #include <ode/ode.h>
  26 #include <drawstuff/drawstuff.h>
  27 #include "texturepath.h"
  28
  29 #ifdef _MSC_VER
  30 #pragma warning(disable:4244 4305)  // for VC++, no precision loss complaints
  31 #endif
  32
  33 // select correct drawing functions
  34
  35 #ifdef dDOUBLE
  36 #define dsDrawBox dsDrawBoxD
  37 #define dsDrawSphere dsDrawSphereD
  38 #define dsDrawCylinder dsDrawCylinderD
  39 #define dsDrawCapsule dsDrawCapsuleD
  40 #define dsDrawLine dsDrawLineD
  41 #define dsDrawTriangle dsDrawTriangleD
  42 #endif
  43
  44
  45 // some constants
  46
  47 #define NUM 200                 // max number of objects
  48 #define DENSITY (5.0)           // density of all objects
  49 #define GPB 3                   // maximum number of geometries per body
  50 #define MAX_CONTACTS 40         // maximum number of contact points per body
  51
  52
  53 // dynamics and collision objects
  54
  55 struct MyObject {
  56   dBodyID body;                 // the body
  57   dGeomID geom[GPB];            // geometries representing this body
  58 };
  59
  60 static int num=0;               // number of objects in simulation
  61 static int nextobj=0;           // next object to recycle if num==NUM
  62 static dWorldID world;
  63 static dSpaceID space;
  64 static MyObject obj[NUM];
  65 static dJointGroupID contactgroup;
  66 static int selected = -1;       // selected object
  67 static int show_aabb = 0;       // show geom AABBs?
  68 static int show_contacts = 0;   // show contact points?
  69 static int random_pos = 1;      // drop objects from random position?
  70
  71 #define VertexCount 5
  72 #define IndexCount 12
  73
  74 static dVector3 Size;
  75 static float Vertices[VertexCount][3];
  76 static dTriIndex Indices[IndexCount];
  77
  78 static dGeomID TriMesh;
  79 static dGeomID Ray;
  80
  81
  82 // this is called by dSpaceCollide when two objects in space are
  83 // potentially colliding.
  84
  85 static void nearCallback (void *data, dGeomID o1, dGeomID o2)
  86 {
  87   int i;
  88   // if (o1->body && o2->body) return;
  89
  90   // exit without doing anything if the two bodies are connected by a joint
  91   dBodyID b1 = dGeomGetBody(o1);
  92   dBodyID b2 = dGeomGetBody(o2);
  93   if (b1 && b2 && dAreConnectedExcluding (b1,b2,dJointTypeContact)) return;
  94
  95   dContact contact[MAX_CONTACTS];   // up to MAX_CONTACTS contacts per box-box
  96   for (i=0; i<MAX_CONTACTS; i++) {
  97     contact[i].surface.mode = dContactBounce | dContactSoftCFM;
  98     contact[i].surface.mu = dInfinity;
  99     contact[i].surface.mu2 = 0;
 100     contact[i].surface.bounce = 0.1;
 101     contact[i].surface.bounce_vel = 0.1;
 102     contact[i].surface.soft_cfm = 0.01;
 103   }
 104   if (int numc = dCollide (o1,o2,MAX_CONTACTS,&contact[0].geom,
 105                            sizeof(dContact))) {
 106     dMatrix3 RI;
 107     dRSetIdentity (RI);
 108     const dReal ss[3] = {0.02,0.02,0.02};
 109     for (i=0; i<numc; i++) {
 110                 if (dGeomGetClass(o1) == dRayClass || dGeomGetClass(o2) == dRayClass){
 111                         dMatrix3 Rotation;
 112                         dRSetIdentity(Rotation);
 113                         dsDrawSphere(contact[i].geom.pos, Rotation, REAL(0.01));
 114
 115                         dVector3 End;
 116                         End[0] = contact[i].geom.pos[0] + (contact[i].geom.normal[0] * contact[i].geom.depth);
 117                         End[1] = contact[i].geom.pos[1] + (contact[i].geom.normal[1] * contact[i].geom.depth);
 118                         End[2] = contact[i].geom.pos[2] + (contact[i].geom.normal[2] * contact[i].geom.depth);
 119                         End[3] = contact[i].geom.pos[3] + (contact[i].geom.normal[3] * contact[i].geom.depth);
 120
 121                         dsDrawLine(contact[i].geom.pos, End);
 122                         continue;
 123                 }
 124
 125       dJointID c = dJointCreateContact (world,contactgroup,contact+i);
 126       dJointAttach (c,b1,b2);
 127       if (show_contacts) dsDrawBox (contact[i].geom.pos,RI,ss);
 128     }
 129   }
 130 }
 131
 132
 133 // start simulation - set viewpoint
 134
 135 static void start()
 136 {
 137   dAllocateODEDataForThread(dAllocateMaskAll);
 138
 139   static float xyz[3] = {2.1640f,-1.3079f,1.7600f};
 140   static float hpr[3] = {125.5000f,-17.0000f,0.0000f};
 141   dsSetViewpoint (xyz,hpr);
 142   printf ("To drop another object, press:\n");
 143   printf ("   b for box.\n");
 144   printf ("   s for sphere.\n");
 145   printf ("   c for cylinder.\n");
 146   printf ("   x for a composite object.\n");
 147   printf ("To select an object, press space.\n");
 148   printf ("To disable the selected object, press d.\n");
 149   printf ("To enable the selected object, press e.\n");
 150   printf ("To toggle showing the geom AABBs, press a.\n");
 151   printf ("To toggle showing the contact points, press t.\n");
 152   printf ("To toggle dropping from random position/orientation, press r.\n");
 153 }
 154
 155
 156 char locase (char c)
 157 {
 158   if (c >= 'A' && c <= 'Z') return c - ('a'-'A');
 159   else return c;
 160 }
 161
 162
 163 // called when a key pressed
 164
 165 static void command (int cmd)
 166 {
 167   int i,j,k;
 168   dReal sides[3];
 169   dMass m;
 170
 171   cmd = locase (cmd);
 172   if (cmd == 'b' || cmd == 's' || cmd == 'c' || cmd == 'x'
 173       /* || cmd == 'l' */) {
 174     if (num < NUM) {
 175       i = num;
 176       num++;
 177     }
 178     else {
 179       i = nextobj;
 180       nextobj++;
 181       if (nextobj >= num) nextobj = 0;
 182
 183       // destroy the body and geoms for slot i
 184       dBodyDestroy (obj[i].body);
 185       for (k=0; k < GPB; k++) {
 186         if (obj[i].geom[k]) dGeomDestroy (obj[i].geom[k]);
 187       }
 188       memset (&obj[i],0,sizeof(obj[i]));
 189     }
 190
 191     obj[i].body = dBodyCreate (world);
 192     for (k=0; k<3; k++) sides[k] = dRandReal()*0.5+0.1;
 193
 194     dMatrix3 R;
 195     if (random_pos) {
 196       dBodySetPosition (obj[i].body,
 197                         dRandReal()*2-1,dRandReal()*2-1,dRandReal()+1);
 198       dRFromAxisAndAngle (R,dRandReal()*2.0-1.0,dRandReal()*2.0-1.0,
 199                           dRandReal()*2.0-1.0,dRandReal()*10.0-5.0);
 200     }
 201     else {
 202       dReal maxheight = 0;
 203       for (k=0; k<num; k++) {
 204         const dReal *pos = dBodyGetPosition (obj[k].body);
 205         if (pos[2] > maxheight) maxheight = pos[2];
 206       }
 207       dBodySetPosition (obj[i].body, 0,0,maxheight+1);
 208       dRFromAxisAndAngle (R,0,0,1,dRandReal()*10.0-5.0);
 209     }
 210     dBodySetRotation (obj[i].body,R);
 211     dBodySetData (obj[i].body,(void*)(size_t)i);
 212
 213     if (cmd == 'b') {
 214       dMassSetBox (&m,DENSITY,sides[0],sides[1],sides[2]);
 215       obj[i].geom[0] = dCreateBox (space,sides[0],sides[1],sides[2]);
 216     }
 217     else if (cmd == 'c') {
 218       sides[0] *= 0.5;
 219       dMassSetCapsule (&m,DENSITY,3,sides[0],sides[1]);
 220       obj[i].geom[0] = dCreateCapsule (space,sides[0],sides[1]);
 221     }
 222 /*
 223     // cylinder option not yet implemented
 224     else if (cmd == 'l') {
 225       sides[1] *= 0.5;
 226       dMassSetCapsule (&m,DENSITY,3,sides[0],sides[1]);
 227       obj[i].geom[0] = dCreateCylinder (space,sides[0],sides[1]);
 228     }
 229 */
 230     else if (cmd == 's') {
 231       sides[0] *= 0.5;
 232       dMassSetSphere (&m,DENSITY,sides[0]);
 233       obj[i].geom[0] = dCreateSphere (space,sides[0]);
 234     }
 235     else if (cmd == 'x') {
 236       dGeomID g2[GPB];          // encapsulated geometries
 237       dReal dpos[GPB][3];       // delta-positions for encapsulated geometries
 238
 239       // start accumulating masses for the encapsulated geometries
 240       dMass m2;
 241       dMassSetZero (&m);
 242
 243       // set random delta positions
 244       for (j=0; j<GPB; j++) {
 245         for (k=0; k<3; k++) dpos[j][k] = dRandReal()*0.3-0.15;
 246       }
 247
 248       for (k=0; k<GPB; k++) {
 249         obj[i].geom[k] = dCreateGeomTransform (space);
 250         dGeomTransformSetCleanup (obj[i].geom[k],1);
 251         if (k==0) {
 252           dReal radius = dRandReal()*0.25+0.05;
 253           g2[k] = dCreateSphere (0,radius);
 254           dMassSetSphere (&m2,DENSITY,radius);
 255         }
 256         else if (k==1) {
 257           g2[k] = dCreateBox (0,sides[0],sides[1],sides[2]);
 258           dMassSetBox (&m2,DENSITY,sides[0],sides[1],sides[2]);
 259         }
 260         else {
 261           dReal radius = dRandReal()*0.1+0.05;
 262           dReal length = dRandReal()*1.0+0.1;
 263           g2[k] = dCreateCapsule (0,radius,length);
 264           dMassSetCapsule (&m2,DENSITY,3,radius,length);
 265         }
 266         dGeomTransformSetGeom (obj[i].geom[k],g2[k]);
 267
 268         // set the transformation (adjust the mass too)
 269         dGeomSetPosition (g2[k],dpos[k][0],dpos[k][1],dpos[k][2]);
 270         dMassTranslate (&m2,dpos[k][0],dpos[k][1],dpos[k][2]);
 271         dMatrix3 Rtx;
 272         dRFromAxisAndAngle (Rtx,dRandReal()*2.0-1.0,dRandReal()*2.0-1.0,
 273                             dRandReal()*2.0-1.0,dRandReal()*10.0-5.0);
 274         dGeomSetRotation (g2[k],Rtx);
 275         dMassRotate (&m2,Rtx);
 276
 277         // add to the total mass
 278         dMassAdd (&m,&m2);
 279       }
 280
 281       // move all encapsulated objects so that the center of mass is (0,0,0)
 282       for (k=0; k<2; k++) {
 283         dGeomSetPosition (g2[k],
 284                           dpos[k][0]-m.c[0],
 285                           dpos[k][1]-m.c[1],
 286                           dpos[k][2]-m.c[2]);
 287       }
 288       dMassTranslate (&m,-m.c[0],-m.c[1],-m.c[2]);
 289     }
 290
 291     for (k=0; k < GPB; k++) {
 292       if (obj[i].geom[k]) dGeomSetBody (obj[i].geom[k],obj[i].body);
 293     }
 294
 295     dBodySetMass (obj[i].body,&m);
 296   }
 297
 298   if (cmd == ' ') {
 299     selected++;
 300     if (selected >= num) selected = 0;
 301     if (selected < 0) selected = 0;
 302   }
 303   else if (cmd == 'd' && selected >= 0 && selected < num) {
 304     dBodyDisable (obj[selected].body);
 305   }
 306   else if (cmd == 'e' && selected >= 0 && selected < num) {
 307     dBodyEnable (obj[selected].body);
 308   }
 309   else if (cmd == 'a') {
 310     show_aabb ^= 1;
 311   }
 312   else if (cmd == 't') {
 313     show_contacts ^= 1;
 314   }
 315   else if (cmd == 'r') {
 316     random_pos ^= 1;
 317   }
 318 }
 319
 320
 321 // draw a geom
 322
 323 void drawGeom (dGeomID g, const dReal *pos, const dReal *R, int show_aabb)
 324 {
 325   if (!g) return;
 326   if (!pos) pos = dGeomGetPosition (g);
 327   if (!R) R = dGeomGetRotation (g);
 328
 329   int type = dGeomGetClass (g);
 330   if (type == dBoxClass) {
 331     dVector3 sides;
 332     dGeomBoxGetLengths (g,sides);
 333     dsDrawBox (pos,R,sides);
 334   }
 335   else if (type == dSphereClass) {
 336     dsDrawSphere (pos,R,dGeomSphereGetRadius (g));
 337   }
 338   else if (type == dCapsuleClass) {
 339     dReal radius,length;
 340     dGeomCapsuleGetParams (g,&radius,&length);
 341     dsDrawCapsule (pos,R,length,radius);
 342   }
 343 /*
 344   // cylinder option not yet implemented
 345   else if (type == dCylinderClass) {
 346     dReal radius,length;
 347     dGeomCylinderGetParams (g,&radius,&length);
 348     dsDrawCylinder (pos,R,length,radius);
 349   }
 350 */
 351   else if (type == dGeomTransformClass) {
 352     dGeomID g2 = dGeomTransformGetGeom (g);
 353     const dReal *pos2 = dGeomGetPosition (g2);
 354     const dReal *R2 = dGeomGetRotation (g2);
 355     dVector3 actual_pos;
 356     dMatrix3 actual_R;
 357     dMultiply0_331 (actual_pos,R,pos2);
 358     actual_pos[0] += pos[0];
 359     actual_pos[1] += pos[1];
 360     actual_pos[2] += pos[2];
 361     dMultiply0_333 (actual_R,R,R2);
 362     drawGeom (g2,actual_pos,actual_R,0);
 363   }
 364
 365   if (show_aabb) {
 366     // draw the bounding box for this geom
 367     dReal aabb[6];
 368     dGeomGetAABB (g,aabb);
 369     dVector3 bbpos;
 370     for (int i=0; i<3; i++) bbpos[i] = 0.5*(aabb[i*2] + aabb[i*2+1]);
 371     dVector3 bbsides;
 372     for (int j=0; j<3; j++) bbsides[j] = aabb[j*2+1] - aabb[j*2];
 373     dMatrix3 RI;
 374     dRSetIdentity (RI);
 375     dsSetColorAlpha (1,0,0,0.5);
 376     dsDrawBox (bbpos,RI,bbsides);
 377   }
 378 }
 379
 380
 381 // simulation loop
 382
 383 static void simLoop (int pause)
 384 {
 385   dsSetColor (0,0,2);
 386   dSpaceCollide (space,0,&nearCallback);
 387   if (!pause) dWorldStep (world,0.05);
 388   //if (!pause) dWorldStepFast (world,0.05, 1);
 389
 390   // remove all contact joints
 391   dJointGroupEmpty (contactgroup);
 392
 393   dsSetColor (1,1,0);
 394   dsSetTexture (DS_WOOD);
 395   for (int i=0; i<num; i++) {
 396     for (int j=0; j < GPB; j++) {
 397       if (i==selected) {
 398         dsSetColor (0,0.7,1);
 399       }
 400       else if (! dBodyIsEnabled (obj[i].body)) {
 401         dsSetColor (1,0,0);
 402       }
 403       else {
 404         dsSetColor (1,1,0);
 405       }
 406       drawGeom (obj[i].geom[j],0,0,show_aabb);
 407     }
 408   }
 409
 410   /*{
 411     for (int i = 1; i < IndexCount; i++) {
 412       dsDrawLine(Vertices[Indices[i - 1]], Vertices[Indices[i]]);
 413     }
 414   }*/
 415
 416   {const dReal* Pos = dGeomGetPosition(TriMesh);
 417   const dReal* Rot = dGeomGetRotation(TriMesh);
 418
 419   {for (int i = 0; i < IndexCount / 3; i++){
 420     const float *p = Vertices[Indices[i * 3 + 0]];
 421     const dVector3 v0 = { p[0], p[1], p[2] };
 422     p = Vertices[Indices[i * 3 + 1]];
 423     const dVector3 v1 = { p[0], p[1], p[2] };
 424     p = Vertices[Indices[i * 3 + 2]];
 425     const dVector3 v2 = { p[0], p[1], p[2] };
 426     dsDrawTriangle(Pos, Rot, v0, v1, v2, 0);
 427   }}}
 428
 429   if (Ray){
 430           dVector3 Origin, Direction;
 431           dGeomRayGet(Ray, Origin, Direction);
 432
 433           dReal Length = dGeomRayGetLength(Ray);
 434
 435           dVector3 End;
 436           End[0] = Origin[0] + (Direction[0] * Length);
 437           End[1] = Origin[1] + (Direction[1] * Length);
 438           End[2] = Origin[2] + (Direction[2] * Length);
 439           End[3] = Origin[3] + (Direction[3] * Length);
 440
 441           dsDrawLine(Origin, End);
 442   }
 443 }
 444
 445
 446 int main (int argc, char **argv)
 447 {
 448   // setup pointers to drawstuff callback functions
 449   dsFunctions fn;
 450   fn.version = DS_VERSION;
 451   fn.start = &start;
 452   fn.step = &simLoop;
 453   fn.command = &command;
 454   fn.stop = 0;
 455   fn.path_to_textures = DRAWSTUFF_TEXTURE_PATH;
 456
 457   // create world
 458   dInitODE2(0);
 459   world = dWorldCreate();
 460
 461   space = dSimpleSpaceCreate(0);
 462   contactgroup = dJointGroupCreate (0);
 463   dWorldSetGravity (world,0,0,-0.5);
 464   dWorldSetCFM (world,1e-5);
 465   //dCreatePlane (space,0,0,1,0);
 466   memset (obj,0,sizeof(obj));
 467
 468   Size[0] = 5.0f;
 469   Size[1] = 5.0f;
 470   Size[2] = 2.5f;
 471
 472   Vertices[0][0] = -Size[0];
 473   Vertices[0][1] = -Size[1];
 474   Vertices[0][2] = Size[2];
 475
 476   Vertices[1][0] = Size[0];
 477   Vertices[1][1] = -Size[1];
 478   Vertices[1][2] = Size[2];
 479
 480   Vertices[2][0] = Size[0];
 481   Vertices[2][1] = Size[1];
 482   Vertices[2][2] = Size[2];
 483
 484   Vertices[3][0] = -Size[0];
 485   Vertices[3][1] = Size[1];
 486   Vertices[3][2] = Size[2];
 487
 488   Vertices[4][0] = 0;
 489   Vertices[4][1] = 0;
 490   Vertices[4][2] = 0;
 491
 492   Indices[0] = 0;
 493   Indices[1] = 1;
 494   Indices[2] = 4;
 495
 496   Indices[3] = 1;
 497   Indices[4] = 2;
 498   Indices[5] = 4;
 499
 500   Indices[6] = 2;
 501   Indices[7] = 3;
 502   Indices[8] = 4;
 503
 504   Indices[9] = 3;
 505   Indices[10] = 0;
 506   Indices[11] = 4;
 507
 508   dTriMeshDataID Data = dGeomTriMeshDataCreate();
 509
 510   //dGeomTriMeshDataBuildSimple(Data, (dReal*)Vertices, VertexCount, Indices, IndexCount);
 511   dGeomTriMeshDataBuildSingle(Data, Vertices[0], 3 * sizeof(float), VertexCount, &Indices[0], IndexCount, 3 * sizeof(dTriIndex));
 512
 513   TriMesh = dCreateTriMesh(space, Data, 0, 0, 0);
 514
 515   //dGeomSetPosition(TriMesh, 0, 0, 1.0);
 516
 517   Ray = dCreateRay(space, 0.9);
 518   dVector3 Origin, Direction;
 519   Origin[0] = 0.0;
 520   Origin[1] = 0;
 521   Origin[2] = 0.5;
 522   Origin[3] = 0;
 523
 524   Direction[0] = 0;
 525   Direction[1] = 1.1f;
 526   Direction[2] = -1;
 527   Direction[3] = 0;
 528   dNormalize3(Direction);
 529
 530   dGeomRaySet(Ray, Origin[0], Origin[1], Origin[2], Direction[0], Direction[1], Direction[2]);
 531
 532   dThreadingImplementationID threading = dThreadingAllocateMultiThreadedImplementation();
 533   dThreadingThreadPoolID pool = dThreadingAllocateThreadPool(4, 0, dAllocateFlagBasicData, NULL);
 534   dThreadingThreadPoolServeMultiThreadedImplementation(pool, threading);
 535   // dWorldSetStepIslandsProcessingMaxThreadCount(world, 1);
 536   dWorldSetStepThreadingImplementation(world, dThreadingImplementationGetFunctions(threading), threading);
 537
 538   // run simulation
 539   dsSimulationLoop (argc,argv,352,288,&fn);
 540
 541   dThreadingImplementationShutdownProcessing(threading);
 542   dThreadingFreeThreadPool(pool);
 543   dWorldSetStepThreadingImplementation(world, NULL, NULL);
 544   dThreadingFreeImplementation(threading);
 545
 546   dJointGroupDestroy (contactgroup);
 547   dSpaceDestroy (space);
 548   dWorldDestroy (world);
 549   dCloseODE();
 550   return 0;
 551 }