aboutsummaryrefslogtreecommitdiffstatshomepage
path: root/libraries/ode-0.9/contrib/BreakableJoints/test_breakable.cpp
diff options
context:
space:
mode:
Diffstat (limited to 'libraries/ode-0.9/contrib/BreakableJoints/test_breakable.cpp')
-rw-r--r--libraries/ode-0.9/contrib/BreakableJoints/test_breakable.cpp416
1 files changed, 0 insertions, 416 deletions
diff --git a/libraries/ode-0.9/contrib/BreakableJoints/test_breakable.cpp b/libraries/ode-0.9/contrib/BreakableJoints/test_breakable.cpp
deleted file mode 100644
index bfed3a3..0000000
--- a/libraries/ode-0.9/contrib/BreakableJoints/test_breakable.cpp
+++ /dev/null
@@ -1,416 +0,0 @@
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/*
24
25buggy with suspension.
26this also shows you how to use geom groups.
27
28*/
29
30
31#include <stdlib.h>
32
33#include <ode/ode.h>
34#include <drawstuff/drawstuff.h>
35
36#ifdef _MSC_VER
37#pragma warning(disable:4244 4305) // for VC++, no precision loss complaints
38#endif
39
40// select correct drawing functions
41
42#ifdef dDOUBLE
43#define dsDrawBox dsDrawBoxD
44#define dsDrawSphere dsDrawSphereD
45#define dsDrawCylinder dsDrawCylinderD
46#define dsDrawCappedCylinder dsDrawCappedCylinderD
47#endif
48
49
50// some constants
51
52#define LENGTH 0.7 // chassis length
53#define WIDTH 0.4 // chassis width
54#define HEIGHT 0.2 // chassis height
55#define RADIUS 0.22 // wheel radius
56#define STARTZ 0.4 // starting height of chassis
57#define CMASS 1 // chassis mass
58#define WMASS 0.2 // wheel mass
59
60// dynamics and collision objects (chassis, 4 wheels, environment, obstacles, chain)
61static dWorldID world;
62static dSpaceID space;
63
64// chain stuff
65static const float chain_radius = 0.1;
66static const float chain_mass = 0.1;
67static const int chain_num = 10;
68static dBodyID chain_body[chain_num];
69static dGeomID chain_geom[chain_num];
70static dJointID chain_joint[chain_num-1];
71
72// 1 chasses, 4 wheels
73static dBodyID body[5];
74// joint[0] is left front wheel, joint[1] is right front wheel
75static dJointID joint[4];
76static int joint_exists[4];
77static dJointGroupID contactgroup;
78static dGeomID ground;
79static dSpaceID car_space;
80static dGeomID box[1];
81static dGeomID sphere[4];
82static dGeomID ground_box;
83static const int obstacle_num = 25;
84static dGeomID obstacle[obstacle_num];
85
86// things that the user controls
87
88static dReal speed=0,steer=0; // user commands
89
90
91
92// this is called by dSpaceCollide when two objects in space are
93// potentially colliding.
94
95static void nearCallback (void *data, dGeomID o1, dGeomID o2)
96{
97 int i,n;
98
99// // do not collide objects that are connected
100// dBodyID b1 = dGeomGetBody (o1),
101// b2 = dGeomGetBody (o2);
102// if (b1 && b2 && dAreConnected(b1, b2)) return;
103
104 const int N = 10;
105 dContact contact[N];
106 n = dCollide (o1,o2,N,&contact[0].geom,sizeof(dContact));
107 if (n > 0) {
108 for (i=0; i<n; i++) {
109 contact[i].surface.mode = dContactSlip1 | dContactSlip2 |
110 dContactSoftERP | dContactSoftCFM | dContactApprox1;
111 contact[i].surface.mu = dInfinity;
112 contact[i].surface.slip1 = 0.1;
113 contact[i].surface.slip2 = 0.1;
114 contact[i].surface.soft_erp = 0.5;
115 contact[i].surface.soft_cfm = 0.3;
116 dJointID c = dJointCreateContact (world,contactgroup,&contact[i]);
117 dJointAttach (c,
118 dGeomGetBody(contact[i].geom.g1),
119 dGeomGetBody(contact[i].geom.g2));
120 }
121 }
122}
123
124// callback function for joints that break
125static void jointBreakCallback (dJointID j)
126{
127 if (j == joint[0]) joint_exists[0] = 0;
128 else if (j == joint[1]) joint_exists[1] = 0;
129 else if (j == joint[2]) joint_exists[2] = 0;
130 else if (j == joint[3]) joint_exists[3] = 0;
131 printf ("A joint just broke\n");
132}
133
134// start simulation - set viewpoint
135
136static void start()
137{
138 static float xyz[3] = {0.8317f,-0.9817f,0.8000f};
139 static float hpr[3] = {121.0000f,-27.5000f,0.0000f};
140 dsSetViewpoint (xyz,hpr);
141 printf ("Press:\t'a' to increase speed.\n"
142 "\t'z' to decrease speed.\n"
143 "\t',' to steer left.\n"
144 "\t'.' to steer right.\n"
145 "\t' ' to reset speed and steering.\n");
146}
147
148
149// called when a key pressed
150
151static void command (int cmd)
152{
153 switch (cmd) {
154 case 'a': case 'A':
155 speed += 0.3;
156 break;
157 case 'z': case 'Z':
158 speed -= 0.3;
159 break;
160 case ',':
161 steer -= 0.5;
162 break;
163 case '.':
164 steer += 0.5;
165 break;
166 case ' ':
167 speed = 0;
168 steer = 0;
169 break;
170 }
171}
172
173
174// simulation loop
175
176static void simLoop (int pause)
177{
178 int i;
179 if (!pause) {
180 for (i=0; i<2; i++) {
181 if (joint_exists[i]) {
182 // motor
183 dJointSetHinge2Param (joint[i],dParamVel2,-speed);
184 dJointSetHinge2Param (joint[i],dParamFMax2,0.1);
185
186 // steering
187 dReal v = steer - dJointGetHinge2Angle1 (joint[i]);
188 if (v > 0.1) v = 0.1;
189 if (v < -0.1) v = -0.1;
190 v *= 10.0;
191 dJointSetHinge2Param (joint[i],dParamVel,v);
192 dJointSetHinge2Param (joint[i],dParamFMax,0.2);
193 dJointSetHinge2Param (joint[i],dParamLoStop,-0.75);
194 dJointSetHinge2Param (joint[i],dParamHiStop,0.75);
195 dJointSetHinge2Param (joint[i],dParamFudgeFactor,0.1);
196 }
197 }
198
199 dSpaceCollide (space,0,&nearCallback);
200 //dWorldStep (world,0.05);
201 dWorldStepFast1 (world,0.05,5);
202
203 // remove all contact joints
204 dJointGroupEmpty (contactgroup);
205 }
206
207 dsSetColor (0,1,1);
208 dsSetTexture (DS_WOOD);
209 dReal sides[3] = {LENGTH,WIDTH,HEIGHT};
210 dsDrawBox (dBodyGetPosition(body[0]),dBodyGetRotation(body[0]),sides);
211 dsSetColor (1,1,1);
212 for (i=1; i<=4; i++)
213 dsDrawCylinder (dBodyGetPosition(body[i]),
214 dBodyGetRotation(body[i]),
215 0.2,
216 RADIUS);
217
218 dVector3 ss;
219 dGeomBoxGetLengths (ground_box,ss);
220 dsDrawBox (dGeomGetPosition(ground_box),dGeomGetRotation(ground_box),ss);
221
222 dsSetColor (1,0,0);
223 for (i=0; i<obstacle_num; i++) {
224 dVector3 ss;
225 dGeomBoxGetLengths (obstacle[i],ss);
226 dsDrawBox (dGeomGetPosition(obstacle[i]),dGeomGetRotation(obstacle[i]),ss);
227 }
228
229 dsSetColor (1,1,0);
230 for (i=0; i<chain_num; i++) {
231 dsDrawSphere (dGeomGetPosition(chain_geom[i]),dGeomGetRotation(chain_geom[i]),chain_radius);
232 }
233
234 /*
235 printf ("%.10f %.10f %.10f %.10f\n",
236 dJointGetHingeAngle (joint[1]),
237 dJointGetHingeAngle (joint[2]),
238 dJointGetHingeAngleRate (joint[1]),
239 dJointGetHingeAngleRate (joint[2]));
240 */
241}
242
243int main (int argc, char **argv)
244{
245 int i;
246 dMass m;
247
248 // setup pointers to drawstuff callback functions
249 dsFunctions fn;
250 fn.version = DS_VERSION;
251 fn.start = &start;
252 fn.step = &simLoop;
253 fn.command = &command;
254 fn.stop = 0;
255 fn.path_to_textures = "../../drawstuff/textures";
256 if(argc==2)
257 {
258 fn.path_to_textures = argv[1];
259 }
260 // create world
261
262 world = dWorldCreate();
263 space = dHashSpaceCreate (0);
264 contactgroup = dJointGroupCreate (0);
265 dWorldSetGravity (world,0,0,-0.5);
266 ground = dCreatePlane (space,0,0,1,0);
267
268 // chassis body
269 body[0] = dBodyCreate (world);
270 dBodySetPosition (body[0],0,0,STARTZ);
271 dMassSetBox (&m,1,LENGTH,WIDTH,HEIGHT);
272 dMassAdjust (&m,CMASS);
273 dBodySetMass (body[0],&m);
274 box[0] = dCreateBox (0,LENGTH,WIDTH,HEIGHT);
275 dGeomSetBody (box[0],body[0]);
276
277 // a chain
278 for (i=0; i<chain_num; i++) {
279 chain_body[i] = dBodyCreate (world);
280 dBodySetPosition (chain_body[i],-LENGTH-(i*2*chain_radius),0,STARTZ-HEIGHT*0.5);
281 dMassSetSphere (&m,1,chain_radius);
282 dMassAdjust (&m,chain_mass);
283 dBodySetMass (chain_body[i],&m);
284 chain_geom[i] = dCreateSphere (space,chain_radius);
285 dGeomSetBody (chain_geom[i],chain_body[i]);
286 }
287
288 // wheel bodies
289 for (i=1; i<=4; i++) {
290 body[i] = dBodyCreate (world);
291 dQuaternion q;
292 dQFromAxisAndAngle (q,1,0,0,M_PI*0.5);
293 dBodySetQuaternion (body[i],q);
294 dMassSetSphere (&m,1,RADIUS);
295 dMassAdjust (&m,WMASS);
296 dBodySetMass (body[i],&m);
297 sphere[i-1] = dCreateSphere (0,RADIUS);
298 dGeomSetBody (sphere[i-1],body[i]);
299 }
300 dBodySetPosition (body[1], 0.5*LENGTH, WIDTH*1.0, STARTZ-HEIGHT*0.5);
301 dBodySetPosition (body[2], 0.5*LENGTH, -WIDTH*1.0, STARTZ-HEIGHT*0.5);
302 dBodySetPosition (body[3], -0.5*LENGTH, WIDTH*1.0, STARTZ-HEIGHT*0.5);
303 dBodySetPosition (body[4], -0.5*LENGTH, -WIDTH*1.0, STARTZ-HEIGHT*0.5);
304
305 // front wheel hinge
306 /*
307 joint[0] = dJointCreateHinge2 (world,0);
308 dJointAttach (joint[0],body[0],body[1]);
309 const dReal *a = dBodyGetPosition (body[1]);
310 dJointSetHinge2Anchor (joint[0],a[0],a[1],a[2]);
311 dJointSetHinge2Axis1 (joint[0],0,0,1);
312 dJointSetHinge2Axis2 (joint[0],0,1,0);
313 */
314
315 // front and back wheel hinges
316 for (i=0; i<4; i++) {
317 joint[i] = dJointCreateHinge2 (world,0);
318 joint_exists[i] = 1;
319 dJointAttach (joint[i],body[0],body[i+1]);
320 const dReal *a = dBodyGetPosition (body[i+1]);
321 dJointSetHinge2Anchor (joint[i],a[0],a[1],a[2]);
322 dJointSetHinge2Axis1 (joint[i],0,0,1);
323 dJointSetHinge2Axis2 (joint[i],0,1,0);
324
325 // the wheels can break
326 dJointSetBreakable (joint[i], 1);
327 // the wheels wil break at a specific force
328 dJointSetBreakMode (joint[i],
329 dJOINT_BREAK_AT_B1_FORCE |
330 dJOINT_BREAK_AT_B2_FORCE |
331 dJOINT_DELETE_ON_BREAK);
332 // specify the force for the first body connected to the joint ...
333 dJointSetBreakForce (joint[i], 0, 2.5, 2.5, 2.5);
334 // and for the second body
335 dJointSetBreakForce (joint[i], 1, 2.5, 2.5, 2.5);
336 // set the callback function
337 dJointSetBreakCallback (joint[i], &jointBreakCallback);
338 }
339
340 // joints for the chain
341 for (i=0; i<chain_num-1; i++) {
342 chain_joint[i] = dJointCreateFixed (world,0);
343 dJointAttach (chain_joint[i],chain_body[i+1],chain_body[i]);
344 dJointSetFixed (chain_joint[i]);
345 // the chain can break
346 dJointSetBreakable (chain_joint[i], 1);
347 // the chain wil break at a specific force
348 dJointSetBreakMode (chain_joint[i],
349 dJOINT_BREAK_AT_B1_FORCE |
350 dJOINT_BREAK_AT_B2_FORCE |
351 dJOINT_DELETE_ON_BREAK);
352 // specify the force for the first body connected to the joint ...
353 dJointSetBreakForce (chain_joint[i], 0, 0.5, 0.5, 0.5);
354 // and for the second body
355 dJointSetBreakForce (chain_joint[i], 1, 0.5, 0.5, 0.5);
356 // set the callback function
357 dJointSetBreakCallback (chain_joint[i], &jointBreakCallback);
358 }
359
360 // set joint suspension
361 for (i=0; i<4; i++) {
362 dJointSetHinge2Param (joint[i],dParamSuspensionERP,0.4);
363 dJointSetHinge2Param (joint[i],dParamSuspensionCFM,0.1);
364 }
365
366 // lock back wheels along the steering axis
367 for (i=1; i<4; i++) {
368 // set stops to make sure wheels always stay in alignment
369 dJointSetHinge2Param (joint[i],dParamLoStop,0);
370 dJointSetHinge2Param (joint[i],dParamHiStop,0);
371 // the following alternative method is no good as the wheels may get out
372 // of alignment:
373 // dJointSetHinge2Param (joint[i],dParamVel,0);
374 // dJointSetHinge2Param (joint[i],dParamFMax,dInfinity);
375 }
376
377 // create car space and add it to the top level space
378 car_space = dSimpleSpaceCreate (space);
379 dSpaceSetCleanup (car_space,0);
380 dSpaceAdd (car_space,box[0]);
381 dSpaceAdd (car_space,sphere[0]);
382 dSpaceAdd (car_space,sphere[1]);
383 dSpaceAdd (car_space,sphere[2]);
384
385 // environment
386 ground_box = dCreateBox (space,2,1.5,1);
387 dMatrix3 R;
388 dRFromAxisAndAngle (R,0,1,0,-0.15);
389 dGeomSetPosition (ground_box,2,0,-0.34);
390 dGeomSetRotation (ground_box,R);
391
392 // obstacles
393 for (i=0; i<obstacle_num; i++) {
394 dReal height = 0.1+(dReal(rand()%10)/10.0);
395 obstacle[i] = dCreateBox (space,0.2,0.2,height);
396 dGeomSetPosition (
397 obstacle[i],
398 (rand()%20)-10,
399 (rand()%20)-10,
400 height/2.0);
401 }
402
403 // run simulation
404 dsSimulationLoop (argc,argv,352,288,&fn);
405
406 dJointGroupDestroy (contactgroup);
407 dSpaceDestroy (space);
408 dWorldDestroy (world);
409 dGeomDestroy (box[0]);
410 for (i=0; i<4; i++)
411 dGeomDestroy (sphere[i]);
412
413 dCloseODE ();
414
415 return 0;
416}
generated by cgit v1.1 at 2024-11-05 15:22:25 +0000