aboutsummaryrefslogtreecommitdiffstatshomepage
path: root/libraries/ode-0.9/ode/demo/demo_space_stress.cpp
diff options
context:
space:
mode:
Diffstat (limited to 'libraries/ode-0.9/ode/demo/demo_space_stress.cpp')
-rw-r--r--libraries/ode-0.9/ode/demo/demo_space_stress.cpp435
1 files changed, 0 insertions, 435 deletions
diff --git a/libraries/ode-0.9/ode/demo/demo_space_stress.cpp b/libraries/ode-0.9/ode/demo/demo_space_stress.cpp
deleted file mode 100644
index e1be369..0000000
--- a/libraries/ode-0.9/ode/demo/demo_space_stress.cpp
+++ /dev/null
@@ -1,435 +0,0 @@
1/*************************************************************************
2 * *
3 * Open Dynamics Engine, Copyright (C) 2001-2003 Russell L. Smith. *
4 * All rights reserved. Email: russ@q12.org Web: www.q12.org *
5 * *
6 * This library is free software; you can redistribute it and/or *
7 * modify it under the terms of EITHER: *
8 * (1) The GNU Lesser General Public License as published by the Free *
9 * Software Foundation; either version 2.1 of the License, or (at *
10 * your option) any later version. The text of the GNU Lesser *
11 * General Public License is included with this library in the *
12 * file LICENSE.TXT. *
13 * (2) The BSD-style license that is included with this library in *
14 * the file LICENSE-BSD.TXT. *
15 * *
16 * This library is distributed in the hope that it will be useful, *
17 * but WITHOUT ANY WARRANTY; without even the implied warranty of *
18 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files *
19 * LICENSE.TXT and LICENSE-BSD.TXT for more details. *
20 * *
21 *************************************************************************/
22
23#include <ode/ode.h>
24#include <drawstuff/drawstuff.h>
25
26#ifdef _MSC_VER
27#pragma warning(disable:4244 4305) // for VC++, no precision loss complaints
28#endif
29
30// select correct drawing functions
31
32#ifdef dDOUBLE
33#define dsDrawBox dsDrawBoxD
34#define dsDrawSphere dsDrawSphereD
35#define dsDrawCylinder dsDrawCylinderD
36#define dsDrawCapsule dsDrawCapsuleD
37#endif
38
39
40// some constants
41
42#define NUM 10000 // max number of objects
43#define DENSITY (5.0) // density of all objects
44#define GPB 3 // maximum number of geometries per body
45#define MAX_CONTACTS 4 // maximum number of contact points per body
46#define WORLD_SIZE 100
47
48
49// dynamics and collision objects
50
51struct MyObject {
52 dBodyID body; // the body
53 dGeomID geom[GPB]; // geometries representing this body
54};
55
56static int num=0; // number of objects in simulation
57static int nextobj=0; // next object to recycle if num==NUM
58static dWorldID world;
59static dSpaceID space;
60static MyObject obj[NUM];
61static dJointGroupID contactgroup;
62static int selected = -1; // selected object
63static int show_aabb = 0; // show geom AABBs?
64static int show_contacts = 0; // show contact points?
65static int random_pos = 1; // drop objects from random position?
66static int draw_geom = 1;
67
68
69// this is called by dSpaceCollide when two objects in space are
70// potentially colliding.
71
72static void nearCallback (void *data, dGeomID o1, dGeomID o2)
73{
74 int i;
75 // if (o1->body && o2->body) return;
76
77 // exit without doing anything if the two bodies are connected by a joint
78 dBodyID b1 = dGeomGetBody(o1);
79 dBodyID b2 = dGeomGetBody(o2);
80 if (b1 && b2 && dAreConnectedExcluding (b1,b2,dJointTypeContact)) return;
81
82 dContact contact[MAX_CONTACTS]; // up to MAX_CONTACTS contacts per box-box
83 for (i=0; i<MAX_CONTACTS; i++) {
84 contact[i].surface.mode = dContactBounce | dContactSoftCFM;
85 contact[i].surface.mu = dInfinity;
86 contact[i].surface.mu2 = 0;
87 contact[i].surface.bounce = 0.1;
88 contact[i].surface.bounce_vel = 0.1;
89 contact[i].surface.soft_cfm = 0.01;
90 }
91 if (int numc = dCollide (o1,o2,MAX_CONTACTS,&contact[0].geom,
92 sizeof(dContact))) {
93 dMatrix3 RI;
94 dRSetIdentity (RI);
95 const dReal ss[3] = {0.02,0.02,0.02};
96 for (i=0; i<numc; i++) {
97 dJointID c = dJointCreateContact (world,contactgroup,contact+i);
98 dJointAttach (c,b1,b2);
99 if (show_contacts) dsDrawBox (contact[i].geom.pos,RI,ss);
100 }
101 }
102}
103
104
105// start simulation - set viewpoint
106
107static void start()
108{
109 static float xyz[3] = {2.1640f,-1.3079f,1.7600f};
110 static float hpr[3] = {125.5000f,-17.0000f,0.0000f};
111 dsSetViewpoint (xyz,hpr);
112 printf ("To drop another object, press:\n");
113 printf (" o to disable rendering.\n");
114 printf (" b for box.\n");
115 printf (" s for sphere.\n");
116 printf (" c for cylinder.\n");
117 printf (" x for a composite object.\n");
118 printf ("To select an object, press space.\n");
119 printf ("To disable the selected object, press d.\n");
120 printf ("To enable the selected object, press e.\n");
121 printf ("To toggle showing the geom AABBs, press a.\n");
122 printf ("To toggle showing the contact points, press t.\n");
123 printf ("To toggle dropping from random position/orientation, press r.\n");
124}
125
126
127char locase (char c)
128{
129 if (c >= 'A' && c <= 'Z') return c - ('a'-'A');
130 else return c;
131}
132
133
134// called when a key pressed
135
136static void command (int cmd)
137{
138 int i,j,k;
139 dReal sides[3];
140 dMass m;
141
142 cmd = locase (cmd);
143 if (cmd == 'b' || cmd == 's' || cmd == 'c' || cmd == 'x'
144 /* || cmd == 'l' */) {
145 if (num < NUM) {
146 i = num;
147 num++;
148 }
149 else {
150 i = nextobj;
151 nextobj++;
152 if (nextobj >= num) nextobj = 0;
153
154 // destroy the body and geoms for slot i
155 dBodyDestroy (obj[i].body);
156 for (k=0; k < GPB; k++) {
157 if (obj[i].geom[k]) dGeomDestroy (obj[i].geom[k]);
158 }
159 memset (&obj[i],0,sizeof(obj[i]));
160 }
161
162 obj[i].body = dBodyCreate (world);
163 for (k=0; k<3; k++) sides[k] = dRandReal()*0.5+0.1;
164
165 dMatrix3 R;
166 if (random_pos) {
167 dBodySetPosition (obj[i].body,
168 dRandReal()*WORLD_SIZE-(WORLD_SIZE/2),dRandReal()*WORLD_SIZE-(WORLD_SIZE/2),dRandReal()+1);
169 dRFromAxisAndAngle (R,dRandReal()*2.0-1.0,dRandReal()*2.0-1.0,
170 dRandReal()*2.0-1.0,dRandReal()*10.0-5.0);
171 }
172 else {
173 dReal maxheight = 0;
174 for (k=0; k<num; k++) {
175 const dReal *pos = dBodyGetPosition (obj[k].body);
176 if (pos[2] > maxheight) maxheight = pos[2];
177 }
178 dBodySetPosition (obj[i].body, 0,0,maxheight+1);
179 dRFromAxisAndAngle (R,0,0,1,dRandReal()*10.0-5.0);
180 }
181 dBodySetRotation (obj[i].body,R);
182 dBodySetData (obj[i].body,(void*)(size_t)i);
183
184 if (cmd == 'b') {
185 dMassSetBox (&m,DENSITY,sides[0],sides[1],sides[2]);
186 obj[i].geom[0] = dCreateBox (space,sides[0],sides[1],sides[2]);
187 }
188 else if (cmd == 'c') {
189 sides[0] *= 0.5;
190 dMassSetCapsule (&m,DENSITY,3,sides[0],sides[1]);
191 obj[i].geom[0] = dCreateCapsule (space,sides[0],sides[1]);
192 }
193/*
194 // cylinder option not yet implemented
195 else if (cmd == 'l') {
196 sides[1] *= 0.5;
197 dMassSetCapsule (&m,DENSITY,3,sides[0],sides[1]);
198 obj[i].geom[0] = dCreateCylinder (space,sides[0],sides[1]);
199 }
200*/
201 else if (cmd == 's') {
202 sides[0] *= 0.5;
203 dMassSetSphere (&m,DENSITY,sides[0]);
204 obj[i].geom[0] = dCreateSphere (space,sides[0]);
205 }
206 else if (cmd == 'x') {
207 dGeomID g2[GPB]; // encapsulated geometries
208 dReal dpos[GPB][3]; // delta-positions for encapsulated geometries
209
210 // start accumulating masses for the encapsulated geometries
211 dMass m2;
212 dMassSetZero (&m);
213
214 // set random delta positions
215 for (j=0; j<GPB; j++) {
216 for (k=0; k<3; k++) dpos[j][k] = dRandReal()*0.3-0.15;
217 }
218
219 for (k=0; k<GPB; k++) {
220 obj[i].geom[k] = dCreateGeomTransform (space);
221 dGeomTransformSetCleanup (obj[i].geom[k],1);
222 if (k==0) {
223 dReal radius = dRandReal()*0.25+0.05;
224 g2[k] = dCreateSphere (0,radius);
225 dMassSetSphere (&m2,DENSITY,radius);
226 }
227 else if (k==1) {
228 g2[k] = dCreateBox (0,sides[0],sides[1],sides[2]);
229 dMassSetBox (&m2,DENSITY,sides[0],sides[1],sides[2]);
230 }
231 else {
232 dReal radius = dRandReal()*0.1+0.05;
233 dReal length = dRandReal()*1.0+0.1;
234 g2[k] = dCreateCapsule (0,radius,length);
235 dMassSetCapsule (&m2,DENSITY,3,radius,length);
236 }
237 dGeomTransformSetGeom (obj[i].geom[k],g2[k]);
238
239 // set the transformation (adjust the mass too)
240 dGeomSetPosition (g2[k],dpos[k][0],dpos[k][1],dpos[k][2]);
241 dMassTranslate (&m2,dpos[k][0],dpos[k][1],dpos[k][2]);
242 dMatrix3 Rtx;
243 dRFromAxisAndAngle (Rtx,dRandReal()*2.0-1.0,dRandReal()*2.0-1.0,
244 dRandReal()*2.0-1.0,dRandReal()*10.0-5.0);
245 dGeomSetRotation (g2[k],Rtx);
246 dMassRotate (&m2,Rtx);
247
248 // add to the total mass
249 dMassAdd (&m,&m2);
250 }
251
252 // move all encapsulated objects so that the center of mass is (0,0,0)
253 for (k=0; k<2; k++) {
254 dGeomSetPosition (g2[k],
255 dpos[k][0]-m.c[0],
256 dpos[k][1]-m.c[1],
257 dpos[k][2]-m.c[2]);
258 }
259 dMassTranslate (&m,-m.c[0],-m.c[1],-m.c[2]);
260 }
261
262 for (k=0; k < GPB; k++) {
263 if (obj[i].geom[k]) dGeomSetBody (obj[i].geom[k],obj[i].body);
264 }
265
266 dBodySetMass (obj[i].body,&m);
267 }
268
269 if (cmd == ' ') {
270 selected++;
271 if (selected >= num) selected = 0;
272 if (selected < 0) selected = 0;
273 }
274 else if (cmd == 'd' && selected >= 0 && selected < num) {
275 dBodyDisable (obj[selected].body);
276 }
277 else if (cmd == 'e' && selected >= 0 && selected < num) {
278 dBodyEnable (obj[selected].body);
279 }
280 else if (cmd == 'a') {
281 show_aabb ^= 1;
282 }
283 else if (cmd == 't') {
284 show_contacts ^= 1;
285 }
286 else if (cmd == 'r') {
287 random_pos ^= 1;
288 }
289 else if (cmd == 'o') {
290 draw_geom ^= 1;
291 }
292}
293
294
295// draw a geom
296
297void drawGeom (dGeomID g, const dReal *pos, const dReal *R, int show_aabb)
298{
299 if (!draw_geom){
300 return;
301 }
302
303 if (!g) return;
304 if (!pos) pos = dGeomGetPosition (g);
305 if (!R) R = dGeomGetRotation (g);
306
307 int type = dGeomGetClass (g);
308 if (type == dBoxClass) {
309 dVector3 sides;
310 dGeomBoxGetLengths (g,sides);
311 dsDrawBox (pos,R,sides);
312 }
313 else if (type == dSphereClass) {
314 dsDrawSphere (pos,R,dGeomSphereGetRadius (g));
315 }
316 else if (type == dCapsuleClass) {
317 dReal radius,length;
318 dGeomCapsuleGetParams (g,&radius,&length);
319 dsDrawCapsule (pos,R,length,radius);
320 }
321/*
322 // cylinder option not yet implemented
323 else if (type == dCylinderClass) {
324 dReal radius,length;
325 dGeomCylinderGetParams (g,&radius,&length);
326 dsDrawCylinder (pos,R,length,radius);
327 }
328*/
329 else if (type == dGeomTransformClass) {
330 dGeomID g2 = dGeomTransformGetGeom (g);
331 const dReal *pos2 = dGeomGetPosition (g2);
332 const dReal *R2 = dGeomGetRotation (g2);
333 dVector3 actual_pos;
334 dMatrix3 actual_R;
335 dMULTIPLY0_331 (actual_pos,R,pos2);
336 actual_pos[0] += pos[0];
337 actual_pos[1] += pos[1];
338 actual_pos[2] += pos[2];
339 dMULTIPLY0_333 (actual_R,R,R2);
340 drawGeom (g2,actual_pos,actual_R,0);
341 }
342
343 if (show_aabb) {
344 // draw the bounding box for this geom
345 dReal aabb[6];
346 dGeomGetAABB (g,aabb);
347 dVector3 bbpos;
348 for (int i=0; i<3; i++) bbpos[i] = 0.5*(aabb[i*2] + aabb[i*2+1]);
349 dVector3 bbsides;
350 for (int j=0; j<3; j++) bbsides[j] = aabb[j*2+1] - aabb[j*2];
351 dMatrix3 RI;
352 dRSetIdentity (RI);
353 dsSetColorAlpha (1,0,0,0.5);
354 dsDrawBox (bbpos,RI,bbsides);
355 }
356}
357
358
359// simulation loop
360
361static void simLoop (int pause)
362{
363 dsSetColor (0,0,2);
364 dSpaceCollide (space,0,&nearCallback);
365 //if (!pause) dWorldStep (world,0.05);
366 //if (!pause) dWorldStepFast (world,0.05, 1);
367
368 // remove all contact joints
369 dJointGroupEmpty (contactgroup);
370
371 dsSetColor (1,1,0);
372 dsSetTexture (DS_WOOD);
373 for (int i=0; i<num; i++) {
374 for (int j=0; j < GPB; j++) {
375 if (i==selected) {
376 dsSetColor (0,0.7,1);
377 }
378 else if (! dBodyIsEnabled (obj[i].body)) {
379 dsSetColor (1,0,0);
380 }
381 else {
382 dsSetColor (1,1,0);
383 }
384 drawGeom (obj[i].geom[j],0,0,show_aabb);
385 }
386 }
387}
388
389
390int main (int argc, char **argv)
391{
392 // setup pointers to drawstuff callback functions
393 dsFunctions fn;
394 fn.version = DS_VERSION;
395 fn.start = &start;
396 fn.step = &simLoop;
397 fn.command = &command;
398 fn.stop = 0;
399 fn.path_to_textures = "../../drawstuff/textures";
400 if(argc==2)
401 {
402 fn.path_to_textures = argv[1];
403 }
404
405 // create world
406 dInitODE();
407 world = dWorldCreate();
408
409
410 dVector3 Center = {0, 0, 0, 0};
411 dVector3 Extents = {WORLD_SIZE * 0.55, WORLD_SIZE * 0.55, WORLD_SIZE * 0.55, 0};
412
413 //space = dSimpleSpaceCreate(0);
414 //space = dHashSpaceCreate (0);
415 space = dQuadTreeSpaceCreate (0, Center, Extents, 6);
416
417 contactgroup = dJointGroupCreate (0);
418 dWorldSetGravity (world,0,0,-0.5);
419 dWorldSetCFM (world,1e-5);
420 dCreatePlane (space,0,0,1,0);
421 memset (obj,0,sizeof(obj));
422
423 for (int i = 0; i < NUM; i++){
424 command('s');
425 }
426
427 // run simulation
428 dsSimulationLoop (argc,argv,352,288,&fn);
429
430 dJointGroupDestroy (contactgroup);
431 dSpaceDestroy (space);
432 dWorldDestroy (world);
433 dCloseODE();
434 return 0;
435}
generated by cgit v1.1 at 2024-12-28 08:02:08 +0000