From 79eca25c945a535a7a0325999034bae17da92412 Mon Sep 17 00:00:00 2001 From: dan miller Date: Fri, 19 Oct 2007 05:15:33 +0000 Subject: resubmitting ode --- libraries/ode-0.9/ode/demo/demo_boxstack.cpp | 577 +++++++++++++++++++++++++++ 1 file changed, 577 insertions(+) create mode 100644 libraries/ode-0.9/ode/demo/demo_boxstack.cpp (limited to 'libraries/ode-0.9/ode/demo/demo_boxstack.cpp') diff --git a/libraries/ode-0.9/ode/demo/demo_boxstack.cpp b/libraries/ode-0.9/ode/demo/demo_boxstack.cpp new file mode 100644 index 0000000..8d237b2 --- /dev/null +++ b/libraries/ode-0.9/ode/demo/demo_boxstack.cpp @@ -0,0 +1,577 @@ +/************************************************************************* + * * + * 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 +#include + +#ifdef _MSC_VER +#pragma warning(disable:4244 4305) // for VC++, no precision loss complaints +#endif + + +//<---- Convex Object +dReal planes[]= // planes for a cube, these should coincide with the face array + { + 1.0f ,0.0f ,0.0f ,0.25f, + 0.0f ,1.0f ,0.0f ,0.25f, + 0.0f ,0.0f ,1.0f ,0.25f, + -1.0f,0.0f ,0.0f ,0.25f, + 0.0f ,-1.0f,0.0f ,0.25f, + 0.0f ,0.0f ,-1.0f,0.25f + /* + 1.0f ,0.0f ,0.0f ,2.0f, + 0.0f ,1.0f ,0.0f ,1.0f, + 0.0f ,0.0f ,1.0f ,1.0f, + 0.0f ,0.0f ,-1.0f,1.0f, + 0.0f ,-1.0f,0.0f ,1.0f, + -1.0f,0.0f ,0.0f ,0.0f + */ + }; +const unsigned int planecount=6; + +dReal points[]= // points for a cube + { + 0.25f,0.25f,0.25f, // point 0 + -0.25f,0.25f,0.25f, // point 1 + + 0.25f,-0.25f,0.25f, // point 2 + -0.25f,-0.25f,0.25f,// point 3 + + 0.25f,0.25f,-0.25f, // point 4 + -0.25f,0.25f,-0.25f,// point 5 + + 0.25f,-0.25f,-0.25f,// point 6 + -0.25f,-0.25f,-0.25f,// point 7 + }; +const unsigned int pointcount=8; +unsigned int polygons[] = //Polygons for a cube (6 squares) + { + 4,0,2,6,4, // positive X + 4,1,0,4,5, // positive Y + 4,0,1,3,2, // positive Z + 4,3,1,5,7, // negative X + 4,2,3,7,6, // negative Y + 4,5,4,6,7, // negative Z + }; +//----> Convex Object + +// select correct drawing functions + +#ifdef dDOUBLE +#define dsDrawBox dsDrawBoxD +#define dsDrawSphere dsDrawSphereD +#define dsDrawCylinder dsDrawCylinderD +#define dsDrawCapsule dsDrawCapsuleD +#define dsDrawConvex dsDrawConvexD +#endif + + +// some constants + +#define NUM 100 // max number of objects +#define DENSITY (5.0) // density of all objects +#define GPB 3 // maximum number of geometries per body +#define MAX_CONTACTS 8 // maximum number of contact points per body +#define USE_GEOM_OFFSET 1 + +// 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? +static int write_world = 0; +static int show_body = 1; + +// 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= 'A' && c <= 'Z') return c - ('a'-'A'); + else return c; +} + + +// called when a key pressed + +static void command (int cmd) +{ + size_t i; + int j,k; + dReal sides[3]; + dMass m; + int setBody; + + cmd = locase (cmd); + if (cmd == 'b' || cmd == 's' || cmd == 'c' || cmd == 'x' || cmd == 'y' || cmd == 'v') + { + setBody = 0; + 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()+2); + 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 maxheight) maxheight = pos[2]; + } + dBodySetPosition (obj[i].body, 0,0,maxheight+1); + dRSetIdentity (R); + //dRFromAxisAndAngle (R,0,0,1,/*dRandReal()*10.0-5.0*/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; + dMassSetCapsule (&m,DENSITY,3,sides[0],sides[1]); + obj[i].geom[0] = dCreateCapsule (space,sides[0],sides[1]); + } + //<---- Convex Object + else if (cmd == 'v') + { + dMassSetBox (&m,DENSITY,0.25,0.25,0.25); + obj[i].geom[0] = dCreateConvex (space, + planes, + planecount, + points, + pointcount, + polygons); + } + //----> Convex Object + else if (cmd == 'y') { + dMassSetCylinder (&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' && USE_GEOM_OFFSET) { + setBody = 1; + // start accumulating masses for the encapsulated geometries + dMass m2; + dMassSetZero (&m); + + dReal dpos[GPB][3]; // delta-positions for encapsulated geometries + dMatrix3 drot[GPB]; + + // set random delta positions + for (j=0; j= 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; + } + else if (cmd == '1') { + write_world = 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 == dCapsuleClass) { + dReal radius,length; + dGeomCapsuleGetParams (g,&radius,&length); + dsDrawCapsule (pos,R,length,radius); + } + //<---- Convex Object + else if (type == dConvexClass) + { + //dVector3 sides={0.50,0.50,0.50}; + dsDrawConvex(pos,R,planes, + planecount, + points, + pointcount, + polygons); + } + //----> Convex Object + 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_body) { + dBodyID body = dGeomGetBody(g); + if (body) { + const dReal *bodypos = dBodyGetPosition (body); + const dReal *bodyr = dBodyGetRotation (body); + dReal bodySides[3] = { 0.1, 0.1, 0.1 }; + dsSetColorAlpha(0,1,0,1); + dsDrawBox(bodypos,bodyr,bodySides); + } + } + 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) dWorldQuickStep (world,0.02); + + if (write_world) { + FILE *f = fopen ("state.dif","wt"); + if (f) { + dWorldExportDIF (world,f,"X"); + fclose (f); + } + write_world = 0; + } + + // remove all contact joints + dJointGroupEmpty (contactgroup); + + dsSetColor (1,1,0); + dsSetTexture (DS_WOOD); + for (int i=0; i