ode/demo/demo_trimesh_collision.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  // TriMesh collision demo.
  24  // Serves as a test for the collision of trimesh geometries.
  25  // By Davy (Dawei) Chen.
  26
  27 #include <assert.h>
  28 #ifdef HAVE_UNISTD_H
  29 #       include <unistd.h>
  30 #endif
  31 #include <ode/ode.h>
  32 #include <drawstuff/drawstuff.h>
  33 #include "texturepath.h"
  34
  35 #ifdef dDOUBLE
  36 #define dsDrawTriangle dsDrawTriangleD
  37 #endif
  38 #define dGeomTriMeshDataBuildReal dGeomTriMeshDataBuildSingle
  39
  40 #ifdef _MSC_VER
  41 #       pragma warning(disable:4244 4305)  // for VC++, no precision loss complaints
  42 #endif
  43
  44
  45 static dWorldID world;
  46 static dSpaceID space;
  47 static dJointGroupID contactgroup;
  48
  49 // 2D Convex hulls to create the TriMesh geometries
  50 const float HULL1[] = { -1.000000, -35.000000, 1.000000, -34.000000, 5.000000, -31.000000, 16.000000, -20.000000, 16.000000, -11.000000, 5.000000, 0.000000, -1.000000, 3.000000, -7.000000, 4.000000, -13.000000, 3.000000, -16.000000, 2.000000, -20.000000, 0.000000, -24.000000, -4.000000, -26.000000, -8.000000, -27.000000, -12.000000, -27.000000, -15.000000, -27.000000, -19.000000, -26.000000, -23.000000, -24.000000, -27.000000, -20.000000, -31.000000, -16.000000, -34.000000 };
  51 const float HULL2[] = { 23.000000, 28.000000, 28.000000, 29.000000, 30.000000, 30.000000, 34.000000, 33.000000, 35.000000, 34.000000, 38.000000, 39.000000, 39.000000, 42.000000, 40.000000, 48.000000, 39.000000, 53.000000, 38.000000, 56.000000, 35.000000, 61.000000, 34.000000, 62.000000, 30.000000, 65.000000, 28.000000, 66.000000, 25.000000, 67.000000, 20.000000, 68.000000, 19.000000, 68.000000, 14.000000, 67.000000, 11.000000, 66.000000, 9.000000, 65.000000, 5.000000, 62.000000, 4.000000, 61.000000, 1.000000, 56.000000, 0.000000, 53.000000, 0.000000, 48.000000, 0.000000, 42.000000, 1.000000, 39.000000, 4.000000, 34.000000, 5.000000, 33.000000, 9.000000, 30.000000, 11.000000, 29.000000 };
  52 const float HULL3[] = { -1.000000, -35.000000, 1.000000, -34.000000, 5.000000, -31.000000, 8.000000, -28.000000, 11.000000, -22.000000, 12.000000, -19.000000, 12.000000, -12.000000, 11.000000, -9.000000, 8.000000, -3.000000, 5.000000, 0.000000, -1.000000, 3.000000, -7.000000, 4.000000, -13.000000, 3.000000, -16.000000, 2.000000, -20.000000, 0.000000, -23.000000, -3.000000, -25.000000, -6.000000, -26.000000, -8.000000, -27.000000, -11.000000, -27.000000, -15.000000, -27.000000, -20.000000, -26.000000, -23.000000, -25.000000, -25.000000, -23.000000, -28.000000, -20.000000, -31.000000, -16.000000, -34.000000 };
  53 const float HULL4[] = { -2.000000, -35.000000, 6.000000, -31.000000, 9.000000, -27.000000, 10.000000, -25.000000, 11.000000, -22.000000, 11.000000, -9.000000, 10.000000, -6.000000, 9.000000, -4.000000, 6.000000, 0.000000, -2.000000, 3.000000, -7.000000, 4.000000, -19.000000, 3.000000, -20.000000, 2.000000, -25.000000, -6.000000, -26.000000, -8.000000, -27.000000, -11.000000, -27.000000, -15.000000, -27.000000, -20.000000, -26.000000, -23.000000, -25.000000, -25.000000, -20.000000, -34.000000 };
  54 const float HULL5[] = { -2.000000, -35.000000, 4.000000, -32.000000, 9.000000, -27.000000, 11.000000, -23.000000, 12.000000, -20.000000, 12.000000, -11.000000, 11.000000, -8.000000, 9.000000, -4.000000, 5.000000, 0.000000, -2.000000, 3.000000, -7.000000, 4.000000, -12.000000, 3.000000, -15.000000, 2.000000, -20.000000, 0.000000, -23.000000, -3.000000, -25.000000, -6.000000, -26.000000, -8.000000, -27.000000, -11.000000, -27.000000, -15.000000, -27.000000, -20.000000, -26.000000, -23.000000, -25.000000, -25.000000, -23.000000, -28.000000, -19.000000, -32.000000, -15.000000, -34.000000 };
  55 const float HULL6[] = { -1.000000, -35.000000, 25.000000, -29.000000, 26.000000, -28.000000, 30.000000, -18.000000, 30.000000, -14.000000, 26.000000, -6.000000, 25.000000, -5.000000, -1.000000, 3.000000, -7.000000, 4.000000, -13.000000, 3.000000, -20.000000, 0.000000, -23.000000, -3.000000, -25.000000, -6.000000, -26.000000, -8.000000, -27.000000, -12.000000, -27.000000, -15.000000, -27.000000, -19.000000, -26.000000, -23.000000, -25.000000, -25.000000, -23.000000, -28.000000, -20.000000, -31.000000, -17.000000, -33.000000 };
  56
  57 // Center points of the 2D Convex hulls
  58 const float CENTER1[] = { -5.500000, -15.500000 };
  59 const float CENTER2[] = { 20.000000, 48.000000 };
  60 const float CENTER3[] = { -7.500000, -15.500000 };
  61 const float CENTER4[] = { -8.000000, -15.500000 };
  62 const float CENTER5[] = { -7.500000, -15.500000 };
  63 const float CENTER6[] = { 1.500000, -15.500000 };
  64
  65 // Where to position the TriMeshes on the Ground plane
  66 // In (X, Y) coordinates pairs
  67 const float BODY_POSITIONS[][2] = {
  68     {-60.0f, -30.0f},
  69     {  0.0f, -30.0f},
  70     { 60.0f, -30.0f},
  71     {-60.0f,  30.0f},
  72     {  0.0f,  30.0f},
  73     { 60.0f,  30.0f}
  74 };
  75
  76 const float *HULLS[] = { HULL1, HULL2, HULL3, HULL4, HULL5, HULL6 };
  77 const int HULL_SIZES[] = {
  78     sizeof(HULL1) / sizeof(float) / 2,
  79     sizeof(HULL2) / sizeof(float) / 2,
  80     sizeof(HULL3) / sizeof(float) / 2,
  81     sizeof(HULL4) / sizeof(float) / 2,
  82     sizeof(HULL5) / sizeof(float) / 2,
  83     sizeof(HULL6) / sizeof(float) / 2
  84 };
  85 const float *CENTERS[] = { CENTER1, CENTER2, CENTER3, CENTER4, CENTER5, CENTER6 };
  86 const int HULLS_COUNT = sizeof(HULLS) / sizeof(float *);
  87
  88 const float TRIMESH_HEIGHT = 2.0f;
  89
  90 static float *odeVerts[HULLS_COUNT];
  91 static dTriIndex *odeInds[HULLS_COUNT];
  92 static int odeIndsCount[HULLS_COUNT];
  93
  94 static dTriMeshDataID triMeshDataId[HULLS_COUNT];
  95 static dGeomID triMeshId[HULLS_COUNT];
  96 static dBodyID bodyId[HULLS_COUNT];
  97
  98 // this is called by dSpaceCollide when two objects in space are
  99 // potentially colliding.
 100
 101 static void nearCallback(void *data, dGeomID o1, dGeomID o2)
 102 {
 103     assert(o1);
 104     assert(o2);
 105     if (dGeomIsSpace(o1) || dGeomIsSpace(o2))
 106     {
 107         // colliding a space with something
 108         dSpaceCollide2(o1, o2, data, &nearCallback);
 109         // Note we do not want to test intersections within a space,
 110         // only between spaces.
 111         return;
 112     }
 113
 114     const int N = 32;
 115     dContact contact[N];
 116     int n = dCollide(o1, o2, N, &(contact[0].geom), sizeof(dContact));
 117     if (n > 0)
 118     {
 119         for (int i = 0; i<n; i++)
 120         {
 121             contact[i].surface.slip1 = 0.7;
 122             contact[i].surface.slip2 = 0.7;
 123             contact[i].surface.mode = dContactSoftERP | dContactSoftCFM | dContactApprox1 | dContactSlip1 | dContactSlip2;
 124             // Friction effect, if set to dInfinity, objects would be unmovable
 125             contact[i].surface.mu = 0.0f;
 126             contact[i].surface.soft_erp = 0.50;
 127             contact[i].surface.soft_cfm = 0.03;
 128             dJointID c = dJointCreateContact(world, contactgroup, &contact[i]);
 129             dJointAttach
 130             (
 131                 c,
 132                 dGeomGetBody(contact[i].geom.g1),
 133                 dGeomGetBody(contact[i].geom.g2)
 134             );
 135         }
 136     }
 137 }
 138
 139 // start simulation - set viewpoint
 140
 141 static void start()
 142 {
 143     dAllocateODEDataForThread(dAllocateMaskAll);
 144     float xyz[3] = { -8,0,5 };
 145     float hpr[3] = { 0.0f,-29.5000f,0.0000f };
 146     dsSetViewpoint(xyz, hpr);
 147     fprintf(stderr, "Press SPACE to reset the simulation1.\n");
 148 }
 149
 150 static void reset()
 151 {
 152     for (int i = 0; i < HULLS_COUNT; i++)
 153     {
 154         dBodySetPosition(bodyId[i],
 155             BODY_POSITIONS[i][0] * 0.05,
 156             BODY_POSITIONS[i][1] * 0.05,
 157             0.0f);
 158
 159         dMatrix3 R;
 160         dRFromAxisAndAngle(R,
 161             1.0f,
 162             1.0f,
 163             1.0f,
 164             0.0f);
 165         dBodySetRotation(bodyId[i], R);
 166
 167         // Enable the body as it might have been auto-disabled
 168         dBodyEnable(bodyId[i]);
 169     }
 170 }
 171
 172 // called when a key pressed
 173
 174 static void command(int cmd)
 175 {
 176     switch (cmd)
 177     {
 178         case ' ':
 179             reset();
 180             break;
 181         default:
 182             break;
 183     }
 184 }
 185
 186 static void simLoop(int pause)
 187 {
 188     double simstep = 1/240.0;
 189     double dt = dsElapsedTime();
 190
 191     int nrofsteps = (int)ceilf(dt/simstep);
 192     nrofsteps = nrofsteps > 8 ? 8 : nrofsteps;
 193
 194     for (int i = 0; i<nrofsteps && !pause; i++)
 195     {
 196         // Add force to TriMesh bodies, and make them gather towards world center
 197         for (int j = 0; j < HULLS_COUNT; j++)
 198         {
 199             const dReal *pos = dBodyGetPosition(bodyId[j]);
 200             // Calculate force tensity according to distance from world center
 201             float length = dCalcVectorLengthSquare3(pos);
 202             dReal pos1[3];
 203             dCopyVector3(pos1, pos);
 204             dNegateVector3(pos1);
 205             dNormalize3(pos1);
 206             dScaleVector3(pos1, 5.0f * length);
 207
 208             dBodyAddForce(bodyId[j], pos1[0], pos1[1], pos1[2]);
 209         }
 210
 211         dSpaceCollide(space, 0, &nearCallback);
 212         dWorldQuickStep(world, simstep);
 213         dJointGroupEmpty(contactgroup);
 214     }
 215
 216     dsSetColor(1, 1, 1);
 217     for (int i = 0; i < HULLS_COUNT; i++)
 218     {
 219         const dReal *Pos = dBodyGetPosition(bodyId[i]);
 220         const dReal *Rot = dBodyGetRotation(bodyId[i]);
 221
 222         // Draw TriMeshes
 223         if (odeVerts)
 224         {
 225             for (int j = 0; j < odeIndsCount[i] / 3; j++)
 226             {
 227                 const dVector3 v0 = {
 228                     odeVerts[i][odeInds[i][j * 3 + 0] * 3],
 229                     odeVerts[i][odeInds[i][j * 3 + 0] * 3 + 1],
 230                     odeVerts[i][odeInds[i][j * 3 + 0] * 3 + 2]
 231                 };
 232                 const dVector3 v1 = {
 233                     odeVerts[i][odeInds[i][j * 3 + 1] * 3],
 234                     odeVerts[i][odeInds[i][j * 3 + 1] * 3 + 1],
 235                     odeVerts[i][odeInds[i][j * 3 + 1] * 3 + 2]
 236                 };
 237                 const dVector3 v2 = {
 238                     odeVerts[i][odeInds[i][j * 3 + 2] * 3],
 239                     odeVerts[i][odeInds[i][j * 3 + 2] * 3 + 1],
 240                     odeVerts[i][odeInds[i][j * 3 + 2] * 3 + 2]
 241                 };
 242                 dsDrawTriangle(Pos, Rot, v0, v1, v2, 1);
 243             }
 244         }
 245     }
 246 }
 247
 248 int main(int argc, char **argv)
 249 {
 250     dMass m;
 251
 252     // setup pointers to drawstuff callback functions
 253     dsFunctions fn;
 254     fn.version = DS_VERSION;
 255     fn.start = &start;
 256     fn.step = &simLoop;
 257     fn.command = &command;
 258     fn.stop = 0;
 259     fn.path_to_textures = DRAWSTUFF_TEXTURE_PATH;
 260
 261     // create world
 262     dInitODE2(0);
 263     world = dWorldCreate();
 264     space = dHashSpaceCreate(0);
 265     dHashSpaceSetLevels(space, -3, 5);
 266     dCreatePlane(space, 0, 0, 1, 0);    // Add a ground plane.
 267
 268     contactgroup = dJointGroupCreate(0);
 269     dWorldSetGravity(world, 0, 0, -1);
 270     dWorldSetQuickStepNumIterations(world, 32);
 271     dWorldSetContactMaxCorrectingVel(world, 40);
 272     dWorldSetMaxAngularSpeed(world, 62.8);
 273     dWorldSetERP(world, 0.7);
 274     dWorldSetQuickStepW(world, 0.75); // For increased stability.
 275
 276     dWorldSetAutoDisableFlag(world, true);
 277     dWorldSetAutoDisableLinearThreshold(world, 0.01);
 278     dWorldSetAutoDisableAngularThreshold(world, 0.03);
 279     dWorldSetAutoDisableTime(world, 0.15f);
 280
 281     // Generate TriMesh geometries and bodies
 282     for (int i = 0; i < HULLS_COUNT; i++)
 283     {
 284         int hullSize = HULL_SIZES[i];
 285         // Vertices
 286         int odeVertsCount1 = hullSize * 3 * 2;
 287         int odeVertsCount = odeVertsCount1 + 2 * 3;
 288         odeVerts[i] = new float[odeVertsCount];
 289         for (int j = 0; j < hullSize; j++)
 290         {
 291             // Bottom layer
 292             odeVerts[i][j * 3] = (HULLS[i][j * 2] - CENTERS[i][0]) * 0.05f;
 293             odeVerts[i][j * 3 + 1] = (HULLS[i][j * 2 + 1] - CENTERS[i][1]) * 0.05f;
 294             odeVerts[i][j * 3 + 2] = 0.0f;
 295             // Top layer
 296             odeVerts[i][(hullSize + j) * 3] = (HULLS[i][j * 2] - CENTERS[i][0]) * 0.05f;
 297             odeVerts[i][(hullSize + j) * 3 + 1] = (HULLS[i][j * 2 + 1] - CENTERS[i][1]) * 0.05f;
 298             odeVerts[i][(hullSize + j) * 3 + 2] = TRIMESH_HEIGHT;
 299         }
 300         // Center vertex on bottom plane
 301         odeVerts[i][odeVertsCount1] = 0.0f;
 302         odeVerts[i][odeVertsCount1 + 1] = 0.0f;
 303         odeVerts[i][odeVertsCount1 + 2] = 0.0f;
 304         // Center vertex on top plane
 305         odeVerts[i][odeVertsCount1 + 3] = 0.0f;
 306         odeVerts[i][odeVertsCount1 + 3 + 1] = 0.0f;
 307         odeVerts[i][odeVertsCount1 + 3 + 2] = TRIMESH_HEIGHT;
 308         // Indices
 309         int odeIndsCount1 = hullSize * 6;
 310         odeIndsCount[i] = odeIndsCount1 * 2;
 311         odeInds[i] = new dTriIndex[odeIndsCount[i]];
 312         for (int j = 0; j < hullSize; j++)
 313         {
 314             // Wall triangles
 315             // Wrap around index
 316             int n1 = j + 1 < hullSize ? j + 1 : 0;
 317             int n2 = hullSize + n1;
 318             odeInds[i][j * 6] = j;
 319             odeInds[i][j * 6 + 1] = n1;
 320             odeInds[i][j * 6 + 2] = hullSize + j;
 321             odeInds[i][j * 6 + 3] = hullSize + j;
 322             odeInds[i][j * 6 + 4] = n1;
 323             odeInds[i][j * 6 + 5] = n2;
 324             // Bottom and Top triangles
 325             odeInds[i][odeIndsCount1 + j * 6] = j;
 326             odeInds[i][odeIndsCount1 + j * 6 + 1] = n1;
 327             odeInds[i][odeIndsCount1 + j * 6 + 2] = hullSize * 2;
 328             odeInds[i][odeIndsCount1 + j * 6 + 3] = hullSize + j;
 329             odeInds[i][odeIndsCount1 + j * 6 + 4] = n2;
 330             odeInds[i][odeIndsCount1 + j * 6 + 5] = hullSize * 2 + 1;
 331         }
 332
 333         bodyId[i] = dBodyCreate(world);
 334         dBodySetPosition(bodyId[i],
 335             BODY_POSITIONS[i][0] * 0.05,
 336             BODY_POSITIONS[i][1] * 0.05,
 337             0.0f);
 338
 339         dMatrix3 R;
 340         dRFromAxisAndAngle(R,
 341             1.0f,
 342             1.0f,
 343             1.0f,
 344             0.0f);
 345         dBodySetRotation(bodyId[i], R);
 346
 347         size_t index = 0;
 348         dBodySetData(bodyId[i], (void *)index);
 349
 350         dMass m1;
 351         dReal sides[3];
 352         sides[0] = 3.0f;
 353         sides[1] = 3.0f;
 354         sides[2] = 3.0f;
 355         const float DENSITY = 1.0f;
 356         dMassSetBox(&m1, DENSITY, sides[0], sides[1], sides[2]);
 357
 358         dBodySetMass(bodyId[i], &m1);
 359         // Make bodies less bouncy
 360         dBodySetLinearDamping(bodyId[i], 0.1);
 361         dBodySetAngularDamping(bodyId[i], 0.1);
 362
 363         triMeshDataId[i] = dGeomTriMeshDataCreate();
 364         dGeomTriMeshDataBuildReal(triMeshDataId[i], odeVerts[i], 3 * sizeof(float), odeVertsCount, odeInds[i], odeIndsCount[i], 3 * sizeof(dTriIndex));
 365         dGeomTriMeshDataPreprocess2(triMeshDataId[i], (1U << dTRIDATAPREPROCESS_BUILD_FACE_ANGLES), NULL);
 366
 367         triMeshId[i] = dCreateTriMesh(space, triMeshDataId[i], 0, 0, 0);
 368         dGeomSetBody(triMeshId[i], bodyId[i]);
 369     }
 370
 371     // run simulation
 372     dsSimulationLoop(argc, argv, DS_SIMULATION_DEFAULT_WIDTH, DS_SIMULATION_DEFAULT_HEIGHT, &fn);
 373
 374     for (int i = 0; i < HULLS_COUNT; i++)
 375     {
 376         dGeomTriMeshDataDestroy(triMeshDataId[i]);
 377         dGeomDestroy(triMeshId[i]);
 378         dBodyDestroy(bodyId[i]);
 379
 380         delete[] odeVerts[i];
 381         odeVerts[i] = NULL;
 382         delete[] odeInds[i];
 383         odeInds[i] = NULL;
 384     }
 385
 386     dJointGroupEmpty(contactgroup);
 387     dJointGroupDestroy(contactgroup);
 388     dSpaceDestroy(space);
 389     dWorldDestroy(world);
 390     dCloseODE();
 391
 392     return 0;
 393 }