summaryrefslogtreecommitdiff
path: root/libs/ode-0.16.1/ode/demo/demo_moving_convex.cpp
diff options
context:
space:
mode:
Diffstat (limited to 'libs/ode-0.16.1/ode/demo/demo_moving_convex.cpp')
-rw-r--r--libs/ode-0.16.1/ode/demo/demo_moving_convex.cpp415
1 files changed, 415 insertions, 0 deletions
diff --git a/libs/ode-0.16.1/ode/demo/demo_moving_convex.cpp b/libs/ode-0.16.1/ode/demo/demo_moving_convex.cpp
new file mode 100644
index 0000000..2f03900
--- /dev/null
+++ b/libs/ode-0.16.1/ode/demo/demo_moving_convex.cpp
@@ -0,0 +1,415 @@
+/*************************************************************************
+ * *
+ * Open Dynamics Engine, Copyright (C) 2001-2003 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>
+#include "texturepath.h"
+#include "bunny_geom.h"
+#include "convex_bunny_geom.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 dsDrawCapsule dsDrawCapsuleD
+#define dsDrawLine dsDrawLineD
+#define dsDrawTriangle dsDrawTriangleD
+#define dsDrawConvex dsDrawConvexD
+#endif
+
+
+// some constants
+
+#define NUM 200 // max number of objects
+#define DENSITY (5.0) // density of all objects
+#define GPB 3 // maximum number of geometries per body
+#define MAX_CONTACTS 64 // 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?
+
+typedef dReal dVector3R[3];
+
+// this is called by dSpaceCollide when two objects in space are
+// potentially colliding.
+
+static void nearCallback( void *, 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 | 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()
+{
+ dAllocateODEDataForThread( dAllocateMaskAll );
+
+ 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 capsule.\n" );
+ printf( " v for a convex.\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,k;
+ dReal sides[3];
+ dMass m;
+
+ cmd = locase( cmd );
+ if ( cmd == 'v' || cmd == 'b' || cmd == 'c' || cmd == 's' || cmd == 'y')
+ {
+ 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,0,maxheight+1 );
+ dRFromAxisAndAngle( R,0,0,1,dRandReal()*10.0-5.0 );
+ }
+ dBodySetRotation( obj[i].body,R );
+ dBodySetData( obj[i].body,( void* )( dsizeint )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] );
+ }
+ 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 == 'v' )
+ {
+ obj[i].geom[0] = dCreateConvex( space,
+ convexBunnyPlanes,
+ convexBunnyPlaneCount,
+ convexBunnyPoints,
+ convexBunnyPointCount,
+ convexBunnyPolygons );
+
+ /// Use equivalent TriMesh to set mass
+ dTriMeshDataID new_tmdata = dGeomTriMeshDataCreate();
+ dGeomTriMeshDataBuildSingle( new_tmdata, &Vertices[0], 3 * sizeof( float ), VertexCount,
+ ( dTriIndex* )&Indices[0], IndexCount, 3 * sizeof( dTriIndex ) );
+ dGeomTriMeshDataPreprocess2( new_tmdata, (1U << dTRIDATAPREPROCESS_BUILD_FACE_ANGLES), NULL );
+
+ dGeomID triMesh = dCreateTriMesh( 0, new_tmdata, 0, 0, 0 );
+
+ dMassSetTrimesh( &m, DENSITY, triMesh );
+
+ dGeomDestroy( triMesh );
+ dGeomTriMeshDataDestroy( new_tmdata );
+
+ printf( "mass at %f %f %f\n", m.c[0], m.c[1], m.c[2] );
+ dGeomSetPosition( obj[i].geom[0], -m.c[0], -m.c[1], -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 )
+{
+ 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 == dCylinderClass) {
+ dReal radius,length;
+ dGeomCylinderGetParams (g,&radius,&length);
+ dsDrawCylinder (pos,R,length,radius);
+ }
+ else if ( type == dCapsuleClass )
+ {
+ dReal radius,length;
+ dGeomCapsuleGetParams( g,&radius,&length );
+ dsDrawCapsule( pos,R,length,radius );
+ }
+ else if ( type == dConvexClass )
+ {
+ dsDrawConvex( pos,R,
+ convexBunnyPlanes,
+ convexBunnyPlaneCount,
+ convexBunnyPoints,
+ convexBunnyPointCount,
+ convexBunnyPolygons );
+ }
+
+ 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 ( int j=0; j<3; j++ ) bbsides[j] = aabb[j*2+1] - aabb[j*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.05 );
+
+ for ( int j = 0; j < dSpaceGetNumGeoms( space ); j++ )
+ {
+ dSpaceGetGeom( space, j );
+ }
+
+ // 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 ( obj[i].geom[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_TEXTURE_PATH;
+
+ // create world
+ dInitODE2( 0 );
+ world = dWorldCreate();
+
+ space = dSimpleSpaceCreate( 0 );
+ contactgroup = dJointGroupCreate( 0 );
+ dWorldSetGravity( world,0,0,-0.5 );
+ dWorldSetCFM( world,1e-5 );
+ dCreatePlane( space,0,0,1,0 );
+ memset( obj,0,sizeof( obj ) );
+
+ // run simulation
+ dsSimulationLoop( argc,argv,352,288,&fn );
+
+ dJointGroupDestroy( contactgroup );
+ dSpaceDestroy( space );
+ dWorldDestroy( world );
+ dCloseODE();
+ return 0;
+}
+
+
generated by cgit v1.2.1 (git 2.18.0) at 2024-11-01 15:31:33 +0000