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 #include "texturepath.h"
26 #include "bunny_geom.h"
27
28 #ifdef _MSC_VER
29 #pragma warning(disable:4244 4305) // for VC++, no precision loss complaints
30 #endif
31
32
33 #define DEGTORAD 0.01745329251994329577f //!< PI / 180.0, convert degrees to radians
34
35 int g_allow_trimesh;
36
37 // Our heightfield geom
38 dGeomID gheight;
39
40
41
42 // Heightfield dimensions
43
44 #define HFIELD_WSTEP 15 // Vertex count along edge >= 2
45 #define HFIELD_DSTEP 31
46
47 #define HFIELD_WIDTH REAL( 4.0 )
48 #define HFIELD_DEPTH REAL( 8.0 )
49
50 #define HFIELD_WSAMP ( HFIELD_WIDTH / ( HFIELD_WSTEP-1 ) )
51 #define HFIELD_DSAMP ( HFIELD_DEPTH / ( HFIELD_DSTEP-1 ) )
52
53
54
55 //<---- Convex Object
56 dReal planes[]= // planes for a cube
57 {
58 1.0f ,0.0f ,0.0f ,0.25f,
59 0.0f ,1.0f ,0.0f ,0.25f,
60 0.0f ,0.0f ,1.0f ,0.25f,
61 0.0f ,0.0f ,-1.0f,0.25f,
62 0.0f ,-1.0f,0.0f ,0.25f,
63 -1.0f,0.0f ,0.0f ,0.25f
64 /*
65 1.0f ,0.0f ,0.0f ,2.0f,
66 0.0f ,1.0f ,0.0f ,1.0f,
67 0.0f ,0.0f ,1.0f ,1.0f,
68 0.0f ,0.0f ,-1.0f,1.0f,
69 0.0f ,-1.0f,0.0f ,1.0f,
70 -1.0f,0.0f ,0.0f ,0.0f
71 */
72 };
73 const unsigned int planecount=6;
74
75 dReal points[]= // points for a cube
76 {
77 0.25f,0.25f,0.25f, // point 0
78 -0.25f,0.25f,0.25f, // point 1
79
80 0.25f,-0.25f,0.25f, // point 2
81 -0.25f,-0.25f,0.25f,// point 3
82
83 0.25f,0.25f,-0.25f, // point 4
84 -0.25f,0.25f,-0.25f,// point 5
85
86 0.25f,-0.25f,-0.25f,// point 6
87 -0.25f,-0.25f,-0.25f,// point 7
88 };
89 const unsigned int pointcount=8;
90 unsigned int polygons[] = //Polygons for a cube (6 squares)
91 {
92 4,0,2,6,4, // positive X
93 4,1,0,4,5, // positive Y
94 4,0,1,3,2, // positive Z
95 4,3,1,5,7, // negative X
96 4,2,3,7,6, // negative Y
97 4,5,4,6,7, // negative Z
98 };
99 //----> Convex Object
100
101 // select correct drawing functions
102
103 #ifdef dDOUBLE
104 #define dsDrawBox dsDrawBoxD
105 #define dsDrawSphere dsDrawSphereD
106 #define dsDrawCylinder dsDrawCylinderD
107 #define dsDrawCapsule dsDrawCapsuleD
108 #define dsDrawConvex dsDrawConvexD
109 #define dsDrawTriangle dsDrawTriangleD
110 #endif
111
112
113 // some constants
114
115 #define NUM 100 // max number of objects
116 #define DENSITY (5.0) // density of all objects
117 #define GPB 3 // maximum number of geometries per body
118 #define MAX_CONTACTS 64 // maximum number of contact points per body
119
120
121 // dynamics and collision objects
122
123 struct MyObject {
124 dBodyID body; // the body
125 dGeomID geom[GPB]; // geometries representing this body
126
127 // Trimesh only - double buffered matrices for 'last transform' setup
128 dReal matrix_dblbuff[ 16 * 2 ];
129 int last_matrix_index;
130 };
131
132 static int num=0; // number of objects in simulation
133 static int nextobj=0; // next object to recycle if num==NUM
134 static dWorldID world;
135 static dSpaceID space;
136 static MyObject obj[NUM];
137 static dJointGroupID contactgroup;
138 static int selected = -1; // selected object
139 static int show_aabb = 0; // show geom AABBs?
140 static int show_contacts = 0; // show contact points?
141 static int random_pos = 1; // drop objects from random position?
142 static int write_world = 0;
143
144
145
146
147 //============================
148
149 dGeomID TriMesh1;
150 dGeomID TriMesh2;
151 //static dTriMeshDataID TriData1, TriData2; // reusable static trimesh data
152
153 //============================
154
155
heightfield_callback(void * pUserData,int x,int z)156 dReal heightfield_callback( void* pUserData, int x, int z )
157 {
158 dReal fx = ( ((dReal)x) - ( HFIELD_WSTEP-1 )/2 ) / (dReal)( HFIELD_WSTEP-1 );
159 dReal fz = ( ((dReal)z) - ( HFIELD_DSTEP-1 )/2 ) / (dReal)( HFIELD_DSTEP-1 );
160
161 // Create an interesting 'hump' shape
162 dReal h = REAL( 1.0 ) + ( REAL( -16.0 ) * ( fx*fx*fx + fz*fz*fz ) );
163
164 return h;
165 }
166
167
168
169
170 // this is called by dSpaceCollide when two objects in space are
171 // potentially colliding.
172
nearCallback(void * data,dGeomID o1,dGeomID o2)173 static void nearCallback (void *data, dGeomID o1, dGeomID o2)
174 {
175 int i;
176 // if (o1->body && o2->body) return;
177
178 // exit without doing anything if the two bodies are connected by a joint
179 dBodyID b1 = dGeomGetBody(o1);
180 dBodyID b2 = dGeomGetBody(o2);
181 if (b1 && b2 && dAreConnectedExcluding (b1,b2,dJointTypeContact)) return;
182
183 dContact contact[MAX_CONTACTS]; // up to MAX_CONTACTS contacts per box-box
184 for (i=0; i<MAX_CONTACTS; i++) {
185 contact[i].surface.mode = dContactBounce | dContactSoftCFM;
186 contact[i].surface.mu = dInfinity;
187 contact[i].surface.mu2 = 0;
188 contact[i].surface.bounce = 0.1;
189 contact[i].surface.bounce_vel = 0.1;
190 contact[i].surface.soft_cfm = 0.01;
191 }
192 if (int numc = dCollide (o1,o2,MAX_CONTACTS,&contact[0].geom,
193 sizeof(dContact))) {
194 dMatrix3 RI;
195 dRSetIdentity (RI);
196 const dReal ss[3] = {0.02,0.02,0.02};
197 for (i=0; i<numc; i++) {
198 dJointID c = dJointCreateContact (world,contactgroup,contact+i);
199 dJointAttach (c,b1,b2);
200 if (show_contacts) dsDrawBox (contact[i].geom.pos,RI,ss);
201 }
202 }
203 }
204
205
206 // start simulation - set viewpoint
207
start()208 static void start()
209 {
210 dAllocateODEDataForThread(dAllocateMaskAll);
211
212 static float xyz[3] = {2.1640f,-1.3079f,1.7600f};
213 static float hpr[3] = {125.5000f,-17.0000f,0.0000f};
214 dsSetViewpoint (xyz,hpr);
215 printf ("To drop another object, press:\n");
216 printf (" b for box.\n");
217 printf (" s for sphere.\n");
218 printf (" c for capsule.\n");
219 printf (" y for cylinder.\n");
220 printf (" v for a convex object.\n");
221 printf (" x for a composite object.\n");
222 if ( g_allow_trimesh )
223 printf (" m for a trimesh.\n");
224 printf ("To select an object, press space.\n");
225 printf ("To disable the selected object, press d.\n");
226 printf ("To enable the selected object, press e.\n");
227 printf ("To toggle showing the geom AABBs, press a.\n");
228 printf ("To toggle showing the contact points, press t.\n");
229 printf ("To toggle dropping from random position/orientation, press r.\n");
230 printf ("To save the current state to 'state.dif', press 1.\n");
231 }
232
233
locase(char c)234 char locase (char c)
235 {
236 if (c >= 'A' && c <= 'Z') return c - ('a'-'A');
237 else return c;
238 }
239
240
241 // called when a key pressed
242
command(int cmd)243 static void command (int cmd)
244 {
245 size_t i;
246 int j,k;
247 dReal sides[3];
248 dMass m;
249
250 cmd = locase (cmd);
251
252
253 //
254 // Geom Creation
255 //
256
257 if ( cmd == 'b' || cmd == 's' || cmd == 'c' || ( cmd == 'm' && g_allow_trimesh ) ||
258 cmd == 'x' || cmd == 'y' || cmd == 'v' )
259 {
260 if ( num < NUM )
261 {
262 i = num;
263 num++;
264 }
265 else
266 {
267 i = nextobj;
268 nextobj++;
269 if (nextobj >= num) nextobj = 0;
270
271 // destroy the body and geoms for slot i
272 dBodyDestroy (obj[i].body);
273 for (k=0; k < GPB; k++)
274 {
275 if (obj[i].geom[k]) dGeomDestroy (obj[i].geom[k]);
276 }
277 memset (&obj[i],0,sizeof(obj[i]));
278 }
279
280 obj[i].body = dBodyCreate (world);
281 for (k=0; k<3; k++) sides[k] = dRandReal()*0.5+0.1;
282
283 dMatrix3 R;
284 if (random_pos) {
285 dBodySetPosition (obj[i].body,
286 (dRandReal()-0.5)*HFIELD_WIDTH*0.75,
287 (dRandReal()-0.5)*HFIELD_DEPTH*0.75,
288 dRandReal() + 2 );
289 dRFromAxisAndAngle (R,dRandReal()*2.0-1.0,dRandReal()*2.0-1.0,
290 dRandReal()*2.0-1.0,dRandReal()*10.0-5.0);
291 }
292 else {
293 dReal maxheight = 0;
294 for (k=0; k<num; k++) {
295 const dReal *pos = dBodyGetPosition (obj[k].body);
296 if (pos[2] > maxheight) maxheight = pos[2];
297 }
298 dBodySetPosition (obj[i].body, 0,maxheight+1,0);
299 dRFromAxisAndAngle (R,0,0,1,dRandReal()*10.0-5.0);
300 }
301 dBodySetRotation (obj[i].body,R);
302 dBodySetData (obj[i].body,(void*) i);
303
304 if (cmd == 'b')
305 {
306 dMassSetBox (&m,DENSITY,sides[0],sides[1],sides[2]);
307 obj[i].geom[0] = dCreateBox (space,sides[0],sides[1],sides[2]);
308 }
309 else if (cmd == 'c')
310 {
311 sides[0] *= 0.5;
312 dMassSetCapsule (&m,DENSITY,3,sides[0],sides[1]);
313 obj[i].geom[0] = dCreateCapsule (space,sides[0],sides[1]);
314 }
315 //<---- Convex Object
316 else if (cmd == 'v')
317 {
318 dMassSetBox (&m,DENSITY,0.25,0.25,0.25);
319 obj[i].geom[0] = dCreateConvex (space,
320 planes,
321 planecount,
322 points,
323 pointcount,
324 polygons);
325 }
326 //----> Convex Object
327 else if (cmd == 'y')
328 {
329 dMassSetCylinder (&m,DENSITY,3,sides[0],sides[1]);
330 obj[i].geom[0] = dCreateCylinder (space,sides[0],sides[1]);
331 }
332 else if (cmd == 's')
333 {
334 sides[0] *= 0.5;
335 dMassSetSphere (&m,DENSITY,sides[0]);
336 obj[i].geom[0] = dCreateSphere (space,sides[0]);
337 }
338 else if (cmd == 'm' && g_allow_trimesh)
339 {
340 dTriMeshDataID new_tmdata = dGeomTriMeshDataCreate();
341 dGeomTriMeshDataBuildSingle(new_tmdata, &Vertices[0], 3 * sizeof(float), VertexCount,
342 &Indices[0], IndexCount, 3 * sizeof(dTriIndex));
343
344 obj[i].geom[0] = dCreateTriMesh(space, new_tmdata, 0, 0, 0);
345
346 dMassSetTrimesh( &m, DENSITY, obj[i].geom[0] );
347 printf("mass at %f %f %f\n", m.c[0], m.c[1], m.c[2]);
348 dGeomSetPosition(obj[i].geom[0], -m.c[0], -m.c[1], -m.c[2]);
349 dMassTranslate(&m, -m.c[0], -m.c[1], -m.c[2]);
350 }
351 else if (cmd == 'x')
352 {
353 dGeomID g2[GPB]; // encapsulated geometries
354 dReal dpos[GPB][3]; // delta-positions for encapsulated geometries
355
356 // start accumulating masses for the encapsulated geometries
357 dMass m2;
358 dMassSetZero (&m);
359
360 // set random delta positions
361 for (j=0; j<GPB; j++) {
362 for (k=0; k<3; k++) dpos[j][k] = dRandReal()*0.3-0.15;
363 }
364
365 for (k=0; k<GPB; k++) {
366 obj[i].geom[k] = dCreateGeomTransform (space);
367 dGeomTransformSetCleanup (obj[i].geom[k],1);
368 if (k==0) {
369 dReal radius = dRandReal()*0.25+0.05;
370 g2[k] = dCreateSphere (0,radius);
371 dMassSetSphere (&m2,DENSITY,radius);
372 }
373 else if (k==1) {
374 g2[k] = dCreateBox (0,sides[0],sides[1],sides[2]);
375 dMassSetBox (&m2,DENSITY,sides[0],sides[1],sides[2]);
376 }
377 else {
378 dReal radius = dRandReal()*0.1+0.05;
379 dReal length = dRandReal()*1.0+0.1;
380 g2[k] = dCreateCapsule (0,radius,length);
381 dMassSetCapsule (&m2,DENSITY,3,radius,length);
382 }
383 dGeomTransformSetGeom (obj[i].geom[k],g2[k]);
384
385 // set the transformation (adjust the mass too)
386 dGeomSetPosition (g2[k],dpos[k][0],dpos[k][1],dpos[k][2]);
387 dMassTranslate (&m2,dpos[k][0],dpos[k][1],dpos[k][2]);
388 dMatrix3 Rtx;
389 dRFromAxisAndAngle (Rtx,dRandReal()*2.0-1.0,dRandReal()*2.0-1.0,
390 dRandReal()*2.0-1.0,dRandReal()*10.0-5.0);
391 dGeomSetRotation (g2[k],Rtx);
392 dMassRotate (&m2,Rtx);
393
394 // add to the total mass
395 dMassAdd (&m,&m2);
396 }
397
398 // move all encapsulated objects so that the center of mass is (0,0,0)
399 for (k=0; k<2; k++) {
400 dGeomSetPosition (g2[k],
401 dpos[k][0]-m.c[0],
402 dpos[k][1]-m.c[1],
403 dpos[k][2]-m.c[2]);
404 }
405 dMassTranslate (&m,-m.c[0],-m.c[1],-m.c[2]);
406 }
407
408 for (k=0; k < GPB; k++)
409 {
410 if (obj[i].geom[k]) dGeomSetBody (obj[i].geom[k],obj[i].body);
411 }
412
413 dBodySetMass (obj[i].body,&m);
414 }
415
416
417 //
418 // Control Commands
419 //
420
421 if (cmd == ' ') {
422 selected++;
423 if (selected >= num) selected = 0;
424 if (selected < 0) selected = 0;
425 }
426 else if (cmd == 'd' && selected >= 0 && selected < num) {
427 dBodyDisable (obj[selected].body);
428 }
429 else if (cmd == 'e' && selected >= 0 && selected < num) {
430 dBodyEnable (obj[selected].body);
431 }
432 else if (cmd == 'a') {
433 show_aabb ^= 1;
434 }
435 else if (cmd == 't') {
436 show_contacts ^= 1;
437 }
438 else if (cmd == 'r') {
439 random_pos ^= 1;
440 }
441 else if (cmd == '1') {
442 write_world = 1;
443 }
444 }
445
446
447 // draw a geom
448
drawGeom(dGeomID g,const dReal * pos,const dReal * R,int show_aabb)449 void drawGeom (dGeomID g, const dReal *pos, const dReal *R, int show_aabb)
450 {
451 int i;
452
453 if (!g) return;
454 if (!pos) pos = dGeomGetPosition (g);
455 if (!R) R = dGeomGetRotation (g);
456
457 int type = dGeomGetClass (g);
458 if (type == dBoxClass) {
459 dVector3 sides;
460 dGeomBoxGetLengths (g,sides);
461 dsDrawBox (pos,R,sides);
462 }
463 else if (type == dSphereClass) {
464 dsDrawSphere (pos,R,dGeomSphereGetRadius (g));
465 }
466 else if (type == dCapsuleClass) {
467 dReal radius,length;
468 dGeomCapsuleGetParams (g,&radius,&length);
469 dsDrawCapsule (pos,R,length,radius);
470 }
471 //<---- Convex Object
472 else if (type == dConvexClass)
473 {
474 //dVector3 sides={0.50,0.50,0.50};
475 dsDrawConvex(pos,R,planes,
476 planecount,
477 points,
478 pointcount,
479 polygons);
480 }
481 //----> Convex Object
482 else if (type == dCylinderClass) {
483 dReal radius,length;
484 dGeomCylinderGetParams (g,&radius,&length);
485 dsDrawCylinder (pos,R,length,radius);
486 }
487 else if (type == dGeomTransformClass) {
488 dGeomID g2 = dGeomTransformGetGeom (g);
489 const dReal *pos2 = dGeomGetPosition (g2);
490 const dReal *R2 = dGeomGetRotation (g2);
491 dVector3 actual_pos;
492 dMatrix3 actual_R;
493 dMultiply0_331 (actual_pos,R,pos2);
494 actual_pos[0] += pos[0];
495 actual_pos[1] += pos[1];
496 actual_pos[2] += pos[2];
497 dMultiply0_333 (actual_R,R,R2);
498 drawGeom (g2,actual_pos,actual_R,0);
499 }
500
501 if (show_aabb) {
502 // draw the bounding box for this geom
503 dReal aabb[6];
504 dGeomGetAABB (g,aabb);
505 dVector3 bbpos;
506 for (i=0; i<3; i++) bbpos[i] = 0.5*(aabb[i*2] + aabb[i*2+1]);
507 dVector3 bbsides;
508 for (i=0; i<3; i++) bbsides[i] = aabb[i*2+1] - aabb[i*2];
509 dMatrix3 RI;
510 dRSetIdentity (RI);
511 dsSetColorAlpha (1,0,0,0.5);
512 dsDrawBox (bbpos,RI,bbsides);
513 }
514
515 }
516
517 // simulation loop
518
simLoop(int pause)519 static void simLoop (int pause)
520 {
521 int i,j;
522
523 dsSetColor (0,0,2);
524
525 dSpaceCollide (space,0,&nearCallback);
526
527 //if (!pause) dWorldStep (world,0.05);
528 if (!pause) dWorldQuickStep (world,0.05);
529
530
531 if (write_world) {
532 FILE *f = fopen ("state.dif","wt");
533 if (f) {
534 dWorldExportDIF (world,f,"X");
535 fclose (f);
536 }
537 write_world = 0;
538 }
539
540 // remove all contact joints
541 dJointGroupEmpty (contactgroup);
542
543
544
545 const dReal* pReal = dGeomGetPosition( gheight );
546
547 const dReal* RReal = dGeomGetRotation( gheight );
548
549 //
550 // Draw Heightfield
551 //
552
553 // Set ox and oz to zero for DHEIGHTFIELD_CORNER_ORIGIN mode.
554 int ox = (int) ( -HFIELD_WIDTH/2 );
555 int oz = (int) ( -HFIELD_DEPTH/2 );
556
557 // for ( int tx = -1; tx < 2; ++tx )
558 // for ( int tz = -1; tz < 2; ++tz )
559 {
560 dsSetColorAlpha (0.5,1,0.5,0.5);
561 dsSetTexture( DS_WOOD );
562
563 for ( int i = 0; i < HFIELD_WSTEP - 1; ++i )
564 for ( int j = 0; j < HFIELD_DSTEP - 1; ++j )
565 {
566 dReal a[3], b[3], c[3], d[3];
567
568 a[ 0 ] = ox + ( i ) * HFIELD_WSAMP;
569 a[ 1 ] = heightfield_callback( NULL, i, j );
570 a[ 2 ] = oz + ( j ) * HFIELD_DSAMP;
571
572 b[ 0 ] = ox + ( i + 1 ) * HFIELD_WSAMP;
573 b[ 1 ] = heightfield_callback( NULL, i + 1, j );
574 b[ 2 ] = oz + ( j ) * HFIELD_DSAMP;
575
576 c[ 0 ] = ox + ( i ) * HFIELD_WSAMP;
577 c[ 1 ] = heightfield_callback( NULL, i, j + 1 );
578 c[ 2 ] = oz + ( j + 1 ) * HFIELD_DSAMP;
579
580 d[ 0 ] = ox + ( i + 1 ) * HFIELD_WSAMP;
581 d[ 1 ] = heightfield_callback( NULL, i + 1, j + 1 );
582 d[ 2 ] = oz + ( j + 1 ) * HFIELD_DSAMP;
583
584 dsDrawTriangle( pReal, RReal, a, c, b, 1 );
585 dsDrawTriangle( pReal, RReal, b, c, d, 1 );
586 }
587 }
588
589
590
591
592
593 dsSetColor (1,1,0);
594 dsSetTexture (DS_WOOD);
595 for (i=0; i<num; i++)
596 {
597 for (j=0; j < GPB; j++)
598 {
599 if (i==selected)
600 {
601 dsSetColor (0,0.7,1);
602 }
603 else if (! dBodyIsEnabled (obj[i].body))
604 {
605 dsSetColor (1,0.8,0);
606 }
607 else
608 {
609 dsSetColor (1,1,0);
610 }
611
612 if ( obj[i].geom[j] && dGeomGetClass(obj[i].geom[j]) == dTriMeshClass )
613 {
614 dTriIndex* Indices = (dTriIndex*)::Indices;
615
616 // assume all trimeshes are drawn as bunnies
617 const dReal* Pos = dGeomGetPosition(obj[i].geom[j]);
618 const dReal* Rot = dGeomGetRotation(obj[i].geom[j]);
619
620 for (int ii = 0; ii < IndexCount / 3; ii++)
621 {
622 const dReal v[9] = { // explicit conversion from float to dReal
623 Vertices[Indices[ii * 3 + 0] * 3 + 0],
624 Vertices[Indices[ii * 3 + 0] * 3 + 1],
625 Vertices[Indices[ii * 3 + 0] * 3 + 2],
626 Vertices[Indices[ii * 3 + 1] * 3 + 0],
627 Vertices[Indices[ii * 3 + 1] * 3 + 1],
628 Vertices[Indices[ii * 3 + 1] * 3 + 2],
629 Vertices[Indices[ii * 3 + 2] * 3 + 0],
630 Vertices[Indices[ii * 3 + 2] * 3 + 1],
631 Vertices[Indices[ii * 3 + 2] * 3 + 2]
632 };
633 dsDrawTriangle(Pos, Rot, &v[0], &v[3], &v[6], 1);
634 }
635
636 // tell the tri-tri collider the current transform of the trimesh --
637 // this is fairly important for good results.
638
639 // Fill in the (4x4) matrix.
640 dReal* p_matrix = obj[i].matrix_dblbuff + ( obj[i].last_matrix_index * 16 );
641
642 p_matrix[ 0 ] = Rot[ 0 ]; p_matrix[ 1 ] = Rot[ 1 ]; p_matrix[ 2 ] = Rot[ 2 ]; p_matrix[ 3 ] = 0;
643 p_matrix[ 4 ] = Rot[ 4 ]; p_matrix[ 5 ] = Rot[ 5 ]; p_matrix[ 6 ] = Rot[ 6 ]; p_matrix[ 7 ] = 0;
644 p_matrix[ 8 ] = Rot[ 8 ]; p_matrix[ 9 ] = Rot[ 9 ]; p_matrix[10 ] = Rot[10 ]; p_matrix[11 ] = 0;
645 p_matrix[12 ] = Pos[ 0 ]; p_matrix[13 ] = Pos[ 1 ]; p_matrix[14 ] = Pos[ 2 ]; p_matrix[15 ] = 1;
646
647 // Flip to other matrix.
648 obj[i].last_matrix_index = !obj[i].last_matrix_index;
649
650 // Apply the 'other' matrix which is the oldest.
651 dGeomTriMeshSetLastTransform( obj[i].geom[j],
652 *(dMatrix4*)( obj[i].matrix_dblbuff + ( obj[i].last_matrix_index * 16 ) ) );
653 }
654 else
655 {
656 drawGeom (obj[i].geom[j],0,0,show_aabb);
657 }
658 }
659 }
660
661 if ( show_aabb )
662 {
663 // draw the bounding box for this geom
664 dReal aabb[6];
665 dGeomGetAABB (gheight,aabb);
666 dVector3 bbpos;
667 for (i=0; i<3; i++) bbpos[i] = 0.5*(aabb[i*2] + aabb[i*2+1]);
668 dVector3 bbsides;
669 for (i=0; i<3; i++) bbsides[i] = aabb[i*2+1] - aabb[i*2];
670 dMatrix3 RI;
671 dRSetIdentity (RI);
672 dsSetColorAlpha (1,0,0,0.5);
673 dsDrawBox (bbpos,RI,bbsides);
674 }
675 }
676
677
main(int argc,char ** argv)678 int main (int argc, char **argv)
679 {
680 printf("ODE configuration: %s\n", dGetConfiguration());
681
682 // Is trimesh support built into this ODE?
683 g_allow_trimesh = dCheckConfiguration( "ODE_EXT_trimesh" );
684
685 // setup pointers to drawstuff callback functions
686 dsFunctions fn;
687 fn.version = DS_VERSION;
688 fn.start = &start;
689 fn.step = &simLoop;
690 fn.command = &command;
691 fn.stop = 0;
692 fn.path_to_textures = DRAWSTUFF_TEXTURE_PATH;
693
694 // create world
695 dInitODE2(0);
696 world = dWorldCreate();
697 space = dHashSpaceCreate (0);
698 contactgroup = dJointGroupCreate (0);
699 dWorldSetGravity (world,0,0,-0.05);
700 dWorldSetCFM (world,1e-5);
701 dWorldSetAutoDisableFlag (world,1);
702 dWorldSetContactMaxCorrectingVel (world,0.1);
703 dWorldSetContactSurfaceLayer (world,0.001);
704 memset (obj,0,sizeof(obj));
705
706 #if 1
707
708 dWorldSetAutoDisableAverageSamplesCount( world, 1 );
709
710 #endif
711
712 // base plane to catch overspill
713 dCreatePlane( space, 0, 0, 1, 0 );
714
715
716 // our heightfield floor
717
718 dHeightfieldDataID heightid = dGeomHeightfieldDataCreate();
719
720 // Create an finite heightfield.
721 dGeomHeightfieldDataBuildCallback( heightid, NULL, heightfield_callback,
722 HFIELD_WIDTH, HFIELD_DEPTH, HFIELD_WSTEP, HFIELD_DSTEP,
723 REAL( 1.0 ), REAL( 0.0 ), REAL( 0.0 ), 0 );
724
725 // Give some very bounds which, while conservative,
726 // makes AABB computation more accurate than +/-INF.
727 dGeomHeightfieldDataSetBounds( heightid, REAL( -4.0 ), REAL( +6.0 ) );
728
729 gheight = dCreateHeightfield( space, heightid, 1 );
730
731 dVector3 pos;
732 pos[ 0 ] = 0;
733 pos[ 1 ] = 0;
734 pos[ 2 ] = 0;
735
736 // Rotate so Z is up, not Y (which is the default orientation)
737 dMatrix3 R;
738 dRSetIdentity( R );
739 dRFromAxisAndAngle( R, 1, 0, 0, DEGTORAD * 90 );
740
741 // Place it.
742 dGeomSetRotation( gheight, R );
743 dGeomSetPosition( gheight, pos[0], pos[1], pos[2] );
744
745 dThreadingImplementationID threading = dThreadingAllocateMultiThreadedImplementation();
746 dThreadingThreadPoolID pool = dThreadingAllocateThreadPool(4, 0, dAllocateFlagBasicData, NULL);
747 dThreadingThreadPoolServeMultiThreadedImplementation(pool, threading);
748 // dWorldSetStepIslandsProcessingMaxThreadCount(world, 1);
749 dWorldSetStepThreadingImplementation(world, dThreadingImplementationGetFunctions(threading), threading);
750
751 // run simulation
752 dsSimulationLoop (argc,argv,352,288,&fn);
753
754 dThreadingImplementationShutdownProcessing(threading);
755 dThreadingFreeThreadPool(pool);
756 dWorldSetStepThreadingImplementation(world, NULL, NULL);
757 dThreadingFreeImplementation(threading);
758
759 dJointGroupDestroy (contactgroup);
760 dSpaceDestroy (space);
761 dWorldDestroy (world);
762
763 // destroy heightfield data, because _we_ own it not ODE
764 dGeomHeightfieldDataDestroy( heightid );
765
766 dCloseODE();
767 return 0;
768 }
769