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