aboutsummaryrefslogtreecommitdiffstatshomepage
path: root/libraries/ode-0.9/ode/demo/demo_boxstack.cpp
diff options
context:
space:
mode:
Diffstat (limited to 'libraries/ode-0.9/ode/demo/demo_boxstack.cpp')
-rw-r--r--libraries/ode-0.9/ode/demo/demo_boxstack.cpp577
1 files changed, 577 insertions, 0 deletions
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 @@
1/*************************************************************************
2 * *
3 * Open Dynamics Engine, Copyright (C) 2001,2002 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
31//<---- Convex Object
32dReal planes[]= // planes for a cube, these should coincide with the face array
33 {
34 1.0f ,0.0f ,0.0f ,0.25f,
35 0.0f ,1.0f ,0.0f ,0.25f,
36 0.0f ,0.0f ,1.0f ,0.25f,
37 -1.0f,0.0f ,0.0f ,0.25f,
38 0.0f ,-1.0f,0.0f ,0.25f,
39 0.0f ,0.0f ,-1.0f,0.25f
40 /*
41 1.0f ,0.0f ,0.0f ,2.0f,
42 0.0f ,1.0f ,0.0f ,1.0f,
43 0.0f ,0.0f ,1.0f ,1.0f,
44 0.0f ,0.0f ,-1.0f,1.0f,
45 0.0f ,-1.0f,0.0f ,1.0f,
46 -1.0f,0.0f ,0.0f ,0.0f
47 */
48 };
49const unsigned int planecount=6;
50
51dReal points[]= // points for a cube
52 {
53 0.25f,0.25f,0.25f, // point 0
54 -0.25f,0.25f,0.25f, // point 1
55
56 0.25f,-0.25f,0.25f, // point 2
57 -0.25f,-0.25f,0.25f,// point 3
58
59 0.25f,0.25f,-0.25f, // point 4
60 -0.25f,0.25f,-0.25f,// point 5
61
62 0.25f,-0.25f,-0.25f,// point 6
63 -0.25f,-0.25f,-0.25f,// point 7
64 };
65const unsigned int pointcount=8;
66unsigned int polygons[] = //Polygons for a cube (6 squares)
67 {
68 4,0,2,6,4, // positive X
69 4,1,0,4,5, // positive Y
70 4,0,1,3,2, // positive Z
71 4,3,1,5,7, // negative X
72 4,2,3,7,6, // negative Y
73 4,5,4,6,7, // negative Z
74 };
75//----> Convex Object
76
77// select correct drawing functions
78
79#ifdef dDOUBLE
80#define dsDrawBox dsDrawBoxD
81#define dsDrawSphere dsDrawSphereD
82#define dsDrawCylinder dsDrawCylinderD
83#define dsDrawCapsule dsDrawCapsuleD
84#define dsDrawConvex dsDrawConvexD
85#endif
86
87
88// some constants
89
90#define NUM 100 // max number of objects
91#define DENSITY (5.0) // density of all objects
92#define GPB 3 // maximum number of geometries per body
93#define MAX_CONTACTS 8 // maximum number of contact points per body
94#define USE_GEOM_OFFSET 1
95
96// dynamics and collision objects
97
98struct MyObject {
99 dBodyID body; // the body
100 dGeomID geom[GPB]; // geometries representing this body
101};
102
103static int num=0; // number of objects in simulation
104static int nextobj=0; // next object to recycle if num==NUM
105static dWorldID world;
106static dSpaceID space;
107static MyObject obj[NUM];
108static dJointGroupID contactgroup;
109static int selected = -1; // selected object
110static int show_aabb = 0; // show geom AABBs?
111static int show_contacts = 0; // show contact points?
112static int random_pos = 1; // drop objects from random position?
113static int write_world = 0;
114static int show_body = 1;
115
116// this is called by dSpaceCollide when two objects in space are
117// potentially colliding.
118
119static void nearCallback (void *data, dGeomID o1, dGeomID o2)
120{
121 int i;
122 // if (o1->body && o2->body) return;
123
124 // exit without doing anything if the two bodies are connected by a joint
125 dBodyID b1 = dGeomGetBody(o1);
126 dBodyID b2 = dGeomGetBody(o2);
127 if (b1 && b2 && dAreConnectedExcluding (b1,b2,dJointTypeContact)) return;
128
129 dContact contact[MAX_CONTACTS]; // up to MAX_CONTACTS contacts per box-box
130 for (i=0; i<MAX_CONTACTS; i++) {
131 contact[i].surface.mode = dContactBounce | dContactSoftCFM;
132 contact[i].surface.mu = dInfinity;
133 contact[i].surface.mu2 = 0;
134 contact[i].surface.bounce = 0.1;
135 contact[i].surface.bounce_vel = 0.1;
136 contact[i].surface.soft_cfm = 0.01;
137 }
138 if (int numc = dCollide (o1,o2,MAX_CONTACTS,&contact[0].geom,
139 sizeof(dContact))) {
140 dMatrix3 RI;
141 dRSetIdentity (RI);
142 const dReal ss[3] = {0.02,0.02,0.02};
143 for (i=0; i<numc; i++) {
144 dJointID c = dJointCreateContact (world,contactgroup,contact+i);
145 dJointAttach (c,b1,b2);
146 if (show_contacts) dsDrawBox (contact[i].geom.pos,RI,ss);
147 }
148 }
149}
150
151
152// start simulation - set viewpoint
153
154static void start()
155{
156 static float xyz[3] = {2.1640f,-1.3079f,1.7600f};
157 static float hpr[3] = {125.5000f,-17.0000f,0.0000f};
158 dsSetViewpoint (xyz,hpr);
159 printf ("To drop another object, press:\n");
160 printf (" b for box.\n");
161 printf (" s for sphere.\n");
162 printf (" c for capsule.\n");
163 printf (" y for cylinder.\n");
164 printf (" v for a convex object.\n");
165 printf (" x for a composite object.\n");
166 printf ("To select an object, press space.\n");
167 printf ("To disable the selected object, press d.\n");
168 printf ("To enable the selected object, press e.\n");
169 printf ("To toggle showing the geom AABBs, press a.\n");
170 printf ("To toggle showing the contact points, press t.\n");
171 printf ("To toggle dropping from random position/orientation, press r.\n");
172 printf ("To save the current state to 'state.dif', press 1.\n");
173}
174
175
176char locase (char c)
177{
178 if (c >= 'A' && c <= 'Z') return c - ('a'-'A');
179 else return c;
180}
181
182
183// called when a key pressed
184
185static void command (int cmd)
186{
187 size_t i;
188 int j,k;
189 dReal sides[3];
190 dMass m;
191 int setBody;
192
193 cmd = locase (cmd);
194 if (cmd == 'b' || cmd == 's' || cmd == 'c' || cmd == 'x' || cmd == 'y' || cmd == 'v')
195 {
196 setBody = 0;
197 if (num < NUM) {
198 i = num;
199 num++;
200 }
201 else {
202 i = nextobj;
203 nextobj++;
204 if (nextobj >= num) nextobj = 0;
205
206 // destroy the body and geoms for slot i
207 dBodyDestroy (obj[i].body);
208 for (k=0; k < GPB; k++) {
209 if (obj[i].geom[k]) dGeomDestroy (obj[i].geom[k]);
210 }
211 memset (&obj[i],0,sizeof(obj[i]));
212 }
213
214 obj[i].body = dBodyCreate (world);
215 for (k=0; k<3; k++) sides[k] = dRandReal()*0.5+0.1;
216
217 dMatrix3 R;
218 if (random_pos)
219 {
220 dBodySetPosition (obj[i].body,
221 dRandReal()*2-1,dRandReal()*2-1,dRandReal()+2);
222 dRFromAxisAndAngle (R,dRandReal()*2.0-1.0,dRandReal()*2.0-1.0,
223 dRandReal()*2.0-1.0,dRandReal()*10.0-5.0);
224 }
225 else
226 {
227 dReal maxheight = 0;
228 for (k=0; k<num; k++)
229 {
230 const dReal *pos = dBodyGetPosition (obj[k].body);
231 if (pos[2] > maxheight) maxheight = pos[2];
232 }
233 dBodySetPosition (obj[i].body, 0,0,maxheight+1);
234 dRSetIdentity (R);
235 //dRFromAxisAndAngle (R,0,0,1,/*dRandReal()*10.0-5.0*/0);
236 }
237 dBodySetRotation (obj[i].body,R);
238 dBodySetData (obj[i].body,(void*) i);
239
240 if (cmd == 'b') {
241 dMassSetBox (&m,DENSITY,sides[0],sides[1],sides[2]);
242 obj[i].geom[0] = dCreateBox (space,sides[0],sides[1],sides[2]);
243 }
244 else if (cmd == 'c') {
245 sides[0] *= 0.5;
246 dMassSetCapsule (&m,DENSITY,3,sides[0],sides[1]);
247 obj[i].geom[0] = dCreateCapsule (space,sides[0],sides[1]);
248 }
249 //<---- Convex Object
250 else if (cmd == 'v')
251 {
252 dMassSetBox (&m,DENSITY,0.25,0.25,0.25);
253 obj[i].geom[0] = dCreateConvex (space,
254 planes,
255 planecount,
256 points,
257 pointcount,
258 polygons);
259 }
260 //----> Convex Object
261 else if (cmd == 'y') {
262 dMassSetCylinder (&m,DENSITY,3,sides[0],sides[1]);
263 obj[i].geom[0] = dCreateCylinder (space,sides[0],sides[1]);
264 }
265 else if (cmd == 's') {
266 sides[0] *= 0.5;
267 dMassSetSphere (&m,DENSITY,sides[0]);
268 obj[i].geom[0] = dCreateSphere (space,sides[0]);
269 }
270 else if (cmd == 'x' && USE_GEOM_OFFSET) {
271 setBody = 1;
272 // start accumulating masses for the encapsulated geometries
273 dMass m2;
274 dMassSetZero (&m);
275
276 dReal dpos[GPB][3]; // delta-positions for encapsulated geometries
277 dMatrix3 drot[GPB];
278
279 // set random delta positions
280 for (j=0; j<GPB; j++) {
281 for (k=0; k<3; k++) dpos[j][k] = dRandReal()*0.3-0.15;
282 }
283
284 for (k=0; k<GPB; k++) {
285 if (k==0) {
286 dReal radius = dRandReal()*0.25+0.05;
287 obj[i].geom[k] = dCreateSphere (space,radius);
288 dMassSetSphere (&m2,DENSITY,radius);
289 }
290 else if (k==1) {
291 obj[i].geom[k] = dCreateBox (space,sides[0],sides[1],sides[2]);
292 dMassSetBox (&m2,DENSITY,sides[0],sides[1],sides[2]);
293 }
294 else {
295 dReal radius = dRandReal()*0.1+0.05;
296 dReal length = dRandReal()*1.0+0.1;
297 obj[i].geom[k] = dCreateCapsule (space,radius,length);
298 dMassSetCapsule (&m2,DENSITY,3,radius,length);
299 }
300
301 dRFromAxisAndAngle (drot[k],dRandReal()*2.0-1.0,dRandReal()*2.0-1.0,
302 dRandReal()*2.0-1.0,dRandReal()*10.0-5.0);
303 dMassRotate (&m2,drot[k]);
304
305 dMassTranslate (&m2,dpos[k][0],dpos[k][1],dpos[k][2]);
306
307 // add to the total mass
308 dMassAdd (&m,&m2);
309
310 }
311 for (k=0; k<GPB; k++) {
312 dGeomSetBody (obj[i].geom[k],obj[i].body);
313 dGeomSetOffsetPosition (obj[i].geom[k],
314 dpos[k][0]-m.c[0],
315 dpos[k][1]-m.c[1],
316 dpos[k][2]-m.c[2]);
317 dGeomSetOffsetRotation(obj[i].geom[k], drot[k]);
318 }
319 dMassTranslate (&m,-m.c[0],-m.c[1],-m.c[2]);
320 dBodySetMass (obj[i].body,&m);
321
322 }
323 else if (cmd == 'x') {
324 dGeomID g2[GPB]; // encapsulated geometries
325 dReal dpos[GPB][3]; // delta-positions for encapsulated geometries
326
327 // start accumulating masses for the encapsulated geometries
328 dMass m2;
329 dMassSetZero (&m);
330
331 // set random delta positions
332 for (j=0; j<GPB; j++) {
333 for (k=0; k<3; k++) dpos[j][k] = dRandReal()*0.3-0.15;
334 }
335
336 for (k=0; k<GPB; k++) {
337 obj[i].geom[k] = dCreateGeomTransform (space);
338 dGeomTransformSetCleanup (obj[i].geom[k],1);
339 if (k==0) {
340 dReal radius = dRandReal()*0.25+0.05;
341 g2[k] = dCreateSphere (0,radius);
342 dMassSetSphere (&m2,DENSITY,radius);
343 }
344 else if (k==1) {
345 g2[k] = dCreateBox (0,sides[0],sides[1],sides[2]);
346 dMassSetBox (&m2,DENSITY,sides[0],sides[1],sides[2]);
347 }
348 else {
349 dReal radius = dRandReal()*0.1+0.05;
350 dReal length = dRandReal()*1.0+0.1;
351 g2[k] = dCreateCapsule (0,radius,length);
352 dMassSetCapsule (&m2,DENSITY,3,radius,length);
353 }
354 dGeomTransformSetGeom (obj[i].geom[k],g2[k]);
355
356 // set the transformation (adjust the mass too)
357 dGeomSetPosition (g2[k],dpos[k][0],dpos[k][1],dpos[k][2]);
358 dMatrix3 Rtx;
359 dRFromAxisAndAngle (Rtx,dRandReal()*2.0-1.0,dRandReal()*2.0-1.0,
360 dRandReal()*2.0-1.0,dRandReal()*10.0-5.0);
361 dGeomSetRotation (g2[k],Rtx);
362 dMassRotate (&m2,Rtx);
363
364 // Translation *after* rotation
365 dMassTranslate (&m2,dpos[k][0],dpos[k][1],dpos[k][2]);
366
367 // add to the total mass
368 dMassAdd (&m,&m2);
369 }
370
371 // move all encapsulated objects so that the center of mass is (0,0,0)
372 for (k=0; k<GPB; k++) {
373 dGeomSetPosition (g2[k],
374 dpos[k][0]-m.c[0],
375 dpos[k][1]-m.c[1],
376 dpos[k][2]-m.c[2]);
377 }
378 dMassTranslate (&m,-m.c[0],-m.c[1],-m.c[2]);
379 }
380
381 if (!setBody)
382 for (k=0; k < GPB; k++) {
383 if (obj[i].geom[k]) dGeomSetBody (obj[i].geom[k],obj[i].body);
384 }
385
386 dBodySetMass (obj[i].body,&m);
387 }
388
389 if (cmd == ' ') {
390 selected++;
391 if (selected >= num) selected = 0;
392 if (selected < 0) selected = 0;
393 }
394 else if (cmd == 'd' && selected >= 0 && selected < num) {
395 dBodyDisable (obj[selected].body);
396 }
397 else if (cmd == 'e' && selected >= 0 && selected < num) {
398 dBodyEnable (obj[selected].body);
399 }
400 else if (cmd == 'a') {
401 show_aabb ^= 1;
402 }
403 else if (cmd == 't') {
404 show_contacts ^= 1;
405 }
406 else if (cmd == 'r') {
407 random_pos ^= 1;
408 }
409 else if (cmd == '1') {
410 write_world = 1;
411 }
412}
413
414
415// draw a geom
416
417void drawGeom (dGeomID g, const dReal *pos, const dReal *R, int show_aabb)
418{
419 int i;
420
421 if (!g) return;
422 if (!pos) pos = dGeomGetPosition (g);
423 if (!R) R = dGeomGetRotation (g);
424
425 int type = dGeomGetClass (g);
426 if (type == dBoxClass) {
427 dVector3 sides;
428 dGeomBoxGetLengths (g,sides);
429 dsDrawBox (pos,R,sides);
430 }
431 else if (type == dSphereClass) {
432 dsDrawSphere (pos,R,dGeomSphereGetRadius (g));
433 }
434 else if (type == dCapsuleClass) {
435 dReal radius,length;
436 dGeomCapsuleGetParams (g,&radius,&length);
437 dsDrawCapsule (pos,R,length,radius);
438 }
439 //<---- Convex Object
440 else if (type == dConvexClass)
441 {
442 //dVector3 sides={0.50,0.50,0.50};
443 dsDrawConvex(pos,R,planes,
444 planecount,
445 points,
446 pointcount,
447 polygons);
448 }
449 //----> Convex Object
450 else if (type == dCylinderClass) {
451 dReal radius,length;
452 dGeomCylinderGetParams (g,&radius,&length);
453 dsDrawCylinder (pos,R,length,radius);
454 }
455 else if (type == dGeomTransformClass) {
456 dGeomID g2 = dGeomTransformGetGeom (g);
457 const dReal *pos2 = dGeomGetPosition (g2);
458 const dReal *R2 = dGeomGetRotation (g2);
459 dVector3 actual_pos;
460 dMatrix3 actual_R;
461 dMULTIPLY0_331 (actual_pos,R,pos2);
462 actual_pos[0] += pos[0];
463 actual_pos[1] += pos[1];
464 actual_pos[2] += pos[2];
465 dMULTIPLY0_333 (actual_R,R,R2);
466 drawGeom (g2,actual_pos,actual_R,0);
467 }
468 if (show_body) {
469 dBodyID body = dGeomGetBody(g);
470 if (body) {
471 const dReal *bodypos = dBodyGetPosition (body);
472 const dReal *bodyr = dBodyGetRotation (body);
473 dReal bodySides[3] = { 0.1, 0.1, 0.1 };
474 dsSetColorAlpha(0,1,0,1);
475 dsDrawBox(bodypos,bodyr,bodySides);
476 }
477 }
478 if (show_aabb) {
479 // draw the bounding box for this geom
480 dReal aabb[6];
481 dGeomGetAABB (g,aabb);
482 dVector3 bbpos;
483 for (i=0; i<3; i++) bbpos[i] = 0.5*(aabb[i*2] + aabb[i*2+1]);
484 dVector3 bbsides;
485 for (i=0; i<3; i++) bbsides[i] = aabb[i*2+1] - aabb[i*2];
486 dMatrix3 RI;
487 dRSetIdentity (RI);
488 dsSetColorAlpha (1,0,0,0.5);
489 dsDrawBox (bbpos,RI,bbsides);
490 }
491}
492
493
494// simulation loop
495
496static void simLoop (int pause)
497{
498 dsSetColor (0,0,2);
499 dSpaceCollide (space,0,&nearCallback);
500 if (!pause) dWorldQuickStep (world,0.02);
501
502 if (write_world) {
503 FILE *f = fopen ("state.dif","wt");
504 if (f) {
505 dWorldExportDIF (world,f,"X");
506 fclose (f);
507 }
508 write_world = 0;
509 }
510
511 // remove all contact joints
512 dJointGroupEmpty (contactgroup);
513
514 dsSetColor (1,1,0);
515 dsSetTexture (DS_WOOD);
516 for (int i=0; i<num; i++) {
517 for (int j=0; j < GPB; j++) {
518 if (i==selected) {
519 dsSetColor (0,0.7,1);
520 }
521 else if (! dBodyIsEnabled (obj[i].body)) {
522 dsSetColor (1,0.8,0);
523 }
524 else {
525 dsSetColor (1,1,0);
526 }
527 drawGeom (obj[i].geom[j],0,0,show_aabb);
528 }
529 }
530}
531
532
533int main (int argc, char **argv)
534{
535 // setup pointers to drawstuff callback functions
536 dsFunctions fn;
537 fn.version = DS_VERSION;
538 fn.start = &start;
539 fn.step = &simLoop;
540 fn.command = &command;
541 fn.stop = 0;
542 fn.path_to_textures = "../../drawstuff/textures";
543 if(argc==2)
544 {
545 fn.path_to_textures = argv[1];
546 }
547
548 // create world
549 dInitODE();
550 world = dWorldCreate();
551 space = dHashSpaceCreate (0);
552 contactgroup = dJointGroupCreate (0);
553 dWorldSetGravity (world,0,0,-0.5);
554 dWorldSetCFM (world,1e-5);
555 dWorldSetAutoDisableFlag (world,1);
556
557#if 1
558
559 dWorldSetAutoDisableAverageSamplesCount( world, 10 );
560
561#endif
562
563
564 dWorldSetContactMaxCorrectingVel (world,0.1);
565 dWorldSetContactSurfaceLayer (world,0.001);
566 dCreatePlane (space,0,0,1,0);
567 memset (obj,0,sizeof(obj));
568
569 // run simulation
570 dsSimulationLoop (argc,argv,352,288,&fn);
571
572 dJointGroupDestroy (contactgroup);
573 dSpaceDestroy (space);
574 dWorldDestroy (world);
575 dCloseODE();
576 return 0;
577}