summaryrefslogtreecommitdiff
path: root/libs/ode-0.16.1/ode/demo/demo_convex.cpp
diff options
context:
space:
mode:
Diffstat (limited to 'libs/ode-0.16.1/ode/demo/demo_convex.cpp')
-rw-r--r--libs/ode-0.16.1/ode/demo/demo_convex.cpp307
1 files changed, 307 insertions, 0 deletions
diff --git a/libs/ode-0.16.1/ode/demo/demo_convex.cpp b/libs/ode-0.16.1/ode/demo/demo_convex.cpp
new file mode 100644
index 0000000..eea5c6e
--- /dev/null
+++ b/libs/ode-0.16.1/ode/demo/demo_convex.cpp
@@ -0,0 +1,307 @@
+/*************************************************************************
+ * *
+ * 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. *
+ * *
+ *************************************************************************/
+
+// Convex demo.
+// Serves as a test for the convex geometry.
+// By Bram Stolk.
+
+#include <assert.h>
+#ifdef HAVE_UNISTD_H
+# include <unistd.h>
+#endif
+#include <ode/ode.h>
+#include <drawstuff/drawstuff.h>
+#include "texturepath.h"
+
+#include "halton235_geom.h"
+
+#ifdef dDOUBLE
+# define dsDrawConvex dsDrawConvexD
+# define dsDrawLine dsDrawLineD
+#endif
+
+
+#ifdef _MSC_VER
+# pragma warning(disable:4244 4305) // for VC++, no precision loss complaints
+#endif
+
+
+// Height at which we drop the composite block.
+const dReal H=4.20;
+
+static dWorldID world;
+static dSpaceID space;
+
+static dBodyID mbody;
+
+static dBodyID hbody[ halton_numc ];
+static dGeomID hgeom[ halton_numc ];
+
+static dJointGroupID contactgroup;
+
+static bool drawpos=false;
+static bool solidkernel=false;
+
+
+// this is called by dSpaceCollide when two objects in space are
+// potentially colliding.
+
+static void nearCallback(void *data, dGeomID o1, dGeomID o2)
+{
+ assert(o1);
+ assert(o2);
+ if (dGeomIsSpace(o1) || dGeomIsSpace(o2))
+ {
+ // colliding a space with something
+ dSpaceCollide2(o1,o2,data,&nearCallback);
+ // Note we do not want to test intersections within a space,
+ // only between spaces.
+ return;
+ }
+
+ const int N = 32;
+ dContact contact[N];
+ int n = dCollide (o1,o2,N,&(contact[0].geom),sizeof(dContact));
+ if (n > 0)
+ {
+ for (int i=0; i<n; i++)
+ {
+ contact[i].surface.slip1 = 0.7;
+ contact[i].surface.slip2 = 0.7;
+ contact[i].surface.mode = dContactSoftERP | dContactSoftCFM | dContactApprox1 | dContactSlip1 | dContactSlip2;
+ contact[i].surface.mu = 500.0; // was: dInfinity
+ contact[i].surface.soft_erp = 0.50;
+ contact[i].surface.soft_cfm = 0.03;
+ dJointID c = dJointCreateContact (world,contactgroup,&contact[i]);
+ dJointAttach
+ (
+ c,
+ dGeomGetBody(contact[i].geom.g1),
+ dGeomGetBody(contact[i].geom.g2)
+ );
+ }
+ }
+}
+
+
+// start simulation - set viewpoint
+
+static void start()
+{
+ dAllocateODEDataForThread(dAllocateMaskAll);
+ static float xyz[3] = {-8,0,5};
+ static float hpr[3] = {0.0f,-29.5000f,0.0000f};
+ dsSetViewpoint (xyz,hpr);
+ fprintf(stderr,"Press SPACE to reset the simulation.\n");
+}
+
+
+static void reset()
+{
+ dQuaternion q;
+ dQSetIdentity(q);
+ dBodySetPosition(mbody,0,0,0+H);
+ dBodySetQuaternion(mbody, q);
+ dBodySetLinearVel(mbody, 0,0,0);
+ dBodySetAngularVel(mbody, 0,0,0);
+ dBodyEnable(mbody);
+ for ( int i=0; i<halton_numc; ++i )
+ {
+ dBodyID body = hbody[i];
+ if ( !body ) continue;
+ dBodySetPosition(body, halton_pos[i][0], halton_pos[i][1], halton_pos[i][2]+H);
+ dBodySetQuaternion(body, q);
+ dBodySetLinearVel(body, 0,0,0);
+ dBodySetAngularVel(body, 0,0,0);
+ dBodyEnable(body);
+ }
+}
+
+
+// called when a key pressed
+
+static void command(int cmd)
+{
+ switch (cmd)
+ {
+ case ' ':
+ reset();
+ break;
+ default:
+ break;
+ }
+}
+
+
+
+static void simLoop(int pause)
+{
+ double simstep = 1/240.0;
+ double dt = dsElapsedTime();
+
+ int nrofsteps = (int) ceilf(dt/simstep);
+ nrofsteps = nrofsteps > 8 ? 8 : nrofsteps;
+
+ for (int i=0; i<nrofsteps && !pause; i++)
+ {
+ dSpaceCollide (space,0,&nearCallback);
+ dWorldQuickStep (world, simstep);
+ dJointGroupEmpty (contactgroup);
+ }
+
+ dsSetColor (1,1,1);
+ // Draw the convex objects.
+ for ( int i=0; i<halton_numc; ++i )
+ {
+ dGeomID geom = hgeom[i];
+ dBodyID body = dGeomGetBody(geom);
+ //const dReal *pos = dBodyGetPosition(body);
+ //const dReal *rot = dBodyGetRotation(body);
+ const dReal *pos = dGeomGetPosition(geom);
+ const dReal *rot = dGeomGetRotation(geom);
+ dsDrawConvex
+ (
+ pos, rot,
+ halton_planes[i],
+ halton_numf[i],
+ halton_verts[i],
+ halton_numv[i],
+ halton_faces[i]
+ );
+ }
+
+ if (drawpos)
+ {
+ dsSetColor(1,0,0.2);
+ dsSetTexture(DS_NONE);
+ const dReal l = 0.35;
+ for ( int i=0; i<halton_numc; ++i )
+ {
+ dBodyID body = hbody[i];
+ const dReal *pos = dBodyGetPosition(body);
+ dReal x0[3] = { pos[0]-l, pos[1], pos[2] };
+ dReal x1[3] = { pos[0]+l, pos[1], pos[2] };
+ dReal y0[3] = { pos[0], pos[1]-l, pos[2] };
+ dReal y1[3] = { pos[0], pos[1]+l, pos[2] };
+ dReal z0[3] = { pos[0], pos[1], pos[2]-l };
+ dReal z1[3] = { pos[0], pos[1], pos[2]+l };
+ dsDrawLine(x0,x1);
+ dsDrawLine(y0,y1);
+ dsDrawLine(z0,z1);
+ }
+ }
+}
+
+
+int main (int argc, char **argv)
+{
+ dMass m;
+
+ // 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 = dHashSpaceCreate (0);
+ dHashSpaceSetLevels(space, -3, 5);
+ dCreatePlane(space,0,0,1,0); // Add a ground plane.
+
+ contactgroup = dJointGroupCreate (0);
+ dWorldSetGravity(world,0,0,-9.8);
+ dWorldSetQuickStepNumIterations(world, 32);
+ dWorldSetContactMaxCorrectingVel(world, 40);
+ dWorldSetMaxAngularSpeed(world, 62.8);
+ dWorldSetERP(world, 0.7);
+ dWorldSetQuickStepW(world, 0.75); // For increased stability.
+
+ dWorldSetAutoDisableFlag( world, true );
+ dWorldSetAutoDisableLinearThreshold( world, 0.01 );
+ dWorldSetAutoDisableAngularThreshold( world, 0.03 );
+ dWorldSetAutoDisableTime( world, 0.15f );
+
+ const float kernelrad = 0.7;
+
+ mbody = dBodyCreate(world);
+ dBodySetPosition(mbody, 0,0,0+H);
+ dMassSetSphere( &m, 5, kernelrad );
+ dBodySetMass( mbody, &m );
+
+ for (int i=0; i<halton_numc; ++i )
+ {
+ dGeomID geom = dCreateConvex
+ (
+ space,
+ halton_planes[i],
+ halton_numf[i],
+ halton_verts[i],
+ halton_numv[i],
+ halton_faces[i]
+ );
+ hgeom[i] = geom;
+ const dReal x = halton_pos[i][0];
+ const dReal y = halton_pos[i][1];
+ const dReal z = halton_pos[i][2];
+ const dReal dsqr = x*x + y*y + z*z;
+
+ if ( dsqr < kernelrad*kernelrad && solidkernel )
+ {
+ dGeomSetBody(geom, mbody);
+ dGeomSetOffsetPosition(geom, x,y,z);
+ }
+ else
+ {
+ dBodyID body = dBodyCreate(world);
+ hbody[i] = body;
+ dBodySetPosition(body, x,y,z+H);
+ dReal volu = halton_volu[i];
+ dReal rad = pow( volu * 3 / (4*M_PI), (1/3.0) );
+ dMassSetSphere( &m,5,rad );
+ dBodySetMass( body,&m );
+#if 1
+ dBodySetLinearDamping (body, 0.0005);
+ dBodySetAngularDamping(body, 0.0300);
+#endif
+ dGeomSetBody(geom,body);
+ }
+ }
+
+ // run simulation
+ const int w=1280;
+ const int h=720;
+ dsSimulationLoop (argc,argv,w,h,&fn);
+
+ dJointGroupEmpty (contactgroup);
+ dJointGroupDestroy (contactgroup);
+ dSpaceDestroy (space);
+ dWorldDestroy (world);
+ dCloseODE();
+ return 0;
+}
+
+
generated by cgit v1.2.1 (git 2.18.0) at 2024-12-23 07:09:26 +0000