aboutsummaryrefslogtreecommitdiffstatshomepage
path: root/libraries/ode-0.9/ode/demo/demo_buggy.cpp
diff options
context:
space:
mode:
Diffstat (limited to 'libraries/ode-0.9/ode/demo/demo_buggy.cpp')
-rw-r--r--libraries/ode-0.9/ode/demo/demo_buggy.cpp325
1 files changed, 0 insertions, 325 deletions
diff --git a/libraries/ode-0.9/ode/demo/demo_buggy.cpp b/libraries/ode-0.9/ode/demo/demo_buggy.cpp
deleted file mode 100644
index a482e02..0000000
--- a/libraries/ode-0.9/ode/demo/demo_buggy.cpp
+++ /dev/null
@@ -1,325 +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 <ode/ode.h>
32#include <drawstuff/drawstuff.h>
33
34#ifdef _MSC_VER
35#pragma warning(disable:4244 4305) // for VC++, no precision loss complaints
36#endif
37
38// select correct drawing functions
39
40#ifdef dDOUBLE
41#define dsDrawBox dsDrawBoxD
42#define dsDrawSphere dsDrawSphereD
43#define dsDrawCylinder dsDrawCylinderD
44#define dsDrawCapsule dsDrawCapsuleD
45#endif
46
47
48// some constants
49
50#define LENGTH 0.7 // chassis length
51#define WIDTH 0.5 // chassis width
52#define HEIGHT 0.2 // chassis height
53#define RADIUS 0.18 // wheel radius
54#define STARTZ 0.5 // starting height of chassis
55#define CMASS 1 // chassis mass
56#define WMASS 0.2 // wheel mass
57
58
59// dynamics and collision objects (chassis, 3 wheels, environment)
60
61static dWorldID world;
62static dSpaceID space;
63static dBodyID body[4];
64static dJointID joint[3]; // joint[0] is the front wheel
65static dJointGroupID contactgroup;
66static dGeomID ground;
67static dSpaceID car_space;
68static dGeomID box[1];
69static dGeomID sphere[3];
70static dGeomID ground_box;
71
72
73// things that the user controls
74
75static dReal speed=0,steer=0; // user commands
76
77
78
79// this is called by dSpaceCollide when two objects in space are
80// potentially colliding.
81
82static void nearCallback (void *data, dGeomID o1, dGeomID o2)
83{
84 int i,n;
85
86 // only collide things with the ground
87 int g1 = (o1 == ground || o1 == ground_box);
88 int g2 = (o2 == ground || o2 == ground_box);
89 if (!(g1 ^ g2)) return;
90
91 const int N = 10;
92 dContact contact[N];
93 n = dCollide (o1,o2,N,&contact[0].geom,sizeof(dContact));
94 if (n > 0) {
95 for (i=0; i<n; i++) {
96 contact[i].surface.mode = dContactSlip1 | dContactSlip2 |
97 dContactSoftERP | dContactSoftCFM | dContactApprox1;
98 contact[i].surface.mu = dInfinity;
99 contact[i].surface.slip1 = 0.1;
100 contact[i].surface.slip2 = 0.1;
101 contact[i].surface.soft_erp = 0.5;
102 contact[i].surface.soft_cfm = 0.3;
103 dJointID c = dJointCreateContact (world,contactgroup,&contact[i]);
104 dJointAttach (c,
105 dGeomGetBody(contact[i].geom.g1),
106 dGeomGetBody(contact[i].geom.g2));
107 }
108 }
109}
110
111
112// start simulation - set viewpoint
113
114static void start()
115{
116 static float xyz[3] = {0.8317f,-0.9817f,0.8000f};
117 static float hpr[3] = {121.0000f,-27.5000f,0.0000f};
118 dsSetViewpoint (xyz,hpr);
119 printf ("Press:\t'a' to increase speed.\n"
120 "\t'z' to decrease speed.\n"
121 "\t',' to steer left.\n"
122 "\t'.' to steer right.\n"
123 "\t' ' to reset speed and steering.\n"
124 "\t'1' to save the current state to 'state.dif'.\n");
125}
126
127
128// called when a key pressed
129
130static void command (int cmd)
131{
132 switch (cmd) {
133 case 'a': case 'A':
134 speed += 0.3;
135 break;
136 case 'z': case 'Z':
137 speed -= 0.3;
138 break;
139 case ',':
140 steer -= 0.5;
141 break;
142 case '.':
143 steer += 0.5;
144 break;
145 case ' ':
146 speed = 0;
147 steer = 0;
148 break;
149 case '1': {
150 FILE *f = fopen ("state.dif","wt");
151 if (f) {
152 dWorldExportDIF (world,f,"");
153 fclose (f);
154 }
155 }
156 }
157}
158
159
160// simulation loop
161
162static void simLoop (int pause)
163{
164 int i;
165 if (!pause) {
166 // motor
167 dJointSetHinge2Param (joint[0],dParamVel2,-speed);
168 dJointSetHinge2Param (joint[0],dParamFMax2,0.1);
169
170 // steering
171 dReal v = steer - dJointGetHinge2Angle1 (joint[0]);
172 if (v > 0.1) v = 0.1;
173 if (v < -0.1) v = -0.1;
174 v *= 10.0;
175 dJointSetHinge2Param (joint[0],dParamVel,v);
176 dJointSetHinge2Param (joint[0],dParamFMax,0.2);
177 dJointSetHinge2Param (joint[0],dParamLoStop,-0.75);
178 dJointSetHinge2Param (joint[0],dParamHiStop,0.75);
179 dJointSetHinge2Param (joint[0],dParamFudgeFactor,0.1);
180
181 dSpaceCollide (space,0,&nearCallback);
182 dWorldStep (world,0.05);
183
184 // remove all contact joints
185 dJointGroupEmpty (contactgroup);
186 }
187
188 dsSetColor (0,1,1);
189 dsSetTexture (DS_WOOD);
190 dReal sides[3] = {LENGTH,WIDTH,HEIGHT};
191 dsDrawBox (dBodyGetPosition(body[0]),dBodyGetRotation(body[0]),sides);
192 dsSetColor (1,1,1);
193 for (i=1; i<=3; i++) dsDrawCylinder (dBodyGetPosition(body[i]),
194 dBodyGetRotation(body[i]),0.02f,RADIUS);
195
196 dVector3 ss;
197 dGeomBoxGetLengths (ground_box,ss);
198 dsDrawBox (dGeomGetPosition(ground_box),dGeomGetRotation(ground_box),ss);
199
200 /*
201 printf ("%.10f %.10f %.10f %.10f\n",
202 dJointGetHingeAngle (joint[1]),
203 dJointGetHingeAngle (joint[2]),
204 dJointGetHingeAngleRate (joint[1]),
205 dJointGetHingeAngleRate (joint[2]));
206 */
207}
208
209
210int main (int argc, char **argv)
211{
212 int i;
213 dMass m;
214
215 // setup pointers to drawstuff callback functions
216 dsFunctions fn;
217 fn.version = DS_VERSION;
218 fn.start = &start;
219 fn.step = &simLoop;
220 fn.command = &command;
221 fn.stop = 0;
222 fn.path_to_textures = "../../drawstuff/textures";
223 if(argc==2)
224 {
225 fn.path_to_textures = argv[1];
226 }
227
228 // create world
229 dInitODE();
230 world = dWorldCreate();
231 space = dHashSpaceCreate (0);
232 contactgroup = dJointGroupCreate (0);
233 dWorldSetGravity (world,0,0,-0.5);
234 ground = dCreatePlane (space,0,0,1,0);
235
236 // chassis body
237 body[0] = dBodyCreate (world);
238 dBodySetPosition (body[0],0,0,STARTZ);
239 dMassSetBox (&m,1,LENGTH,WIDTH,HEIGHT);
240 dMassAdjust (&m,CMASS);
241 dBodySetMass (body[0],&m);
242 box[0] = dCreateBox (0,LENGTH,WIDTH,HEIGHT);
243 dGeomSetBody (box[0],body[0]);
244
245 // wheel bodies
246 for (i=1; i<=3; i++) {
247 body[i] = dBodyCreate (world);
248 dQuaternion q;
249 dQFromAxisAndAngle (q,1,0,0,M_PI*0.5);
250 dBodySetQuaternion (body[i],q);
251 dMassSetSphere (&m,1,RADIUS);
252 dMassAdjust (&m,WMASS);
253 dBodySetMass (body[i],&m);
254 sphere[i-1] = dCreateSphere (0,RADIUS);
255 dGeomSetBody (sphere[i-1],body[i]);
256 }
257 dBodySetPosition (body[1],0.5*LENGTH,0,STARTZ-HEIGHT*0.5);
258 dBodySetPosition (body[2],-0.5*LENGTH, WIDTH*0.5,STARTZ-HEIGHT*0.5);
259 dBodySetPosition (body[3],-0.5*LENGTH,-WIDTH*0.5,STARTZ-HEIGHT*0.5);
260
261 // front wheel hinge
262 /*
263 joint[0] = dJointCreateHinge2 (world,0);
264 dJointAttach (joint[0],body[0],body[1]);
265 const dReal *a = dBodyGetPosition (body[1]);
266 dJointSetHinge2Anchor (joint[0],a[0],a[1],a[2]);
267 dJointSetHinge2Axis1 (joint[0],0,0,1);
268 dJointSetHinge2Axis2 (joint[0],0,1,0);
269 */
270
271 // front and back wheel hinges
272 for (i=0; i<3; i++) {
273 joint[i] = dJointCreateHinge2 (world,0);
274 dJointAttach (joint[i],body[0],body[i+1]);
275 const dReal *a = dBodyGetPosition (body[i+1]);
276 dJointSetHinge2Anchor (joint[i],a[0],a[1],a[2]);
277 dJointSetHinge2Axis1 (joint[i],0,0,1);
278 dJointSetHinge2Axis2 (joint[i],0,1,0);
279 }
280
281 // set joint suspension
282 for (i=0; i<3; i++) {
283 dJointSetHinge2Param (joint[i],dParamSuspensionERP,0.4);
284 dJointSetHinge2Param (joint[i],dParamSuspensionCFM,0.8);
285 }
286
287 // lock back wheels along the steering axis
288 for (i=1; i<3; i++) {
289 // set stops to make sure wheels always stay in alignment
290 dJointSetHinge2Param (joint[i],dParamLoStop,0);
291 dJointSetHinge2Param (joint[i],dParamHiStop,0);
292 // the following alternative method is no good as the wheels may get out
293 // of alignment:
294 // dJointSetHinge2Param (joint[i],dParamVel,0);
295 // dJointSetHinge2Param (joint[i],dParamFMax,dInfinity);
296 }
297
298 // create car space and add it to the top level space
299 car_space = dSimpleSpaceCreate (space);
300 dSpaceSetCleanup (car_space,0);
301 dSpaceAdd (car_space,box[0]);
302 dSpaceAdd (car_space,sphere[0]);
303 dSpaceAdd (car_space,sphere[1]);
304 dSpaceAdd (car_space,sphere[2]);
305
306 // environment
307 ground_box = dCreateBox (space,2,1.5,1);
308 dMatrix3 R;
309 dRFromAxisAndAngle (R,0,1,0,-0.15);
310 dGeomSetPosition (ground_box,2,0,-0.34);
311 dGeomSetRotation (ground_box,R);
312
313 // run simulation
314 dsSimulationLoop (argc,argv,352,288,&fn);
315
316 dGeomDestroy (box[0]);
317 dGeomDestroy (sphere[0]);
318 dGeomDestroy (sphere[1]);
319 dGeomDestroy (sphere[2]);
320 dJointGroupDestroy (contactgroup);
321 dSpaceDestroy (space);
322 dWorldDestroy (world);
323 dCloseODE();
324 return 0;
325}
generated by cgit v1.1 at 2024-11-09 22:37:51 +0000