[ODE] Geomgroups vs composite

skjold@cistron.nl skjold at cistron.nl
Mon Feb 10 06:43:01 2003


As I indicated previously, I tinkered with the test_boxstack a bit, and for me that solved the crashing (which btw was not due to stack overflow but some access violation, but I haven't done extensive debugging on the exact cause of it). I include the new source below. Here are the most notable changes:

- I haven't seen it crash anymore :D
- Removed global MyObject array, changed the rest of the app accordingly (no bookkeeping is done anymore, at all, other than having the globals world, space and contactgroup).
- Composite objects are put in a new SimpleSpace - see compoundGeom(). Changed drawGeom to handle this (recursively calls itself to draw geoms in spaces now). Also changed nearCallback to handle spaces (is now called proximityCallback, it's a personal itch of mine to rename everything ;)
- Space aabb's are now also shown, as green transparent boxes.
- MAXCOMPOUNDGEOMS = 5 (was 3), MAXCONTACTS = 6 (was 4), these defines can be changed as desired (also fixed a few occurances where the MAXCOMPOUNDGEOMS should have been used, i.s.o. just '3').
- Optimized proximityCallback a bit, by initializing the contact array only once (as a static local array).
- Maybe some other minor things I forgot about.

Hope it works :) By the way, I compile from the MSVC 6.0 IDE, but despite the pragma to disable some warnings I still get those warnings... Ah well. OH, and for completeness: I set my stacksize to 16MB :)

Here goes:



/*************************************************************************
 *                                                                       *
 * 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.                     *
 *                                                                       *
 *************************************************************************/


#include <ode/ode.h>
#include <drawstuff/drawstuff.h>

#ifdef MSVC
#pragma warning(disable:4244 4305)  // for VC++, no precision loss complaints
#endif

#ifdef dDOUBLE
#define dsDrawBox dsDrawBoxD
#define dsDrawSphere dsDrawSphereD
#define dsDrawCylinder dsDrawCylinderD
#define dsDrawCappedCylinder dsDrawCappedCylinderD
#endif

#define DENSITY (1.0)      // default density
#define MAXCONTACTS (6)    // max contact joints per collision pair
#define MAXCOMPOUNDGEOMS (5)

static int show_aabb = 0;	// show geom AABBs?
static int show_contacts = 0;	// show contact points?
static int random_pos = 1;	// drop objects from random position?

static dWorldID world;
static dSpaceID space;
static dJointGroupID contactgroup;


static void proximityCallback (void *data, dGeomID o1, dGeomID o2) {

    // Recurse inter/intra-space collisions

    if(dGeomIsSpace(o1)) {
        dSpaceCollide((dSpaceID)o1, data, &proximityCallback); // Collide space internally
        dSpaceCollide2(o1, o2, data, &proximityCallback); // Collide space with the other

    } else if(dGeomIsSpace(o2)) {
        dSpaceCollide((dSpaceID)o2, data, &proximityCallback); // Collide space internally
        dSpaceCollide2(o1, o2, data, &proximityCallback); // Collide space with the other

    } else {

        // Handle potential collision of two geometries
        int i;
        dBodyID lBody1 = dGeomGetBody(o1);
        dBodyID lBody2 = dGeomGetBody(o2);

        // Are both geometries from the same body (composite)?
        if(lBody1 == lBody2) {puts("Bingo!"); return;} // @@@NOTE: Never gets here...

        // Are the two bodies already connected by a normal joint?
        if(lBody1 && lBody2 && dAreConnectedExcluding(lBody1, lBody2, dJointTypeContact)) return;

        static dContact lContacts[MAXCONTACTS];
        static int initialized = 0;
        if(!initialized) { // One-time initialization of static contacts array
            for (i = 0; i < MAXCONTACTS; i++) {
                lContacts[i].surface.mode = dContactSoftCFM | dContactSoftERP | dContactBounce;
                lContacts[i].surface.mu = dInfinity;
                lContacts[i].surface.mu2 = 0;
                lContacts[i].surface.bounce = 0.05;
                lContacts[i].surface.bounce_vel = 0.1;
                lContacts[i].surface.soft_cfm = 0.01;
                lContacts[i].surface.soft_erp = 0.4;
            }
            initialized++;
        }
 
        int lNumContacts = dCollide (o1, o2, MAXCONTACTS, &lContacts[0].geom, sizeof(dContact));
        if(lNumContacts > 0) {
            for(i = 0; i < lNumContacts; i++) {
                dJointID lContactJoint = dJointCreateContact(world, contactgroup, &lContacts[i]);
                dJointAttach(lContactJoint, lBody1,lBody2);
                if(show_contacts) {
                    dMatrix3 RI;
                    dRSetIdentity(RI);
                    const dReal ss[3] = {0.02,0.02,0.02};
                    dsDrawBox(lContacts[i].geom.pos, RI, ss);
                }
            }
        }
    }
}

static void compoundGeom(
    dBodyID aBodyID,
    dSpaceID aSpaceID,
    int aNumGeoms,
    dGeomID* aGeoms,
    dMass* aMasses,
    dVector3* aPos,
    dMatrix3* aRot) {

    int i;
    dGeomID lXgeoms[MAXCOMPOUNDGEOMS]; // @@@NOTE: Hardcoded max number of components!

    dMass lMass;
    dMassSetZero (&lMass);

    dSpaceID lSimpleSpaceID = dSimpleSpaceCreate(aSpaceID); // Put into new space

    for(i = 0; i < aNumGeoms; i++) {
        // Encapsulate the component into a transform-geometry
        lXgeoms[i] = dCreateGeomTransform(lSimpleSpaceID);
        dGeomTransformSetCleanup(lXgeoms[i], 1);
        dGeomTransformSetGeom(lXgeoms[i], aGeoms[i]);

        // Set the transformation (adjust the mass too)
        dGeomSetPosition(aGeoms[i], aPos[i][0],aPos[i][1],aPos[i][2]);
        dMassTranslate(&aMasses[i],aPos[i][0],aPos[i][1],aPos[i][2]);
        dGeomSetRotation(aGeoms[i],aRot[i]);
        dMassRotate(&aMasses[i], aRot[i]);

        // add to the total mass
        dMassAdd (&lMass,&aMasses[i]);
    }

    // move all encapsulated objects so that the center of mass is (0,0,0)
    for(i = 0; i < aNumGeoms; i++) {
        dGeomSetPosition (aGeoms[i],
            aPos[i][0] - lMass.c[0],
            aPos[i][1] - lMass.c[1],
            aPos[i][2] - lMass.c[2]);
    }
    dMassTranslate (&lMass,-lMass.c[0],-lMass.c[1],-lMass.c[2]);

    for(i = 0; i < aNumGeoms; i++) {
        dGeomSetBody(lXgeoms[i], aBodyID);
    }
    dBodySetMass(aBodyID, &lMass);
}

static void start() {
  static float xyz[3] = {0,-10,5};
  static float hpr[3] = {90,-20,0};
  dsSetViewpoint (xyz,hpr);
  //dsSetSphereQuality(2);
  //dsSetCappedCylinderQuality(4);
}

char lowercase (char c) {
  if (c >= 'A' && c <= 'Z') return c - ('a'-'A');
  else return c;
}

static void command (int cmd) {

    cmd = lowercase (cmd);

    if (cmd == 'b' || cmd == 's' || cmd == 'c' || cmd == 'x') {
        int j,k;
        dReal sides[3];
        dMass m;
        dBodyID lBodyID = dBodyCreate (world);
        for (k=0; k<3; k++) sides[k] = dRandReal()+0.1;

        dMatrix3 R;
        if(random_pos) {
            dBodySetPosition(lBodyID, dRandReal()*10-5, dRandReal()*10-5, dRandReal()+5);
            dRFromAxisAndAngle(R, dRandReal()*2.0-1.0, dRandReal()*2.0-1.0, dRandReal()*2.0-1.0, dRandReal()*10.0-5.0);
        }
        else {
            dReal maxheight = 5;
            dBodySetPosition (lBodyID, 0,0,maxheight+1);
            dRFromAxisAndAngle (R, 0, 0, 1, dRandReal()*10.0-5.0);
        }

        dBodySetRotation (lBodyID,R);
        //dBodySetData(lBodyID,(void*) i);

        dGeomID lGeomID = 0;
        if (cmd == 'b') {
            dMassSetBox (&m,DENSITY,sides[0],sides[1],sides[2]);
            lGeomID = dCreateBox (space,sides[0],sides[1],sides[2]);
            dGeomSetBody(lGeomID, lBodyID);
            dBodySetMass (lBodyID,&m);
        } else if (cmd == 'c') {
            sides[0] *= 0.5;
            dMassSetCappedCylinder (&m,DENSITY,3,sides[0],sides[1]);
            lGeomID = dCreateCCylinder (space,sides[0],sides[1]);
            dGeomSetBody(lGeomID, lBodyID);
            dBodySetMass (lBodyID,&m);
        } else if (cmd == 's') {
            sides[0] *= 0.5;
            dMassSetSphere (&m,DENSITY,sides[0]);
            lGeomID = dCreateSphere (space,sides[0]);
            dGeomSetBody(lGeomID, lBodyID);
            dBodySetMass (lBodyID,&m);
        }
        else if (cmd == 'x') {
            dGeomID geoms[MAXCOMPOUNDGEOMS];
            dVector3 dpos[MAXCOMPOUNDGEOMS];
            dMatrix3 rot[MAXCOMPOUNDGEOMS];
            dMass masses[MAXCOMPOUNDGEOMS];

            // set random delta positions
            for (j=0; j<MAXCOMPOUNDGEOMS; j++) {
                for (k=0; k<3; k++) dpos[j][k] = dRandReal()*0.5-0.25;
            }

            for (k=0; k<MAXCOMPOUNDGEOMS; k++) {
                if (k==0) {
                    sides[0] *= 0.5;
                    geoms[k] = dCreateSphere (0,sides[0]);
                    dMassSetSphere (&masses[k],DENSITY,sides[0]);
                } else if (k==1) {
                    geoms[k] = dCreateBox (0,sides[0],sides[1],sides[2]);
                    dMassSetBox (&masses[k],DENSITY,sides[0],sides[1],sides[2]);
                } else {
                    sides[0] *= 0.5;
                    dReal radius = dRandReal()*0.1+0.05;
                    dReal length = dRandReal()*1.0+0.1;
                    geoms[k] = dCreateCCylinder (0,sides[0],sides[1]);
                    dMassSetCappedCylinder (&masses[k],DENSITY,3,sides[0],sides[1]);
                }

                dRFromAxisAndAngle (rot[k],dRandReal()*2.0-1.0,dRandReal()*2.0-1.0,
                    dRandReal()*2.0-1.0,dRandReal()*10.0-5.0);
            }
            compoundGeom(lBodyID, space, MAXCOMPOUNDGEOMS, geoms, masses, dpos, rot);
        }

        // @@@NOTE: If composites are put into a new space, it only counts as 1 geom here!
        printf("Geometries in simulation: %d\n", dSpaceGetNumGeoms(space));
    }

  if (cmd == 'a') {
    show_aabb ^= 1;
  }
  else if (cmd == 't') {
    show_contacts ^= 1;
  }
  else if (cmd == 'r') {
    random_pos ^= 1;
  }
}

// draw a geom
void drawGeom (dGeomID g, const dReal *pos, const dReal *R, int show_aabb)
{
  if (!g) return;

  // Recursively draw geoms in spaces
  if(dGeomIsSpace(g)) {
    for (int i = 0; i < dSpaceGetNumGeoms((dSpaceID)g); i++) {
        //drawGeom (obj[i].geom[j],0,0,show_aabb);
        drawGeom (dSpaceGetGeom((dSpaceID)g, i),0,0,show_aabb);
    }
    if (show_aabb) {
      // draw the bounding box for this space
      dReal aabb[6];
      dGeomGetAABB (g,aabb);
      dVector3 bbpos;
      for (int i=0; i<3; i++) bbpos[i] = 0.5*(aabb[i*2] + aabb[i*2+1]);
      dVector3 bbsides;
      for (i=0; i<3; i++) bbsides[i] = aabb[i*2+1] - aabb[i*2];
      dMatrix3 RI;
      dRSetIdentity (RI);
      dsSetColorAlpha (0,1,0,0.5);
      dsSetTexture (DS_NONE);
      dsDrawBox (bbpos,RI,bbsides);
      dsSetTexture (DS_WOOD);
    }
    return;
  }

  if (!pos) pos = dGeomGetPosition (g);
  if (!R) R = dGeomGetRotation (g);

  int type = dGeomGetClass (g);
  if (type == dBoxClass) {
    dVector3 sides;
    dGeomBoxGetLengths (g,sides);
    dsDrawBox (pos,R,sides);
  }
  else if (type == dSphereClass) {
    dsDrawSphere (pos,R,dGeomSphereGetRadius (g));
  }
  else if (type == dCCylinderClass) {
    dReal radius,length;
    dGeomCCylinderGetParams (g,&radius,&length);
    dsDrawCappedCylinder (pos,R,length,radius);
  }
  else if (type == dGeomTransformClass) {
    dGeomID g2 = dGeomTransformGetGeom (g);
    const dReal *pos2 = dGeomGetPosition (g2);
    const dReal *R2 = dGeomGetRotation (g2);
    dVector3 actual_pos;
    dMatrix3 actual_R;
    dMULTIPLY0_331 (actual_pos,R,pos2);
    actual_pos[0] += pos[0];
    actual_pos[1] += pos[1];
    actual_pos[2] += pos[2];
    dMULTIPLY0_333 (actual_R,R,R2);
    drawGeom (g2,actual_pos,actual_R,0);
  }

  if (show_aabb) {
    // draw the bounding box for this geom
    dReal aabb[6];
    dGeomGetAABB (g,aabb);
    dVector3 bbpos;
    for (int i=0; i<3; i++) bbpos[i] = 0.5*(aabb[i*2] + aabb[i*2+1]);
    dVector3 bbsides;
    for (i=0; i<3; i++) bbsides[i] = aabb[i*2+1] - aabb[i*2];
    dMatrix3 RI;
    dRSetIdentity (RI);
    dsSetColorAlpha (1,0,0,0.5);
    dsSetTexture (DS_NONE);
    dsDrawBox (bbpos,RI,bbsides);
    dsSetTexture (DS_WOOD);
  }
}


// simulation loop

static void simLoop (int pause)
{
  if (!pause) {
	  dsSetColor (0,0,2);
	  dSpaceCollide (space,0,&proximityCallback);
	  dWorldStep (world,0.05);
	  dJointGroupEmpty (contactgroup);
  }
  dsSetColor(1,1,0);
  dsSetTexture (DS_WOOD);
  //for (int i = 0; i < dSpaceGetNumGeoms(space); i++) {
    drawGeom((dGeomID)space,0,0,show_aabb);
  //}
}

int main (int argc, char **argv) {
  // 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";

  // create world

  world = dWorldCreate();
  space = dHashSpaceCreate (0);
  contactgroup = dJointGroupCreate (0);
  dWorldSetGravity (world,0,0,-9.81);
  dWorldSetCFM (world,1e-5);
  dWorldSetERP (world, 0.8);
  dCreatePlane (space,0,0,1,0);

  // run simulation
  dsSimulationLoop (argc,argv,640,480,&fn);

  dJointGroupDestroy (contactgroup);
  dSpaceDestroy (space);
  dWorldDestroy (world);
  dCloseODE();
  return 0;
}