Planet

navi

home

PPS

about

screenshots

download

development

forum

Context Navigation

source: code/branches/ode/ode-0.9/ode/demo/demo_crash.cpp @ 216

Last change on this file since 216 was 216, checked in by mathiask, 16 years ago
[Physik] add ode-0.9
File size: 18.3 KB

Line
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	// This is a demo of the QuickStep and StepFast methods,
24	// originally by David Whittaker.
25
26	#include <ode/ode.h>
27	#include <drawstuff/drawstuff.h>
28
29	#ifdef _MSC_VER
30	#pragma warning(disable:4244 4305) // for VC++, no precision loss complaints
31	#endif
32
33	// select correct drawing functions
34
35	#ifdef dDOUBLE
36	#define dsDrawBox dsDrawBoxD
37	#define dsDrawSphere dsDrawSphereD
38	#define dsDrawCylinder dsDrawCylinderD
39	#define dsDrawCapsule dsDrawCapsuleD
40	#endif
41
42
43	// select the method you want to test here (only uncomment one line)
44	#define QUICKSTEP 1
45	//#define STEPFAST 1
46
47	// some constants
48
49	#define LENGTH 3.5 // chassis length
50	#define WIDTH 2.5 // chassis width
51	#define HEIGHT 1.0 // chassis height
52	#define RADIUS 0.5 // wheel radius
53	#define STARTZ 1.0 // starting height of chassis
54	#define CMASS 1 // chassis mass
55	#define WMASS 1 // wheel mass
56	#define COMOFFSET -5 // center of mass offset
57	#define WALLMASS 1 // wall box mass
58	#define BALLMASS 1 // ball mass
59	#define FMAX 25 // car engine fmax
60	#define ROWS 1 // rows of cars
61	#define COLS 1 // columns of cars
62	#define ITERS 20 // number of iterations
63	#define WBOXSIZE 1.0 // size of wall boxes
64	#define WALLWIDTH 12 // width of wall
65	#define WALLHEIGHT 10 // height of wall
66	#define DISABLE_THRESHOLD 0.008 // maximum velocity (squared) a body can have and be disabled
67	#define DISABLE_STEPS 10 // number of steps a box has to have been disable-able before it will be disabled
68	#define CANNON_X -10 // x position of cannon
69	#define CANNON_Y 5 // y position of cannon
70	#define CANNON_BALL_MASS 10 // mass of the cannon ball
71	#define CANNON_BALL_RADIUS 0.5
72
73	//#define BOX
74	#define CARS
75	#define WALL
76	//#define BALLS
77	//#define BALLSTACK
78	//#define ONEBALL
79	//#define CENTIPEDE
80	#define CANNON
81
82	// dynamics and collision objects (chassis, 3 wheels, environment)
83
84	static dWorldID world;
85	static dSpaceID space;
86	static dBodyID body[10000];
87	static int bodies;
88	static dJointID joint[100000];
89	static int joints;
90	static dJointGroupID contactgroup;
91	static dGeomID ground;
92	static dGeomID box[10000];
93	static int boxes;
94	static dGeomID sphere[10000];
95	static int spheres;
96	static dGeomID wall_boxes[10000];
97	static dBodyID wall_bodies[10000];
98	static dGeomID cannon_ball_geom;
99	static dBodyID cannon_ball_body;
100	static int wb_stepsdis[10000];
101	static int wb;
102	static bool doFast;
103	static dBodyID b;
104	static dMass m;
105
106
107	// things that the user controls
108
109	static dReal turn = 0, speed = 0; // user commands
110	static dReal cannon_angle=0,cannon_elevation=-1.2;
111
112
113
114	// this is called by dSpaceCollide when two objects in space are
115	// potentially colliding.
116
117	static void nearCallback (void *data, dGeomID o1, dGeomID o2)
118	{
119	int i,n;
120
121	dBodyID b1 = dGeomGetBody(o1);
122	dBodyID b2 = dGeomGetBody(o2);
123	if (b1 && b2 && dAreConnected(b1, b2))
124	return;
125
126	const int N = 4;
127	dContact contact[N];
128	n = dCollide (o1,o2,N,&contact[0].geom,sizeof(dContact));
129	if (n > 0) {
130	for (i=0; i<n; i++) {
131	contact[i].surface.mode = dContactSlip1 \| dContactSlip2 \| dContactSoftERP \| dContactSoftCFM \| dContactApprox1;
132	if (dGeomGetClass(o1) == dSphereClass \|\| dGeomGetClass(o2) == dSphereClass)
133	contact[i].surface.mu = 20;
134	else
135	contact[i].surface.mu = 0.5;
136	contact[i].surface.slip1 = 0.0;
137	contact[i].surface.slip2 = 0.0;
138	contact[i].surface.soft_erp = 0.8;
139	contact[i].surface.soft_cfm = 0.01;
140	dJointID c = dJointCreateContact (world,contactgroup,contact+i);
141	dJointAttach (c,dGeomGetBody(o1),dGeomGetBody(o2));
142	}
143	}
144	}
145
146
147	// start simulation - set viewpoint
148
149	static void start()
150	{
151	static float xyz[3] = {3.8548f,9.0843f,7.5900f};
152	static float hpr[3] = {-145.5f,-22.5f,0.25f};
153	dsSetViewpoint (xyz,hpr);
154	printf ("Press:\t'a' to increase speed.\n"
155	"\t'z' to decrease speed.\n"
156	"\t',' to steer left.\n"
157	"\t'.' to steer right.\n"
158	"\t' ' to reset speed and steering.\n"
159	"\t'[' to turn the cannon left.\n"
160	"\t']' to turn the cannon right.\n"
161	"\t'1' to raise the cannon.\n"
162	"\t'2' to lower the cannon.\n"
163	"\t'x' to shoot from the cannon.\n"
164	"\t'f' to toggle fast step mode.\n"
165	"\t'+' to increase AutoEnableDepth.\n"
166	"\t'-' to decrease AutoEnableDepth.\n"
167	"\t'r' to reset simulation.\n");
168	}
169
170
171	void makeCar(dReal x, dReal y, int &bodyI, int &jointI, int &boxI, int &sphereI)
172	{
173	int i;
174	dMass m;
175
176	// chassis body
177	body[bodyI] = dBodyCreate (world);
178	dBodySetPosition (body[bodyI],x,y,STARTZ);
179	dMassSetBox (&m,1,LENGTH,WIDTH,HEIGHT);
180	dMassAdjust (&m,CMASS/2.0);
181	dBodySetMass (body[bodyI],&m);
182	box[boxI] = dCreateBox (space,LENGTH,WIDTH,HEIGHT);
183	dGeomSetBody (box[boxI],body[bodyI]);
184
185	// wheel bodies
186	for (i=1; i<=4; i++) {
187	body[bodyI+i] = dBodyCreate (world);
188	dQuaternion q;
189	dQFromAxisAndAngle (q,1,0,0,M_PI*0.5);
190	dBodySetQuaternion (body[bodyI+i],q);
191	dMassSetSphere (&m,1,RADIUS);
192	dMassAdjust (&m,WMASS);
193	dBodySetMass (body[bodyI+i],&m);
194	sphere[sphereI+i-1] = dCreateSphere (space,RADIUS);
195	dGeomSetBody (sphere[sphereI+i-1],body[bodyI+i]);
196	}
197	dBodySetPosition (body[bodyI+1],x+0.4LENGTH-0.5RADIUS,y+WIDTH0.5,STARTZ-HEIGHT0.5);
198	dBodySetPosition (body[bodyI+2],x+0.4LENGTH-0.5RADIUS,y-WIDTH0.5,STARTZ-HEIGHT0.5);
199	dBodySetPosition (body[bodyI+3],x-0.4LENGTH+0.5RADIUS,y+WIDTH0.5,STARTZ-HEIGHT0.5);
200	dBodySetPosition (body[bodyI+4],x-0.4LENGTH+0.5RADIUS,y-WIDTH0.5,STARTZ-HEIGHT0.5);
201
202	// front and back wheel hinges
203	for (i=0; i<4; i++) {
204	joint[jointI+i] = dJointCreateHinge2 (world,0);
205	dJointAttach (joint[jointI+i],body[bodyI],body[bodyI+i+1]);
206	const dReal *a = dBodyGetPosition (body[bodyI+i+1]);
207	dJointSetHinge2Anchor (joint[jointI+i],a[0],a[1],a[2]);
208	dJointSetHinge2Axis1 (joint[jointI+i],0,0,(i<2 ? 1 : -1));
209	dJointSetHinge2Axis2 (joint[jointI+i],0,1,0);
210	dJointSetHinge2Param (joint[jointI+i],dParamSuspensionERP,0.8);
211	dJointSetHinge2Param (joint[jointI+i],dParamSuspensionCFM,1e-5);
212	dJointSetHinge2Param (joint[jointI+i],dParamVel2,0);
213	dJointSetHinge2Param (joint[jointI+i],dParamFMax2,FMAX);
214	}
215
216	//center of mass offset body. (hang another copy of the body COMOFFSET units below it by a fixed joint)
217	dBodyID b = dBodyCreate (world);
218	dBodySetPosition (b,x,y,STARTZ+COMOFFSET);
219	dMassSetBox (&m,1,LENGTH,WIDTH,HEIGHT);
220	dMassAdjust (&m,CMASS/2.0);
221	dBodySetMass (b,&m);
222	dJointID j = dJointCreateFixed(world, 0);
223	dJointAttach(j, body[bodyI], b);
224	dJointSetFixed(j);
225	//box[boxI+1] = dCreateBox(space,LENGTH,WIDTH,HEIGHT);
226	//dGeomSetBody (box[boxI+1],b);
227
228	bodyI += 5;
229	jointI += 4;
230	boxI += 1;
231	sphereI += 4;
232	}
233
234
235	void resetSimulation()
236	{
237	int i;
238	i = 0;
239	// destroy world if it exists
240	if (bodies)
241	{
242	dJointGroupDestroy (contactgroup);
243	dSpaceDestroy (space);
244	dWorldDestroy (world);
245	}
246
247	for (i = 0; i < 1000; i++)
248	wb_stepsdis[i] = 0;
249
250	// recreate world
251
252	world = dWorldCreate();
253	space = dHashSpaceCreate (0);
254	contactgroup = dJointGroupCreate (0);
255	dWorldSetGravity (world,0,0,-1.5);
256	dWorldSetCFM (world, 1e-5);
257	dWorldSetERP (world, 0.8);
258	dWorldSetQuickStepNumIterations (world,ITERS);
259	ground = dCreatePlane (space,0,0,1,0);
260
261	bodies = 0;
262	joints = 0;
263	boxes = 0;
264	spheres = 0;
265	wb = 0;
266
267	#ifdef CARS
268	for (dReal x = 0.0; x < COLS*(LENGTH+RADIUS); x += LENGTH+RADIUS)
269	for (dReal y = -((ROWS-1)(WIDTH/2+RADIUS)); y <= ((ROWS-1)(WIDTH/2+RADIUS)); y += WIDTH+RADIUS*2)
270	makeCar(x, y, bodies, joints, boxes, spheres);
271	#endif
272	#ifdef WALL
273	bool offset = false;
274	for (dReal z = WBOXSIZE/2.0; z <= WALLHEIGHT; z+=WBOXSIZE)
275	{
276	offset = !offset;
277	for (dReal y = (-WALLWIDTH+z)/2; y <= (WALLWIDTH-z)/2; y+=WBOXSIZE)
278	{
279	wall_bodies[wb] = dBodyCreate (world);
280	dBodySetPosition (wall_bodies[wb],-20,y,z);
281	dMassSetBox (&m,1,WBOXSIZE,WBOXSIZE,WBOXSIZE);
282	dMassAdjust (&m, WALLMASS);
283	dBodySetMass (wall_bodies[wb],&m);
284	wall_boxes[wb] = dCreateBox (space,WBOXSIZE,WBOXSIZE,WBOXSIZE);
285	dGeomSetBody (wall_boxes[wb],wall_bodies[wb]);
286	//dBodyDisable(wall_bodies[wb++]);
287	wb++;
288	}
289	}
290	dMessage(0,"wall boxes: %i", wb);
291	#endif
292	#ifdef BALLS
293	for (dReal x = -7; x <= -4; x+=1)
294	for (dReal y = -1.5; y <= 1.5; y+=1)
295	for (dReal z = 1; z <= 4; z+=1)
296	{
297	b = dBodyCreate (world);
298	dBodySetPosition (b,xRADIUS2,yRADIUS2,zRADIUS2);
299	dMassSetSphere (&m,1,RADIUS);
300	dMassAdjust (&m, BALLMASS);
301	dBodySetMass (b,&m);
302	sphere[spheres] = dCreateSphere (space,RADIUS);
303	dGeomSetBody (sphere[spheres++],b);
304	}
305	#endif
306	#ifdef ONEBALL
307	b = dBodyCreate (world);
308	dBodySetPosition (b,0,0,2);
309	dMassSetSphere (&m,1,RADIUS);
310	dMassAdjust (&m, 1);
311	dBodySetMass (b,&m);
312	sphere[spheres] = dCreateSphere (space,RADIUS);
313	dGeomSetBody (sphere[spheres++],b);
314	#endif
315	#ifdef BALLSTACK
316	for (dReal z = 1; z <= 6; z+=1)
317	{
318	b = dBodyCreate (world);
319	dBodySetPosition (b,0,0,zRADIUS2);
320	dMassSetSphere (&m,1,RADIUS);
321	dMassAdjust (&m, 0.1);
322	dBodySetMass (b,&m);
323	sphere[spheres] = dCreateSphere (space,RADIUS);
324	dGeomSetBody (sphere[spheres++],b);
325	}
326	#endif
327	#ifdef CENTIPEDE
328	dBodyID lastb = 0;
329	for (dReal y = 0; y < 10*LENGTH; y+=LENGTH+0.1)
330	{
331	// chassis body
332
333	b = body[bodies] = dBodyCreate (world);
334	dBodySetPosition (body[bodies],-15,y,STARTZ);
335	dMassSetBox (&m,1,WIDTH,LENGTH,HEIGHT);
336	dMassAdjust (&m,CMASS);
337	dBodySetMass (body[bodies],&m);
338	box[boxes] = dCreateBox (space,WIDTH,LENGTH,HEIGHT);
339	dGeomSetBody (box[boxes++],body[bodies++]);
340
341	for (dReal x = -17; x > -20; x-=RADIUS*2)
342	{
343	body[bodies] = dBodyCreate (world);
344	dBodySetPosition(body[bodies], x, y, STARTZ);
345	dMassSetSphere(&m, 1, RADIUS);
346	dMassAdjust(&m, WMASS);
347	dBodySetMass(body[bodies], &m);
348	sphere[spheres] = dCreateSphere (space, RADIUS);
349	dGeomSetBody (sphere[spheres++], body[bodies]);
350
351	joint[joints] = dJointCreateHinge2 (world,0);
352	if (x == -17)
353	dJointAttach (joint[joints],b,body[bodies]);
354	else
355	dJointAttach (joint[joints],body[bodies-2],body[bodies]);
356	const dReal *a = dBodyGetPosition (body[bodies++]);
357	dJointSetHinge2Anchor (joint[joints],a[0],a[1],a[2]);
358	dJointSetHinge2Axis1 (joint[joints],0,0,1);
359	dJointSetHinge2Axis2 (joint[joints],1,0,0);
360	dJointSetHinge2Param (joint[joints],dParamSuspensionERP,1.0);
361	dJointSetHinge2Param (joint[joints],dParamSuspensionCFM,1e-5);
362	dJointSetHinge2Param (joint[joints],dParamLoStop,0);
363	dJointSetHinge2Param (joint[joints],dParamHiStop,0);
364	dJointSetHinge2Param (joint[joints],dParamVel2,-10.0);
365	dJointSetHinge2Param (joint[joints++],dParamFMax2,FMAX);
366
367	body[bodies] = dBodyCreate (world);
368	dBodySetPosition(body[bodies], -30 - x, y, STARTZ);
369	dMassSetSphere(&m, 1, RADIUS);
370	dMassAdjust(&m, WMASS);
371	dBodySetMass(body[bodies], &m);
372	sphere[spheres] = dCreateSphere (space, RADIUS);
373	dGeomSetBody (sphere[spheres++], body[bodies]);
374
375	joint[joints] = dJointCreateHinge2 (world,0);
376	if (x == -17)
377	dJointAttach (joint[joints],b,body[bodies]);
378	else
379	dJointAttach (joint[joints],body[bodies-2],body[bodies]);
380	const dReal *b = dBodyGetPosition (body[bodies++]);
381	dJointSetHinge2Anchor (joint[joints],b[0],b[1],b[2]);
382	dJointSetHinge2Axis1 (joint[joints],0,0,1);
383	dJointSetHinge2Axis2 (joint[joints],1,0,0);
384	dJointSetHinge2Param (joint[joints],dParamSuspensionERP,1.0);
385	dJointSetHinge2Param (joint[joints],dParamSuspensionCFM,1e-5);
386	dJointSetHinge2Param (joint[joints],dParamLoStop,0);
387	dJointSetHinge2Param (joint[joints],dParamHiStop,0);
388	dJointSetHinge2Param (joint[joints],dParamVel2,10.0);
389	dJointSetHinge2Param (joint[joints++],dParamFMax2,FMAX);
390	}
391	if (lastb)
392	{
393	dJointID j = dJointCreateFixed(world,0);
394	dJointAttach (j, b, lastb);
395	dJointSetFixed(j);
396	}
397	lastb = b;
398	}
399	#endif
400	#ifdef BOX
401	body[bodies] = dBodyCreate (world);
402	dBodySetPosition (body[bodies],0,0,HEIGHT/2);
403	dMassSetBox (&m,1,LENGTH,WIDTH,HEIGHT);
404	dMassAdjust (&m, 1);
405	dBodySetMass (body[bodies],&m);
406	box[boxes] = dCreateBox (space,LENGTH,WIDTH,HEIGHT);
407	dGeomSetBody (box[boxes++],body[bodies++]);
408	#endif
409	#ifdef CANNON
410	cannon_ball_body = dBodyCreate (world);
411	cannon_ball_geom = dCreateSphere (space,CANNON_BALL_RADIUS);
412	dMassSetSphereTotal (&m,CANNON_BALL_MASS,CANNON_BALL_RADIUS);
413	dBodySetMass (cannon_ball_body,&m);
414	dGeomSetBody (cannon_ball_geom,cannon_ball_body);
415	dBodySetPosition (cannon_ball_body,CANNON_X,CANNON_Y,CANNON_BALL_RADIUS);
416	#endif
417	}
418
419	// called when a key pressed
420
421	static void command (int cmd)
422	{
423	switch (cmd) {
424	case 'a': case 'A':
425	speed += 0.3;
426	break;
427	case 'z': case 'Z':
428	speed -= 0.3;
429	break;
430	case ',':
431	turn += 0.1;
432	if (turn > 0.3)
433	turn = 0.3;
434	break;
435	case '.':
436	turn -= 0.1;
437	if (turn < -0.3)
438	turn = -0.3;
439	break;
440	case ' ':
441	speed = 0;
442	turn = 0;
443	break;
444	case 'f': case 'F':
445	doFast = !doFast;
446	break;
447	case '+':
448	dWorldSetAutoEnableDepthSF1 (world, dWorldGetAutoEnableDepthSF1 (world) + 1);
449	break;
450	case '-':
451	dWorldSetAutoEnableDepthSF1 (world, dWorldGetAutoEnableDepthSF1 (world) - 1);
452	break;
453	case 'r': case 'R':
454	resetSimulation();
455	break;
456	case '[':
457	cannon_angle += 0.1;
458	break;
459	case ']':
460	cannon_angle -= 0.1;
461	break;
462	case '1':
463	cannon_elevation += 0.1;
464	break;
465	case '2':
466	cannon_elevation -= 0.1;
467	break;
468	case 'x': case 'X': {
469	dMatrix3 R2,R3,R4;
470	dRFromAxisAndAngle (R2,0,0,1,cannon_angle);
471	dRFromAxisAndAngle (R3,0,1,0,cannon_elevation);
472	dMultiply0 (R4,R2,R3,3,3,3);
473	dReal cpos[3] = {CANNON_X,CANNON_Y,1};
474	for (int i=0; i<3; i++) cpos[i] += 3R4[i4+2];
475	dBodySetPosition (cannon_ball_body,cpos[0],cpos[1],cpos[2]);
476	dReal force = 10;
477	dBodySetLinearVel (cannon_ball_body,forceR4[2],forceR4[6],force*R4[10]);
478	dBodySetAngularVel (cannon_ball_body,0,0,0);
479	break;
480	}
481	}
482	}
483
484
485	// simulation loop
486
487	static void simLoop (int pause)
488	{
489	int i, j;
490
491	dsSetTexture (DS_WOOD);
492
493	if (!pause) {
494	#ifdef BOX
495	dBodyAddForce(body[bodies-1],lspeed,0,0);
496	#endif
497	for (j = 0; j < joints; j++)
498	{
499	dReal curturn = dJointGetHinge2Angle1 (joint[j]);
500	//dMessage (0,"curturn %e, turn %e, vel %e", curturn, turn, (turn-curturn)*1.0);
501	dJointSetHinge2Param(joint[j],dParamVel,(turn-curturn)*1.0);
502	dJointSetHinge2Param(joint[j],dParamFMax,dInfinity);
503	dJointSetHinge2Param(joint[j],dParamVel2,speed);
504	dJointSetHinge2Param(joint[j],dParamFMax2,FMAX);
505	dBodyEnable(dJointGetBody(joint[j],0));
506	dBodyEnable(dJointGetBody(joint[j],1));
507	}
508	if (doFast)
509	{
510	dSpaceCollide (space,0,&nearCallback);
511	#if defined(QUICKSTEP)
512	dWorldQuickStep (world,0.05);
513	#elif defined(STEPFAST)
514	dWorldStepFast1 (world,0.05,ITERS);
515	#endif
516	dJointGroupEmpty (contactgroup);
517	}
518	else
519	{
520	dSpaceCollide (space,0,&nearCallback);
521	dWorldStep (world,0.05);
522	dJointGroupEmpty (contactgroup);
523	}
524
525	for (i = 0; i < wb; i++)
526	{
527	b = dGeomGetBody(wall_boxes[i]);
528	if (dBodyIsEnabled(b))
529	{
530	bool disable = true;
531	const dReal *lvel = dBodyGetLinearVel(b);
532	dReal lspeed = lvel[0]lvel[0]+lvel[1]lvel[1]+lvel[2]*lvel[2];
533	if (lspeed > DISABLE_THRESHOLD)
534	disable = false;
535	const dReal *avel = dBodyGetAngularVel(b);
536	dReal aspeed = avel[0]avel[0]+avel[1]avel[1]+avel[2]*avel[2];
537	if (aspeed > DISABLE_THRESHOLD)
538	disable = false;
539
540	if (disable)
541	wb_stepsdis[i]++;
542	else
543	wb_stepsdis[i] = 0;
544
545	if (wb_stepsdis[i] > DISABLE_STEPS)
546	{
547	dBodyDisable(b);
548	dsSetColor(0.5,0.5,1);
549	}
550	else
551	dsSetColor(1,1,1);
552
553	}
554	else
555	dsSetColor(0.4,0.4,0.4);
556	dVector3 ss;
557	dGeomBoxGetLengths (wall_boxes[i], ss);
558	dsDrawBox(dGeomGetPosition(wall_boxes[i]), dGeomGetRotation(wall_boxes[i]), ss);
559	}
560	}
561	else
562	{
563	for (i = 0; i < wb; i++)
564	{
565	b = dGeomGetBody(wall_boxes[i]);
566	if (dBodyIsEnabled(b))
567	dsSetColor(1,1,1);
568	else
569	dsSetColor(0.4,0.4,0.4);
570	dVector3 ss;
571	dGeomBoxGetLengths (wall_boxes[i], ss);
572	dsDrawBox(dGeomGetPosition(wall_boxes[i]), dGeomGetRotation(wall_boxes[i]), ss);
573	}
574	}
575
576	dsSetColor (0,1,1);
577	dReal sides[3] = {LENGTH,WIDTH,HEIGHT};
578	for (i = 0; i < boxes; i++)
579	dsDrawBox (dGeomGetPosition(box[i]),dGeomGetRotation(box[i]),sides);
580	dsSetColor (1,1,1);
581	for (i=0; i< spheres; i++) dsDrawSphere (dGeomGetPosition(sphere[i]),
582	dGeomGetRotation(sphere[i]),RADIUS);
583
584	// draw the cannon
585	dsSetColor (1,1,0);
586	dMatrix3 R2,R3,R4;
587	dRFromAxisAndAngle (R2,0,0,1,cannon_angle);
588	dRFromAxisAndAngle (R3,0,1,0,cannon_elevation);
589	dMultiply0 (R4,R2,R3,3,3,3);
590	dReal cpos[3] = {CANNON_X,CANNON_Y,1};
591	dReal csides[3] = {2,2,2};
592	dsDrawBox (cpos,R2,csides);
593	for (i=0; i<3; i++) cpos[i] += 1.5R4[i4+2];
594	dsDrawCylinder (cpos,R4,3,0.5);
595
596	// draw the cannon ball
597	dsDrawSphere (dBodyGetPosition(cannon_ball_body),dBodyGetRotation(cannon_ball_body),
598	CANNON_BALL_RADIUS);
599	}
600
601	int main (int argc, char **argv)
602	{
603	doFast = true;
604
605	// setup pointers to drawstuff callback functions
606	dsFunctions fn;
607	fn.version = DS_VERSION;
608	fn.start = &start;
609	fn.step = &simLoop;
610	fn.command = &command;
611	fn.stop = 0;
612	fn.path_to_textures = "../../drawstuff/textures";
613	if(argc==2)
614	{
615	fn.path_to_textures = argv[1];
616	}
617
618	dInitODE();
619
620	bodies = 0;
621	joints = 0;
622	boxes = 0;
623	spheres = 0;
624
625	resetSimulation();
626
627	// run simulation
628	dsSimulationLoop (argc,argv,352,288,&fn);
629
630	dJointGroupDestroy (contactgroup);
631	dSpaceDestroy (space);
632	dWorldDestroy (world);
633	dCloseODE();
634	return 0;
635	}

Note: See TracBrowser for help on using the repository browser.

Download in other formats: