aboutsummaryrefslogtreecommitdiffstatshomepage
path: root/libraries/ode-0.9/ode/demo/demo_jointPR.cpp
diff options
context:
space:
mode:
Diffstat (limited to 'libraries/ode-0.9/ode/demo/demo_jointPR.cpp')
-rw-r--r--libraries/ode-0.9/ode/demo/demo_jointPR.cpp377
1 files changed, 0 insertions, 377 deletions
diff --git a/libraries/ode-0.9/ode/demo/demo_jointPR.cpp b/libraries/ode-0.9/ode/demo/demo_jointPR.cpp
deleted file mode 100644
index e0d002d..0000000
--- a/libraries/ode-0.9/ode/demo/demo_jointPR.cpp
+++ /dev/null
@@ -1,377 +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
25This file try to demonstrate how the PR joint is working.
26
27The axisP is draw in red and the axisR is in green
28
29*/
30
31
32#include <ode/ode.h>
33#include <drawstuff/drawstuff.h>
34#include <iostream>
35#include <math.h>
36
37
38#define DRAWSTUFF_TEXTURE_PATH "../../drawstuff/textures"
39
40
41#ifdef _MSC_VER
42#pragma warning(disable:4244 4305) // for VC++, no precision loss complaints
43#endif
44// select correct drawing functions
45#ifdef dDOUBLE
46#define dsDrawBox dsDrawBoxD
47#endif
48
49// physics parameters
50#define BOX1_LENGTH 2 // Size along the X axis
51#define BOX1_WIDTH 1 // Size along the Y axis
52#define BOX1_HEIGHT 0.4 // Size along the Z axis (up) since gravity is (0,0,-10)
53#define BOX2_LENGTH 0.2
54#define BOX2_WIDTH 0.1
55#define BOX2_HEIGHT 0.4
56#define Mass1 10
57#define Mass2 0.1
58
59
60#define PRISMATIC_ONLY 1
61#define ROTOIDE_ONLY 2
62int flag = 0;
63
64
65//camera view
66static float xyz[3] = {2.0f,-3.5f,2.0000f};
67static float hpr[3] = {90.000f,-25.5000f,0.0000f};
68//world,space,body & geom
69static dWorldID world;
70static dSpaceID space;
71static dSpaceID box1_space;
72static dSpaceID box2_space;
73static dBodyID box1_body[1];
74static dBodyID box2_body[1];
75static dJointID joint[1];
76static dJointGroupID contactgroup;
77static dGeomID ground;
78static dGeomID box1[1];
79static dGeomID box2[1];
80
81
82//collision detection
83static void nearCallback (void *data, dGeomID o1, dGeomID o2)
84{
85 int i,n;
86
87 dBodyID b1 = dGeomGetBody(o1);
88 dBodyID b2 = dGeomGetBody(o2);
89 if (b1 && b2 && dAreConnectedExcluding (b1,b2,dJointTypeContact)) return;
90 const int N = 10;
91 dContact contact[N];
92 n = dCollide (o1,o2,N,&contact[0].geom,sizeof(dContact));
93 if (n > 0)
94 {
95 for (i=0; i<n; i++)
96 {
97 contact[i].surface.mode = dContactSlip1 | dContactSlip2 |
98 dContactSoftERP | dContactSoftCFM | dContactApprox1;
99 contact[i].surface.mu = 0.1;
100 contact[i].surface.slip1 = 0.02;
101 contact[i].surface.slip2 = 0.02;
102 contact[i].surface.soft_erp = 0.1;
103 contact[i].surface.soft_cfm = 0.0001;
104 dJointID c = dJointCreateContact (world,contactgroup,&contact[i]);
105 dJointAttach (c,dGeomGetBody(contact[i].geom.g1),dGeomGetBody(contact[i].geom.g2));
106 }
107 }
108}
109
110
111// start simulation - set viewpoint
112static void start()
113{
114 dsSetViewpoint (xyz,hpr);
115 printf ("Press 'd' to add force along positive x direction.\nPress 'a' to add force along negative x direction.\n");
116 printf ("Press 'w' to add force along positive y direction.\nPress 's' to add force along negative y direction.\n");
117 printf ("Press 'e' to add torque around positive z direction.\nPress 'q' to add torque around negative z direction.\n");
118 printf ("Press 'o' to add force around positive x direction \n");
119}
120
121// function to update camera position at each step.
122void update()
123{
124// const dReal *a =(dBodyGetPosition (box1_body[0]));
125// float dx=a[0];
126// float dy=a[1];
127// float dz=a[2];
128// xyz[0]=dx;
129// xyz[1]=dy-5;
130// xyz[2]=dz+2;
131// hpr[1]=-22.5000f;
132// dsSetViewpoint (xyz,hpr);
133}
134
135
136// called when a key pressed
137static void command (int cmd)
138{
139 switch(cmd)
140 {
141 case 'w': case 'W':
142 dBodyAddForce(box2_body[0],0,500,0);
143 std::cout<<(dBodyGetPosition(box2_body[0])[1]-dBodyGetPosition(box1_body[0])[1])<<'\n';
144 break;
145 case 's': case 'S':
146 dBodyAddForce(box2_body[0],0,-500,0);
147 std::cout<<(dBodyGetPosition(box2_body[0])[1]-dBodyGetPosition(box1_body[0])[1])<<'\n';
148 break;
149 case 'd': case 'D':
150 dBodyAddForce(box2_body[0],500,0,0);
151 std::cout<<(dBodyGetPosition(box2_body[0])[0]-dBodyGetPosition(box1_body[0])[0])<<'\n';
152 break;
153 case 'a': case 'A':
154 dBodyAddForce(box2_body[0],-500,0,0);
155 std::cout<<(dBodyGetPosition(box2_body[0])[0]-dBodyGetPosition(box1_body[0])[0])<<'\n';
156 break;
157 case 'e': case 'E':
158 dBodyAddRelTorque(box2_body[0],0,0,200);
159 break;
160 case 'q': case 'Q':
161 dBodyAddRelTorque(box2_body[0],0,0,-200);
162 break;
163 case 'o': case 'O':
164 dBodyAddForce(box1_body[0],10000,0,0);
165 break;
166 }
167}
168
169
170// simulation loop
171static void simLoop (int pause)
172{
173 if (!pause)
174 {
175 //draw 2 boxes
176 dVector3 ss;
177 dsSetTexture (DS_WOOD);
178
179 const dReal *posBox2 = dGeomGetPosition(box2[0]);
180 const dReal *rotBox2 = dGeomGetRotation(box2[0]);
181 dsSetColor (1,1,0);
182 dGeomBoxGetLengths (box2[0],ss);
183 dsDrawBox (posBox2, rotBox2, ss);
184
185 const dReal *posBox1 = dGeomGetPosition(box1[0]);
186 const dReal *rotBox1 = dGeomGetRotation(box1[0]);
187 dsSetColor (1,1,2);
188 dGeomBoxGetLengths (box1[0], ss);
189 dsDrawBox (posBox1, rotBox1, ss);
190
191 dVector3 anchorPos;
192 dJointGetPRAnchor (joint[0], anchorPos);
193
194 // Draw the axisP
195 if (ROTOIDE_ONLY != flag )
196 {
197 dsSetColor (1,0,0);
198 dVector3 sizeP = {0, 0.1, 0.1};
199 for (int i=0; i<3; ++i)
200 sizeP[0] += (anchorPos[i] - posBox1[i])*(anchorPos[i] - posBox1[i]);
201 sizeP[0] = sqrt(sizeP[0]);
202 dVector3 posAxisP;
203 for (int i=0; i<3; ++i)
204 posAxisP[i] = posBox1[i] + (anchorPos[i] - posBox1[i])/2.0;
205 dsDrawBox (posAxisP, rotBox1, sizeP);
206 }
207
208
209 // Draw the axisR
210 if (PRISMATIC_ONLY != flag )
211 {
212 dsSetColor (0,1,0);
213 dVector3 sizeR = {0, 0.1, 0.1};
214 for (int i=0; i<3; ++i)
215 sizeR[0] += (anchorPos[i] - posBox2[i])*(anchorPos[i] - posBox2[i]);
216 sizeR[0] = sqrt(sizeR[0]);
217 dVector3 posAxisR;
218 for (int i=0; i<3; ++i)
219 posAxisR[i] = posBox2[i] + (anchorPos[i] - posBox2[i])/2.0;
220 dsDrawBox (posAxisR, rotBox2, sizeR);
221 }
222
223 dSpaceCollide (space,0,&nearCallback);
224 dWorldQuickStep (world,0.0001);
225 update();
226 dJointGroupEmpty (contactgroup);
227 }
228}
229
230
231void Help(char **argv)
232{
233 printf("%s ", argv[0]);
234 printf(" -h | --help : print this help\n");
235 printf(" -b | --both : Display how the complete joint works\n");
236 printf(" Default behavior\n");
237 printf(" -p | --prismatic-only : Display how the prismatic part works\n");
238 printf(" The anchor pts is set at the center of body 2\n");
239 printf(" -r | --rotoide-only : Display how the rotoide part works\n");
240 printf(" The anchor pts is set at the center of body 1\n");
241 printf(" -t | --texture-path path : Path to the texture.\n");
242 printf(" Default = %s\n", DRAWSTUFF_TEXTURE_PATH);
243 printf("--------------------------------------------------\n");
244 printf("Hit any key to continue:");
245 getchar();
246
247 exit(0);
248}
249
250int main (int argc, char **argv)
251{
252 // setup pointers to drawstuff callback functions
253 dsFunctions fn;
254 fn.version = DS_VERSION;
255 fn.start = &start;
256 fn.step = &simLoop;
257 fn.command = &command;
258 fn.stop = 0;
259 fn.path_to_textures = DRAWSTUFF_TEXTURE_PATH;
260
261 if (argc >= 2 )
262 {
263 for (int i=1; i < argc; ++i)
264 {
265 if( 0 == strcmp("-h", argv[i]) || 0 == strcmp("--help", argv[i]) )
266 Help(argv);
267
268 if(!flag && (0 == strcmp("-p", argv[i]) ||0 == strcmp("--prismatic-only", argv[i])) )
269 flag = PRISMATIC_ONLY;
270
271 if(!flag && (0 == strcmp("-r", argv[i]) || 0 == strcmp("--rotoide-only", argv[i])) )
272 flag = ROTOIDE_ONLY;
273
274 if(0 == strcmp("-t", argv[i]) || 0 == strcmp("--texture-path", argv[i]))
275 {
276 int j = i+1;
277 if ( j+1 > argc || // Check if we have enough arguments
278 argv[j] == '\0' || // We should have a path here
279 argv[j][0] == '-' ) // We should have a path not a command line
280 Help(argv);
281 else
282 fn.path_to_textures = argv[++i]; // Increase i since we use this argument
283 }
284 }
285 }
286
287 // create world
288 world = dWorldCreate();
289 space = dHashSpaceCreate (0);
290 contactgroup = dJointGroupCreate (0);
291 dWorldSetGravity (world,0,0,-10);
292 ground = dCreatePlane (space,0,0,1,0);
293
294 //create two boxes
295 dMass m;
296 box1_body[0] = dBodyCreate (world);
297 dMassSetBox (&m,1,BOX1_LENGTH,BOX1_WIDTH,BOX1_HEIGHT);
298 dMassAdjust (&m,Mass1);
299 dBodySetMass (box1_body[0],&m);
300 box1[0] = dCreateBox (0,BOX1_LENGTH,BOX1_WIDTH,BOX1_HEIGHT);
301 dGeomSetBody (box1[0],box1_body[0]);
302
303 box2_body[0] = dBodyCreate (world);
304 dMassSetBox (&m,10,BOX2_LENGTH,BOX2_WIDTH,BOX2_HEIGHT);
305 dMassAdjust (&m,Mass2);
306 dBodySetMass (box2_body[0],&m);
307 box2[0] = dCreateBox (0,BOX2_LENGTH,BOX2_WIDTH,BOX2_HEIGHT);
308 dGeomSetBody (box2[0],box2_body[0]);
309
310 //set the initial positions of body1 and body2
311 dMatrix3 R;
312 dRSetIdentity(R);
313 dBodySetPosition (box1_body[0],0,0,BOX1_HEIGHT/2.0);
314 dBodySetRotation (box1_body[0], R);
315
316 dBodySetPosition (box2_body[0],
317 2.1,
318 0.0,
319 BOX2_HEIGHT/2.0);
320 dBodySetRotation (box2_body[0], R);
321
322
323 //set PR joint
324 joint[0] = dJointCreatePR(world,0);
325 dJointAttach (joint[0],box1_body[0],box2_body[0]);
326 switch (flag)
327 {
328 case PRISMATIC_ONLY:
329 dJointSetPRAnchor (joint[0],
330 2.1,
331 0.0,
332 BOX2_HEIGHT/2.0);
333 dJointSetPRParam (joint[0],dParamLoStop, -0.5);
334 dJointSetPRParam (joint[0],dParamHiStop, 1.5);
335 break;
336
337 case ROTOIDE_ONLY:
338 dJointSetPRAnchor (joint[0],
339 0.0,
340 0.0,
341 BOX2_HEIGHT/2.0);
342 dJointSetPRParam (joint[0],dParamLoStop, 0.0);
343 dJointSetPRParam (joint[0],dParamHiStop, 0.0);
344 break;
345
346 default:
347 dJointSetPRAnchor (joint[0],
348 1.1,
349 0.0,
350 BOX2_HEIGHT/2.0);
351 dJointSetPRParam (joint[0],dParamLoStop, -0.5);
352 dJointSetPRParam (joint[0],dParamHiStop, 1.5);
353 break;
354 }
355
356 dJointSetPRAxis1(joint[0],1,0,0);
357 dJointSetPRAxis2(joint[0],0,0,1);
358// We position the 2 body
359// The position of the rotoide joint is on the second body so it can rotate on itself
360// and move along the X axis.
361// With this anchor
362// - A force in X will move only the body 2 inside the low and hi limit
363// of the prismatic
364// - A force in Y will make the 2 bodies to rotate around on the plane
365
366 box1_space = dSimpleSpaceCreate (space);
367 dSpaceSetCleanup (box1_space,0);
368 dSpaceAdd(box1_space,box1[0]);
369
370 // run simulation
371 dsSimulationLoop (argc,argv,400,300,&fn);
372 dJointGroupDestroy (contactgroup);
373 dSpaceDestroy (space);
374 dWorldDestroy (world);
375 return 0;
376}
377
generated by cgit v1.1 at 2024-11-09 13:08:42 +0000