aboutsummaryrefslogtreecommitdiffstatshomepage
path: root/libraries/ode-0.9/ode/demo/demo_cyl.cpp
diff options
context:
space:
mode:
Diffstat (limited to '')
-rw-r--r--libraries/ode-0.9/ode/demo/demo_cyl.cpp318
1 files changed, 0 insertions, 318 deletions
diff --git a/libraries/ode-0.9/ode/demo/demo_cyl.cpp b/libraries/ode-0.9/ode/demo/demo_cyl.cpp
deleted file mode 100644
index 3692234..0000000
--- a/libraries/ode-0.9/ode/demo/demo_cyl.cpp
+++ /dev/null
@@ -1,318 +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// Test for non-capped cylinder, by Bram Stolk
24#include <ode/config.h>
25#include <assert.h>
26#ifdef HAVE_UNISTD_H
27#include <unistd.h>
28#endif
29#include <ode/ode.h>
30#include <drawstuff/drawstuff.h>
31
32#include "world_geom3.h" // this is our world mesh
33
34#ifdef _MSC_VER
35#pragma warning(disable:4244 4305) // for VC++, no precision loss complaints
36#endif
37
38#define BOX
39#define CYL
40
41// some constants
42
43#define RADIUS 0.22 // wheel radius
44#define WMASS 0.2 // wheel mass
45#define WHEELW 0.2 // wheel width
46#define BOXSZ 0.4 // box size
47//#define CYL_GEOM_OFFSET // rotate cylinder using geom offset
48
49// dynamics and collision objects (chassis, 3 wheels, environment)
50
51static dWorldID world;
52static dSpaceID space;
53#ifdef BOX
54static dBodyID boxbody;
55static dGeomID boxgeom;
56#endif
57#ifdef CYL
58static dBodyID cylbody;
59static dGeomID cylgeom;
60#endif
61static dJointGroupID contactgroup;
62static dGeomID world_mesh;
63
64
65// this is called by dSpaceCollide when two objects in space are
66// potentially colliding.
67
68static void nearCallback (void *data, dGeomID o1, dGeomID o2)
69{
70 assert(o1);
71 assert(o2);
72
73 if (dGeomIsSpace(o1) || dGeomIsSpace(o2))
74 {
75 fprintf(stderr,"testing space %p %p\n", o1,o2);
76 // colliding a space with something
77 dSpaceCollide2(o1,o2,data,&nearCallback);
78 // Note we do not want to test intersections within a space,
79 // only between spaces.
80 return;
81 }
82
83// fprintf(stderr,"testing geoms %p %p\n", o1, o2);
84
85 const int N = 32;
86 dContact contact[N];
87 int n = dCollide (o1,o2,N,&(contact[0].geom),sizeof(dContact));
88 if (n > 0)
89 {
90 for (int i=0; i<n; i++)
91 {
92 contact[i].surface.slip1 = 0.7;
93 contact[i].surface.slip2 = 0.7;
94 contact[i].surface.mode = dContactSoftERP | dContactSoftCFM | dContactApprox1 | dContactSlip1 | dContactSlip2;
95 contact[i].surface.mu = 50.0; // was: dInfinity
96 contact[i].surface.soft_erp = 0.96;
97 contact[i].surface.soft_cfm = 0.04;
98 dJointID c = dJointCreateContact (world,contactgroup,&contact[i]);
99 dJointAttach (c,
100 dGeomGetBody(contact[i].geom.g1),
101 dGeomGetBody(contact[i].geom.g2));
102 }
103 }
104}
105
106
107// start simulation - set viewpoint
108
109static void start()
110{
111 static float xyz[3] = {-8,-9,3};
112 static float hpr[3] = {45.0000f,-27.5000f,0.0000f};
113 dsSetViewpoint (xyz,hpr);
114}
115
116
117
118static void reset_state(void)
119{
120 float sx=-4, sy=-4, sz=2;
121 dQuaternion q;
122 dQFromAxisAndAngle (q,1,0,0,M_PI*0.5);
123#ifdef BOX
124 dBodySetPosition (boxbody, sx, sy+1, sz);
125 dBodySetLinearVel (boxbody, 0,0,0);
126 dBodySetAngularVel (boxbody, 0,0,0);
127 dBodySetQuaternion (boxbody, q);
128#endif
129#ifdef CYL
130 dBodySetPosition (cylbody, sx, sy, sz);
131 dBodySetLinearVel (cylbody, 0,0,0);
132 dBodySetAngularVel (cylbody, 0,0,0);
133 dBodySetQuaternion (cylbody, q);
134#endif
135}
136
137
138// called when a key pressed
139
140static void command (int cmd)
141{
142 switch (cmd)
143 {
144 case ' ':
145 reset_state();
146 break;
147 }
148}
149
150
151
152// simulation loop
153
154static void simLoop (int pause)
155{
156 double simstep = 0.005; // 5ms simulation steps
157 double dt = dsElapsedTime();
158 int nrofsteps = (int) ceilf(dt/simstep);
159 for (int i=0; i<nrofsteps && !pause; i++)
160 {
161 dSpaceCollide (space,0,&nearCallback);
162 dWorldQuickStep (world, simstep);
163 dJointGroupEmpty (contactgroup);
164 }
165
166 dsSetColor (1,1,1);
167#ifdef BOX
168 const dReal *BPos = dBodyGetPosition(boxbody);
169 const dReal *BRot = dBodyGetRotation(boxbody);
170 float bpos[3] = {BPos[0], BPos[1], BPos[2]};
171 float brot[12] = { BRot[0], BRot[1], BRot[2], BRot[3], BRot[4], BRot[5], BRot[6], BRot[7], BRot[8], BRot[9], BRot[10], BRot[11] };
172 float sides[3] = {BOXSZ, BOXSZ, BOXSZ};
173 dsDrawBox
174 (
175 bpos,
176 brot,
177 sides
178 ); // single precision
179#endif
180#ifdef CYL
181 const dReal *CPos = dGeomGetPosition(cylgeom);
182 const dReal *CRot = dGeomGetRotation(cylgeom);
183 float cpos[3] = {CPos[0], CPos[1], CPos[2]};
184 float crot[12] = { CRot[0], CRot[1], CRot[2], CRot[3], CRot[4], CRot[5], CRot[6], CRot[7], CRot[8], CRot[9], CRot[10], CRot[11] };
185 dsDrawCylinder
186 (
187// dBodyGetPosition(cylbody),
188// dBodyGetRotation(cylbody),
189 cpos,
190 crot,
191 WHEELW,
192 RADIUS
193 ); // single precision
194#endif
195
196 // draw world trimesh
197 dsSetColor(0.7,0.7,0.4);
198 dsSetTexture (DS_NONE);
199
200 const dReal* Pos = dGeomGetPosition(world_mesh);
201 float pos[3] = { Pos[0], Pos[1], Pos[2] };
202
203 const dReal* Rot = dGeomGetRotation(world_mesh);
204 float rot[12] = { Rot[0], Rot[1], Rot[2], Rot[3], Rot[4], Rot[5], Rot[6], Rot[7], Rot[8], Rot[9], Rot[10], Rot[11] };
205
206 int numi = sizeof(world_indices) / sizeof(int);
207
208 for (int i=0; i<numi/3; i++)
209 {
210 int i0 = world_indices[i*3+0];
211 int i1 = world_indices[i*3+1];
212 int i2 = world_indices[i*3+2];
213 float *v0 = world_vertices+i0*3;
214 float *v1 = world_vertices+i1*3;
215 float *v2 = world_vertices+i2*3;
216 dsDrawTriangle(pos, rot, v0,v1,v2, true); // single precision draw
217 }
218}
219
220
221int main (int argc, char **argv)
222{
223 dMass m;
224 dMatrix3 R;
225
226 // setup pointers to drawstuff callback functions
227 dsFunctions fn;
228 fn.version = DS_VERSION;
229 fn.start = &start;
230 fn.step = &simLoop;
231 fn.command = &command;
232 fn.stop = 0;
233 fn.path_to_textures = "../../drawstuff/textures";
234 if(argc==2)
235 {
236 fn.path_to_textures = argv[1];
237 }
238
239 // create world
240 dInitODE();
241 world = dWorldCreate();
242 space = dHashSpaceCreate (0);
243 contactgroup = dJointGroupCreate (0);
244 dWorldSetGravity (world,0,0,-9.8);
245 dWorldSetQuickStepNumIterations (world, 12);
246
247
248 // Create a static world using a triangle mesh that we can collide with.
249 int numv = sizeof(world_vertices)/(3*sizeof(float));
250 int numi = sizeof(world_indices)/ sizeof(int);
251 printf("numv=%d, numi=%d\n", numv, numi);
252 dTriMeshDataID Data = dGeomTriMeshDataCreate();
253
254 dGeomTriMeshDataBuildSingle
255 (
256 Data,
257 world_vertices,
258 3 * sizeof(float),
259 numv,
260 world_indices,
261 numi,
262 3 * sizeof(int)
263 );
264
265 world_mesh = dCreateTriMesh(space, Data, 0, 0, 0);
266 dGeomSetPosition(world_mesh, 0, 0, 0.5);
267 dRFromAxisAndAngle (R, 0,1,0, 0.0);
268 dGeomSetRotation (world_mesh, R);
269
270
271#ifdef BOX
272 boxbody = dBodyCreate (world);
273 dMassSetBox (&m,1, BOXSZ, BOXSZ, BOXSZ);
274 dMassAdjust (&m, 1);
275 dBodySetMass (boxbody,&m);
276 boxgeom = dCreateBox (0, BOXSZ, BOXSZ, BOXSZ);
277 dGeomSetBody (boxgeom,boxbody);
278 dSpaceAdd (space, boxgeom);
279#endif
280#ifdef CYL
281 cylbody = dBodyCreate (world);
282 dMassSetSphere (&m,1,RADIUS);
283 dMassAdjust (&m,WMASS);
284 dBodySetMass (cylbody,&m);
285 cylgeom = dCreateCylinder(0, RADIUS, WHEELW);
286 dGeomSetBody (cylgeom,cylbody);
287
288 #if defined(CYL_GEOM_OFFSET)
289 dMatrix3 mat;
290 dRFromAxisAndAngle(mat,1.0f,0.0f,0.0f,M_PI/2.0);
291 dGeomSetOffsetRotation(cylgeom,mat);
292 #endif
293
294 dSpaceAdd (space, cylgeom);
295#endif
296 reset_state();
297
298 // run simulation
299 dsSimulationLoop (argc,argv,352,288,&fn);
300
301 dJointGroupEmpty (contactgroup);
302 dJointGroupDestroy (contactgroup);
303
304 // First destroy geoms, then space, then the world.
305#ifdef CYL
306 dGeomDestroy (cylgeom);
307#endif
308#ifdef BOX
309 dGeomDestroy (boxgeom);
310#endif
311 dGeomDestroy (world_mesh);
312
313 dSpaceDestroy (space);
314 dWorldDestroy (world);
315 dCloseODE();
316 return 0;
317}
318
generated by cgit v1.1 at 2024-09-13 08:05:26 +0000