source: code/branches/ode/ode-0.9/contrib/TerrainAndCone/test_boxstackb.cpp @ 216

/*************************************************************************


*                                                                                                                                                *


* 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 _MSC_VER


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


#endif





// select correct drawing functions





#ifdef dDOUBLE


#define dsDrawBox dsDrawBoxD


#define dsDrawSphere dsDrawSphereD


#define dsDrawCylinder dsDrawCylinderD


#define dsDrawCappedCylinder dsDrawCappedCylinderD


#endif








// some constants





const dReal vTerrainLength = 4.f;


const dReal vTerrainHeight = 0.5f;


const int TERRAINNODES = 4;


dReal pTerrainHeights[TERRAINNODES*TERRAINNODES];





dGeomID terrainZ = NULL;


dGeomID terrainY = NULL;





#define NUM 20                  // max number of objects


#define DENSITY (5.0)           // density of all objects


#define GPB 3                   // maximum number of geometries per body


#define MAX_CONTACTS 4          // maximum number of contact points per body








// dynamics and collision objects





struct MyObject {


        dBodyID body;           // the body


        dGeomID geom[GPB];              // geometries representing this body


};





static int num=0;               // number of objects in simulation


static int nextobj=0;           // next object to recycle if num==NUM


static dWorldID world;


static dSpaceID space;


static MyObject obj[NUM];


static dJointGroupID contactgroup;


static int selected = -1;       // selected object


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?








// this is called by dSpaceCollide when two objects in space are


// potentially colliding.





static void nearCallback (void *data, dGeomID o1, dGeomID o2)


{


        int i;


        // if (o1->body && o2->body) return;


        


        // exit without doing anything if the two bodies are connected by a joint


        dBodyID b1 = dGeomGetBody(o1);


        dBodyID b2 = dGeomGetBody(o2);


        if (b1 && b2 && dAreConnectedExcluding (b1,b2,dJointTypeContact)) return;


        


        dContact contact[MAX_CONTACTS]; // up to MAX_CONTACTS contacts per box-box


        for (i=0; i<MAX_CONTACTS; i++) {


                contact[i].surface.mode = dContactBounce | dContactApprox1;     //dContactSoftCFM;


                contact[i].surface.mu = dInfinity;


                contact[i].surface.mu2 = 0;


                contact[i].surface.bounce = 0.1;


                contact[i].surface.bounce_vel = 0.1;


                contact[i].surface.soft_cfm = 0.01;


        }


        if (int numc = dCollide (o1,o2,MAX_CONTACTS,&contact[0].geom,


                sizeof(dContact))) {


                dMatrix3 RI;


                dRSetIdentity (RI);


                const dReal ss[3] = {0.02,0.02,0.02};


                for (i=0; i<numc; i++) {


                        dJointID c = dJointCreateContact (world,contactgroup,contact+i);


                        dJointAttach (c,b1,b2);


                        if (show_contacts) dsDrawBox (contact[i].geom.pos,RI,ss);


                }


        }


}








// start simulation - set viewpoint





static void start()


{


        static float xyz[3] = {2.1640f,-1.3079f,1.7600f};


        static float hpr[3] = {125.5000f,-17.0000f,0.0000f};


        dsSetViewpoint (xyz,hpr);


        printf ("To drop another object, press:\n");


        printf ("   b for box.\n");


        printf ("   s for sphere.\n");


        printf ("   c for cylinder.\n");


        printf ("   x for a composite object.\n");


        printf ("To select an object, press space.\n");


        printf ("To disable the selected object, press d.\n");


        printf ("To enable the selected object, press e.\n");


        printf ("To toggle showing the geom AABBs, press a.\n");


        printf ("To toggle showing the contact points, press t.\n");


        printf ("To toggle dropping from random position/orientation, press r.\n");


}








char locase (char c)


{


        if (c >= 'A' && c <= 'Z') return c - ('a'-'A');


        else return c;


}








// called when a key pressed





static void command (int cmd)


{


        int i,j,k;


        dReal sides[3];


        dMass m;


        


        cmd = locase (cmd);


        if (cmd == 'b' || cmd == 's' || cmd == 'c' || cmd == 'x'


                /* || cmd == 'l' */) {


                if (num < NUM) {


                        i = num;


                        num++;


                }


                else {


                        i = nextobj;


                        nextobj++;


                        if (nextobj >= num) nextobj = 0;


                        


                        // destroy the body and geoms for slot i


                        dBodyDestroy (obj[i].body);


                        for (k=0; k < GPB; k++) {


                                if (obj[i].geom[k]) dGeomDestroy (obj[i].geom[k]);


                        }


                        memset (&obj[i],0,sizeof(obj[i]));


                }


                


                obj[i].body = dBodyCreate (world);


                for (k=0; k<3; k++) sides[k] = dRandReal()*0.5+0.1;


                


                dMatrix3 R;


                if (random_pos) {


                        dBodySetPosition (obj[i].body,


                                dRandReal()*2-1,dRandReal()*2+1,dRandReal()+3);


                        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 = 0;


                        for (k=0; k<num; k++) {


                                const dReal *pos = dBodyGetPosition (obj[k].body);


                                if (pos[2] > maxheight) maxheight = pos[2];


                        }


                        dBodySetPosition (obj[i].body, 0,maxheight+1,maxheight+3);


                        dRFromAxisAndAngle (R,0,0,1,dRandReal()*10.0-5.0);


                }


                dBodySetRotation (obj[i].body,R);


                dBodySetData (obj[i].body,(void*) i);


                


                if (cmd == 'b') {


                        dMassSetBox (&m,DENSITY,sides[0],sides[1],sides[2]);


                        obj[i].geom[0] = dCreateBox (space,sides[0],sides[1],sides[2]);


                }


                else if (cmd == 'c') {


                        sides[0] *= 0.5;


                        dMassSetCappedCylinder (&m,DENSITY,3,sides[0],sides[1]);


                        obj[i].geom[0] = dCreateCCylinder (space,sides[0],sides[1]);


                }


                /*


                // cylinder option not yet implemented


                else if (cmd == 'l') {


                sides[1] *= 0.5;


                dMassSetCappedCylinder (&m,DENSITY,3,sides[0],sides[1]);


                obj[i].geom[0] = dCreateCylinder (space,sides[0],sides[1]);


                }


                */


                else if (cmd == 's') {


                        sides[0] *= 0.5;


                        dMassSetSphere (&m,DENSITY,sides[0]);


                        obj[i].geom[0] = dCreateSphere (space,sides[0]);


                }


                else if (cmd == 'x') {


                        dGeomID g2[GPB];                // encapsulated geometries


                        dReal dpos[GPB][3];     // delta-positions for encapsulated geometries


                        


                        // start accumulating masses for the encapsulated geometries


                        dMass m2;


                        dMassSetZero (&m);


                        


                        // set random delta positions


                        for (j=0; j<GPB; j++) {


                                for (k=0; k<3; k++) dpos[j][k] = dRandReal()*0.3-0.15;


                        }


                        


                        for (k=0; k<GPB; k++) {


                                obj[i].geom[k] = dCreateGeomTransform (space);


                                dGeomTransformSetCleanup (obj[i].geom[k],1);


                                if (k==0) {


                                        dReal radius = dRandReal()*0.25+0.05;


                                        g2[k] = dCreateSphere (0,radius);


                                        dMassSetSphere (&m2,DENSITY,radius);


                                }


                                else if (k==1) {


                                        g2[k] = dCreateBox (0,sides[0],sides[1],sides[2]);


                                        dMassSetBox (&m2,DENSITY,sides[0],sides[1],sides[2]);


                                }


                                else {


                                        dReal radius = dRandReal()*0.1+0.05;


                                        dReal length = dRandReal()*1.0+0.1;


                                        g2[k] = dCreateCCylinder (0,radius,length);


                                        dMassSetCappedCylinder (&m2,DENSITY,3,radius,length);


                                }


                                dGeomTransformSetGeom (obj[i].geom[k],g2[k]);


                                


                                // set the transformation (adjust the mass too)


                                dGeomSetPosition (g2[k],dpos[k][0],dpos[k][1],dpos[k][2]);


                                dMassTranslate (&m2,dpos[k][0],dpos[k][1],dpos[k][2]);


                                dMatrix3 Rtx;


                                dRFromAxisAndAngle (Rtx,dRandReal()*2.0-1.0,dRandReal()*2.0-1.0,


                                        dRandReal()*2.0-1.0,dRandReal()*10.0-5.0);


                                dGeomSetRotation (g2[k],Rtx);


                                dMassRotate (&m2,Rtx);


                                


                                // add to the total mass


                                dMassAdd (&m,&m2);


                        }


                        


                        // move all encapsulated objects so that the center of mass is (0,0,0)


                        for (k=0; k<2; k++) {


                                dGeomSetPosition (g2[k],


                                        dpos[k][0]-m.c[0],


                                        dpos[k][1]-m.c[1],


                                        dpos[k][2]-m.c[2]);


                        }


                        dMassTranslate (&m,-m.c[0],-m.c[1],-m.c[2]);


                }


                


                for (k=0; k < GPB; k++) {


                        if (obj[i].geom[k]) dGeomSetBody (obj[i].geom[k],obj[i].body);


                }


                


                dBodySetMass (obj[i].body,&m);


  }


  


  if (cmd == ' ') {


          selected++;


          if (selected >= num) selected = 0;


          if (selected < 0) selected = 0;


  }


  else if (cmd == 'd' && selected >= 0 && selected < num) {


          dBodyDisable (obj[selected].body);


  }


  else if (cmd == 'e' && selected >= 0 && selected < num) {


          dBodyEnable (obj[selected].body);


  }


  else 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)


{


        int i;


        


        if (!g) 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);


        }


        /*


        // cylinder option not yet implemented


        else if (type == dCylinderClass) {


        dReal radius,length;


        dGeomCylinderGetParams (g,&radius,&length);


        dsDrawCylinder (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 (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);


                dsDrawBox (bbpos,RI,bbsides);


        }


}








// simulation loop





static void simLoop (int pause)


{


        dsSetColor (0,0,2);


        dSpaceCollide (space,0,&nearCallback);


        if (!pause) dWorldStep (world,0.05);





        dAASSERT(terrainY);


        dAASSERT(terrainZ);


        dsSetColor (0,1,0);


        dsDrawTerrainY(0,0,vTerrainLength,vTerrainLength/TERRAINNODES,TERRAINNODES,pTerrainHeights,dGeomGetRotation(terrainY),dGeomGetPosition(terrainY));


        dsDrawTerrainZ(0,0,vTerrainLength,vTerrainLength/TERRAINNODES,TERRAINNODES,pTerrainHeights,dGeomGetRotation(terrainZ),dGeomGetPosition(terrainZ));





        if (show_aabb) 


        {


                dReal aabb[6];


                dGeomGetAABB (terrainY,aabb);


                dVector3 bbpos;


                int i;


                for (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);


                dsDrawBox (bbpos,RI,bbsides);





                dGeomGetAABB (terrainZ,aabb);


                for (i=0; i<3; i++) bbpos[i] = 0.5*(aabb[i*2] + aabb[i*2+1]);


                for (i=0; i<3; i++) bbsides[i] = aabb[i*2+1] - aabb[i*2];


                dsDrawBox (bbpos,RI,bbsides);


        }





        dsSetColor (1,1,0);


        


        // remove all contact joints


        dJointGroupEmpty (contactgroup);


        


        dsSetColor (1,1,0);


        dsSetTexture (DS_WOOD);


        for (int i=0; i<num; i++) {


                for (int j=0; j < GPB; j++) {


                        if (i==selected) {


                                dsSetColor (0,0.7,1);


                        }


                        else if (! dBodyIsEnabled (obj[i].body)) {


                                dsSetColor (1,0,0);


                        }


                        else {


                                dsSetColor (1,1,0);


                        }


                        drawGeom (obj[i].geom[j],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";


  if(argc==2)
    {
        fn.path_to_textures = argv[1];
    }
        


        // create world


        


        world = dWorldCreate();


        space = dHashSpaceCreate (0);


        contactgroup = dJointGroupCreate (0);


        dWorldSetGravity (world,0,0,-0.5); //-0.5


        dWorldSetCFM (world,1e-5);


        dCreatePlane (space,0,0,1,0);


        memset (obj,0,sizeof(obj));





        for (int i=0;i<TERRAINNODES*TERRAINNODES;i++)   pTerrainHeights[i] = vTerrainHeight * dRandReal();


        terrainY = dCreateTerrainY(space,pTerrainHeights,vTerrainLength,TERRAINNODES,1,1);


        terrainZ = dCreateTerrainZ(space,pTerrainHeights,vTerrainLength,TERRAINNODES,1,1);





        dMatrix3 R;


        dRFromZAxis(R, 0.2f, 0.2f, 0.2f);


        dGeomSetPosition(terrainY,0.f,0.f,0.5f);


        dGeomSetRotation(terrainY,R);


        dGeomSetPosition(terrainZ,0.f,0.f,0.5f);


        dGeomSetRotation(terrainZ,R);





        // run simulation


        dsSimulationLoop (argc,argv,352,288,&fn);


        


        dJointGroupDestroy (contactgroup);


        dSpaceDestroy (space);


        dWorldDestroy (world);


        


        return 0;


}