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 | |
---|
25 | perform tests on all the joint types. |
---|
26 | this should be done using the double precision version of the library. |
---|
27 | |
---|
28 | usage: |
---|
29 | test_joints [-nXXX] [-g] [-i] [-e] [path_to_textures] |
---|
30 | |
---|
31 | if a test number is given then that specific test is performed, otherwise |
---|
32 | all the tests are performed. the tests are numbered `xxyy', where xx |
---|
33 | corresponds to the joint type and yy is the sub-test number. not every |
---|
34 | number maps to an actual test. |
---|
35 | |
---|
36 | flags: |
---|
37 | i: the test is interactive. |
---|
38 | g: turn off graphical display (can't use this with `i'). |
---|
39 | e: turn on occasional error perturbations |
---|
40 | n: performe test XXX |
---|
41 | some tests compute and display error values. these values are scaled so |
---|
42 | <1 is good and >1 is bad. other tests just show graphical results which |
---|
43 | you must verify visually. |
---|
44 | |
---|
45 | */ |
---|
46 | |
---|
47 | #include <ctype.h> |
---|
48 | #include <ode/ode.h> |
---|
49 | #include <drawstuff/drawstuff.h> |
---|
50 | |
---|
51 | #ifdef _MSC_VER |
---|
52 | #pragma warning(disable:4244 4305) // for VC++, no precision loss complaints |
---|
53 | #endif |
---|
54 | |
---|
55 | // select correct drawing functions |
---|
56 | #ifdef dDOUBLE |
---|
57 | #define dsDrawBox dsDrawBoxD |
---|
58 | #endif |
---|
59 | |
---|
60 | |
---|
61 | // some constants |
---|
62 | #define NUM_JOINTS 10 // number of joints to test (the `xx' value) |
---|
63 | #define SIDE (0.5f) // side length of a box - don't change this |
---|
64 | #define MASS (1.0) // mass of a box |
---|
65 | #define STEPSIZE 0.05 |
---|
66 | |
---|
67 | |
---|
68 | // dynamics objects |
---|
69 | static dWorldID world; |
---|
70 | static dBodyID body[2]; |
---|
71 | static dJointID joint; |
---|
72 | |
---|
73 | |
---|
74 | // data from the command line arguments |
---|
75 | static int cmd_test_num = -1; |
---|
76 | static int cmd_interactive = 0; |
---|
77 | static int cmd_graphics = 1; |
---|
78 | static char *cmd_path_to_textures = NULL; |
---|
79 | static int cmd_occasional_error = 0; // perturb occasionally |
---|
80 | |
---|
81 | |
---|
82 | // info about the current test |
---|
83 | struct TestInfo; |
---|
84 | static int test_num = 0; // number of the current test |
---|
85 | static int iteration = 0; |
---|
86 | static int max_iterations = 0; |
---|
87 | static dReal max_error = 0; |
---|
88 | |
---|
89 | //**************************************************************************** |
---|
90 | // utility stuff |
---|
91 | |
---|
92 | static char loCase (char a) |
---|
93 | { |
---|
94 | if (a >= 'A' && a <= 'Z') return a + ('a'-'A'); |
---|
95 | else return a; |
---|
96 | } |
---|
97 | |
---|
98 | |
---|
99 | static dReal length (dVector3 a) |
---|
100 | { |
---|
101 | return dSqrt (a[0]*a[0] + a[1]*a[1] + a[2]*a[2]); |
---|
102 | } |
---|
103 | |
---|
104 | |
---|
105 | // get the max difference between a 3x3 matrix and the identity |
---|
106 | |
---|
107 | dReal cmpIdentity (const dMatrix3 A) |
---|
108 | { |
---|
109 | dMatrix3 I; |
---|
110 | dSetZero (I,12); |
---|
111 | I[0] = 1; |
---|
112 | I[5] = 1; |
---|
113 | I[10] = 1; |
---|
114 | return dMaxDifference (A,I,3,3); |
---|
115 | } |
---|
116 | |
---|
117 | //**************************************************************************** |
---|
118 | // test world construction and utilities |
---|
119 | |
---|
120 | void constructWorldForTest (dReal gravity, int bodycount, |
---|
121 | /* body 1 pos */ dReal pos1x, dReal pos1y, dReal pos1z, |
---|
122 | /* body 2 pos */ dReal pos2x, dReal pos2y, dReal pos2z, |
---|
123 | /* body 1 rotation axis */ dReal ax1x, dReal ax1y, dReal ax1z, |
---|
124 | /* body 1 rotation axis */ dReal ax2x, dReal ax2y, dReal ax2z, |
---|
125 | /* rotation angles */ dReal a1, dReal a2) |
---|
126 | { |
---|
127 | // create world |
---|
128 | world = dWorldCreate(); |
---|
129 | dWorldSetERP (world,0.2); |
---|
130 | dWorldSetCFM (world,1e-6); |
---|
131 | dWorldSetGravity (world,0,0,gravity); |
---|
132 | |
---|
133 | dMass m; |
---|
134 | dMassSetBox (&m,1,SIDE,SIDE,SIDE); |
---|
135 | dMassAdjust (&m,MASS); |
---|
136 | |
---|
137 | body[0] = dBodyCreate (world); |
---|
138 | dBodySetMass (body[0],&m); |
---|
139 | dBodySetPosition (body[0], pos1x, pos1y, pos1z); |
---|
140 | dQuaternion q; |
---|
141 | dQFromAxisAndAngle (q,ax1x,ax1y,ax1z,a1); |
---|
142 | dBodySetQuaternion (body[0],q); |
---|
143 | |
---|
144 | if (bodycount==2) { |
---|
145 | body[1] = dBodyCreate (world); |
---|
146 | dBodySetMass (body[1],&m); |
---|
147 | dBodySetPosition (body[1], pos2x, pos2y, pos2z); |
---|
148 | dQFromAxisAndAngle (q,ax2x,ax2y,ax2z,a2); |
---|
149 | dBodySetQuaternion (body[1],q); |
---|
150 | } |
---|
151 | else body[1] = 0; |
---|
152 | } |
---|
153 | |
---|
154 | |
---|
155 | // add an oscillating torque to body 0 |
---|
156 | |
---|
157 | void addOscillatingTorque (dReal tscale) |
---|
158 | { |
---|
159 | static dReal a=0; |
---|
160 | dBodyAddTorque (body[0],tscale*cos(2*a),tscale*cos(2.7183*a), |
---|
161 | tscale*cos(1.5708*a)); |
---|
162 | a += 0.01; |
---|
163 | } |
---|
164 | |
---|
165 | |
---|
166 | void addOscillatingTorqueAbout(dReal tscale, dReal x, dReal y, dReal z) |
---|
167 | { |
---|
168 | static dReal a=0; |
---|
169 | dBodyAddTorque (body[0], tscale*cos(a) * x, tscale*cos(a) * y, |
---|
170 | tscale * cos(a) * z); |
---|
171 | a += 0.02; |
---|
172 | } |
---|
173 | |
---|
174 | |
---|
175 | // damp the rotational motion of body 0 a bit |
---|
176 | |
---|
177 | void dampRotationalMotion (dReal kd) |
---|
178 | { |
---|
179 | const dReal *w = dBodyGetAngularVel (body[0]); |
---|
180 | dBodyAddTorque (body[0],-kd*w[0],-kd*w[1],-kd*w[2]); |
---|
181 | } |
---|
182 | |
---|
183 | |
---|
184 | // add a spring force to keep the bodies together, otherwise they may fly |
---|
185 | // apart with some joints. |
---|
186 | |
---|
187 | void addSpringForce (dReal ks) |
---|
188 | { |
---|
189 | const dReal *p1 = dBodyGetPosition (body[0]); |
---|
190 | const dReal *p2 = dBodyGetPosition (body[1]); |
---|
191 | dBodyAddForce (body[0],ks*(p2[0]-p1[0]),ks*(p2[1]-p1[1]),ks*(p2[2]-p1[2])); |
---|
192 | dBodyAddForce (body[1],ks*(p1[0]-p2[0]),ks*(p1[1]-p2[1]),ks*(p1[2]-p2[2])); |
---|
193 | } |
---|
194 | |
---|
195 | |
---|
196 | // add an oscillating Force to body 0 |
---|
197 | |
---|
198 | void addOscillatingForce (dReal fscale) |
---|
199 | { |
---|
200 | static dReal a=0; |
---|
201 | dBodyAddForce (body[0],fscale*cos(2*a),fscale*cos(2.7183*a), |
---|
202 | fscale*cos(1.5708*a)); |
---|
203 | a += 0.01; |
---|
204 | } |
---|
205 | |
---|
206 | //**************************************************************************** |
---|
207 | // stuff specific to the tests |
---|
208 | // |
---|
209 | // 0xx : fixed |
---|
210 | // 1xx : ball and socket |
---|
211 | // 2xx : hinge |
---|
212 | // 3xx : slider |
---|
213 | // 4xx : hinge 2 |
---|
214 | // 5xx : contact |
---|
215 | // 6xx : amotor |
---|
216 | // 7xx : universal joint |
---|
217 | // 8xx : PR joint (Prismatic and Rotoide) |
---|
218 | |
---|
219 | // setup for the given test. return 0 if there is no such test |
---|
220 | |
---|
221 | int setupTest (int n) |
---|
222 | { |
---|
223 | switch (n) { |
---|
224 | |
---|
225 | // ********** fixed joint |
---|
226 | |
---|
227 | case 0: { // 2 body |
---|
228 | constructWorldForTest (0,2, |
---|
229 | 0.5*SIDE,0.5*SIDE,1, -0.5*SIDE,-0.5*SIDE,1, |
---|
230 | 1,1,0, 1,1,0, |
---|
231 | 0.25*M_PI,0.25*M_PI); |
---|
232 | joint = dJointCreateFixed (world,0); |
---|
233 | dJointAttach (joint,body[0],body[1]); |
---|
234 | dJointSetFixed (joint); |
---|
235 | return 1; |
---|
236 | } |
---|
237 | |
---|
238 | case 1: { // 1 body to static env |
---|
239 | constructWorldForTest (0,1, |
---|
240 | 0.5*SIDE,0.5*SIDE,1, 0,0,0, |
---|
241 | 1,0,0, 1,0,0, |
---|
242 | 0,0); |
---|
243 | joint = dJointCreateFixed (world,0); |
---|
244 | dJointAttach (joint,body[0],0); |
---|
245 | dJointSetFixed (joint); |
---|
246 | return 1; |
---|
247 | } |
---|
248 | |
---|
249 | case 2: { // 2 body with relative rotation |
---|
250 | constructWorldForTest (0,2, |
---|
251 | 0.5*SIDE,0.5*SIDE,1, -0.5*SIDE,-0.5*SIDE,1, |
---|
252 | 1,1,0, 1,1,0, |
---|
253 | 0.25*M_PI,-0.25*M_PI); |
---|
254 | joint = dJointCreateFixed (world,0); |
---|
255 | dJointAttach (joint,body[0],body[1]); |
---|
256 | dJointSetFixed (joint); |
---|
257 | return 1; |
---|
258 | } |
---|
259 | |
---|
260 | case 3: { // 1 body to static env with relative rotation |
---|
261 | constructWorldForTest (0,1, |
---|
262 | 0.5*SIDE,0.5*SIDE,1, 0,0,0, |
---|
263 | 1,0,0, 1,0,0, |
---|
264 | 0.25*M_PI,0); |
---|
265 | joint = dJointCreateFixed (world,0); |
---|
266 | dJointAttach (joint,body[0],0); |
---|
267 | dJointSetFixed (joint); |
---|
268 | return 1; |
---|
269 | } |
---|
270 | |
---|
271 | // ********** hinge joint |
---|
272 | |
---|
273 | case 200: // 2 body |
---|
274 | constructWorldForTest (0,2, |
---|
275 | 0.5*SIDE,0.5*SIDE,1, -0.5*SIDE,-0.5*SIDE,1, |
---|
276 | 1,1,0, 1,1,0, 0.25*M_PI,0.25*M_PI); |
---|
277 | joint = dJointCreateHinge (world,0); |
---|
278 | dJointAttach (joint,body[0],body[1]); |
---|
279 | dJointSetHingeAnchor (joint,0,0,1); |
---|
280 | dJointSetHingeAxis (joint,1,-1,1.41421356); |
---|
281 | return 1; |
---|
282 | |
---|
283 | case 220: // hinge angle polarity test |
---|
284 | case 221: // hinge angle rate test |
---|
285 | constructWorldForTest (0,2, |
---|
286 | 0.5*SIDE,0.5*SIDE,1, -0.5*SIDE,-0.5*SIDE,1, |
---|
287 | 1,0,0, 1,0,0, 0,0); |
---|
288 | joint = dJointCreateHinge (world,0); |
---|
289 | dJointAttach (joint,body[0],body[1]); |
---|
290 | dJointSetHingeAnchor (joint,0,0,1); |
---|
291 | dJointSetHingeAxis (joint,0,0,1); |
---|
292 | max_iterations = 50; |
---|
293 | return 1; |
---|
294 | |
---|
295 | case 230: // hinge motor rate (and polarity) test |
---|
296 | case 231: // ...with stops |
---|
297 | constructWorldForTest (0,2, |
---|
298 | 0.5*SIDE,0.5*SIDE,1, -0.5*SIDE,-0.5*SIDE,1, |
---|
299 | 1,0,0, 1,0,0, 0,0); |
---|
300 | joint = dJointCreateHinge (world,0); |
---|
301 | dJointAttach (joint,body[0],body[1]); |
---|
302 | dJointSetHingeAnchor (joint,0,0,1); |
---|
303 | dJointSetHingeAxis (joint,0,0,1); |
---|
304 | dJointSetHingeParam (joint,dParamFMax,1); |
---|
305 | if (n==231) { |
---|
306 | dJointSetHingeParam (joint,dParamLoStop,-0.5); |
---|
307 | dJointSetHingeParam (joint,dParamHiStop,0.5); |
---|
308 | } |
---|
309 | return 1; |
---|
310 | |
---|
311 | case 250: // limit bounce test (gravity down) |
---|
312 | case 251: { // ...gravity up |
---|
313 | constructWorldForTest ((n==251) ? 0.1 : -0.1, 2, |
---|
314 | 0.5*SIDE,0,1+0.5*SIDE, -0.5*SIDE,0,1-0.5*SIDE, |
---|
315 | 1,0,0, 1,0,0, 0,0); |
---|
316 | joint = dJointCreateHinge (world,0); |
---|
317 | dJointAttach (joint,body[0],body[1]); |
---|
318 | dJointSetHingeAnchor (joint,0,0,1); |
---|
319 | dJointSetHingeAxis (joint,0,1,0); |
---|
320 | dJointSetHingeParam (joint,dParamLoStop,-0.9); |
---|
321 | dJointSetHingeParam (joint,dParamHiStop,0.7854); |
---|
322 | dJointSetHingeParam (joint,dParamBounce,0.5); |
---|
323 | // anchor 2nd body with a fixed joint |
---|
324 | dJointID j = dJointCreateFixed (world,0); |
---|
325 | dJointAttach (j,body[1],0); |
---|
326 | dJointSetFixed (j); |
---|
327 | return 1; |
---|
328 | } |
---|
329 | |
---|
330 | // ********** slider |
---|
331 | |
---|
332 | case 300: // 2 body |
---|
333 | constructWorldForTest (0,2, |
---|
334 | 0,0,1, 0.2,0.2,1.2, |
---|
335 | 0,0,1, -1,1,0, 0,0.25*M_PI); |
---|
336 | joint = dJointCreateSlider (world,0); |
---|
337 | dJointAttach (joint,body[0],body[1]); |
---|
338 | dJointSetSliderAxis (joint,1,1,1); |
---|
339 | return 1; |
---|
340 | |
---|
341 | case 320: // slider angle polarity test |
---|
342 | case 321: // slider angle rate test |
---|
343 | constructWorldForTest (0,2, |
---|
344 | 0,0,1, 0,0,1.2, |
---|
345 | 1,0,0, 1,0,0, 0,0); |
---|
346 | joint = dJointCreateSlider (world,0); |
---|
347 | dJointAttach (joint,body[0],body[1]); |
---|
348 | dJointSetSliderAxis (joint,0,0,1); |
---|
349 | max_iterations = 50; |
---|
350 | return 1; |
---|
351 | |
---|
352 | case 330: // slider motor rate (and polarity) test |
---|
353 | case 331: // ...with stops |
---|
354 | constructWorldForTest (0, 2, |
---|
355 | 0,0,1, 0,0,1.2, |
---|
356 | 1,0,0, 1,0,0, 0,0); |
---|
357 | joint = dJointCreateSlider (world,0); |
---|
358 | dJointAttach (joint,body[0],body[1]); |
---|
359 | dJointSetSliderAxis (joint,0,0,1); |
---|
360 | dJointSetSliderParam (joint,dParamFMax,100); |
---|
361 | if (n==331) { |
---|
362 | dJointSetSliderParam (joint,dParamLoStop,-0.4); |
---|
363 | dJointSetSliderParam (joint,dParamHiStop,0.4); |
---|
364 | } |
---|
365 | return 1; |
---|
366 | |
---|
367 | case 350: // limit bounce tests |
---|
368 | case 351: { |
---|
369 | constructWorldForTest ((n==351) ? 0.1 : -0.1, 2, |
---|
370 | 0,0,1, 0,0,1.2, |
---|
371 | 1,0,0, 1,0,0, 0,0); |
---|
372 | joint = dJointCreateSlider (world,0); |
---|
373 | dJointAttach (joint,body[0],body[1]); |
---|
374 | dJointSetSliderAxis (joint,0,0,1); |
---|
375 | dJointSetSliderParam (joint,dParamLoStop,-0.5); |
---|
376 | dJointSetSliderParam (joint,dParamHiStop,0.5); |
---|
377 | dJointSetSliderParam (joint,dParamBounce,0.5); |
---|
378 | // anchor 2nd body with a fixed joint |
---|
379 | dJointID j = dJointCreateFixed (world,0); |
---|
380 | dJointAttach (j,body[1],0); |
---|
381 | dJointSetFixed (j); |
---|
382 | return 1; |
---|
383 | } |
---|
384 | |
---|
385 | // ********** hinge-2 joint |
---|
386 | |
---|
387 | case 420: // hinge-2 steering angle polarity test |
---|
388 | case 421: // hinge-2 steering angle rate test |
---|
389 | constructWorldForTest (0,2, |
---|
390 | 0.5*SIDE,0,1, -0.5*SIDE,0,1, |
---|
391 | 1,0,0, 1,0,0, 0,0); |
---|
392 | joint = dJointCreateHinge2 (world,0); |
---|
393 | dJointAttach (joint,body[0],body[1]); |
---|
394 | dJointSetHinge2Anchor (joint,-0.5*SIDE,0,1); |
---|
395 | dJointSetHinge2Axis1 (joint,0,0,1); |
---|
396 | dJointSetHinge2Axis2 (joint,1,0,0); |
---|
397 | max_iterations = 50; |
---|
398 | return 1; |
---|
399 | |
---|
400 | case 430: // hinge 2 steering motor rate (+polarity) test |
---|
401 | case 431: // ...with stops |
---|
402 | case 432: // hinge 2 wheel motor rate (+polarity) test |
---|
403 | constructWorldForTest (0,2, |
---|
404 | 0.5*SIDE,0,1, -0.5*SIDE,0,1, |
---|
405 | 1,0,0, 1,0,0, 0,0); |
---|
406 | joint = dJointCreateHinge2 (world,0); |
---|
407 | dJointAttach (joint,body[0],body[1]); |
---|
408 | dJointSetHinge2Anchor (joint,-0.5*SIDE,0,1); |
---|
409 | dJointSetHinge2Axis1 (joint,0,0,1); |
---|
410 | dJointSetHinge2Axis2 (joint,1,0,0); |
---|
411 | dJointSetHinge2Param (joint,dParamFMax,1); |
---|
412 | dJointSetHinge2Param (joint,dParamFMax2,1); |
---|
413 | if (n==431) { |
---|
414 | dJointSetHinge2Param (joint,dParamLoStop,-0.5); |
---|
415 | dJointSetHinge2Param (joint,dParamHiStop,0.5); |
---|
416 | } |
---|
417 | return 1; |
---|
418 | |
---|
419 | // ********** angular motor joint |
---|
420 | |
---|
421 | case 600: // test euler angle calculations |
---|
422 | constructWorldForTest (0,2, |
---|
423 | -SIDE*0.5,0,1, SIDE*0.5,0,1, |
---|
424 | 0,0,1, 0,0,1, 0,0); |
---|
425 | joint = dJointCreateAMotor (world,0); |
---|
426 | dJointAttach (joint,body[0],body[1]); |
---|
427 | |
---|
428 | dJointSetAMotorNumAxes (joint,3); |
---|
429 | dJointSetAMotorAxis (joint,0,1, 0,0,1); |
---|
430 | dJointSetAMotorAxis (joint,2,2, 1,0,0); |
---|
431 | dJointSetAMotorMode (joint,dAMotorEuler); |
---|
432 | max_iterations = 200; |
---|
433 | return 1; |
---|
434 | |
---|
435 | // ********** universal joint |
---|
436 | |
---|
437 | case 700: // 2 body |
---|
438 | case 701: |
---|
439 | case 702: |
---|
440 | constructWorldForTest (0,2, |
---|
441 | 0.5*SIDE,0.5*SIDE,1, -0.5*SIDE,-0.5*SIDE,1, |
---|
442 | 1,1,0, 1,1,0, 0.25*M_PI,0.25*M_PI); |
---|
443 | joint = dJointCreateUniversal (world,0); |
---|
444 | dJointAttach (joint,body[0],body[1]); |
---|
445 | dJointSetUniversalAnchor (joint,0,0,1); |
---|
446 | dJointSetUniversalAxis1 (joint, 1, -1, 1.41421356); |
---|
447 | dJointSetUniversalAxis2 (joint, 1, -1, -1.41421356); |
---|
448 | return 1; |
---|
449 | |
---|
450 | case 720: // universal transmit torque test |
---|
451 | case 721: |
---|
452 | case 722: |
---|
453 | case 730: // universal torque about axis 1 |
---|
454 | case 731: |
---|
455 | case 732: |
---|
456 | case 740: // universal torque about axis 2 |
---|
457 | case 741: |
---|
458 | case 742: |
---|
459 | constructWorldForTest (0,2, |
---|
460 | 0.5*SIDE,0.5*SIDE,1, -0.5*SIDE,-0.5*SIDE,1, |
---|
461 | 1,0,0, 1,0,0, 0,0); |
---|
462 | joint = dJointCreateUniversal (world,0); |
---|
463 | dJointAttach (joint,body[0],body[1]); |
---|
464 | dJointSetUniversalAnchor (joint,0,0,1); |
---|
465 | dJointSetUniversalAxis1 (joint,0,0,1); |
---|
466 | dJointSetUniversalAxis2 (joint, 1, -1,0); |
---|
467 | max_iterations = 100; |
---|
468 | return 1; |
---|
469 | |
---|
470 | // Joint PR (Prismatic and Rotoide) |
---|
471 | case 800: // 2 body |
---|
472 | case 801: // 2 bodies with spring force and prismatic fixed |
---|
473 | case 802: // 2 bodies with torque on body1 and prismatic fixed |
---|
474 | constructWorldForTest (0, 2, |
---|
475 | -1.0, 0.0, 1.0, |
---|
476 | 1.0, 0.0, 1.0, |
---|
477 | 1,0,0, 1,0,0, |
---|
478 | 0, 0); |
---|
479 | joint = dJointCreatePR (world, 0); |
---|
480 | dJointAttach (joint, body[0], body[1]); |
---|
481 | dJointSetPRAnchor (joint,-0.5, 0.0, 1.0); |
---|
482 | dJointSetPRAxis1 (joint, 0, 1, 0); |
---|
483 | dJointSetPRAxis2 (joint, 1, 0, 0); |
---|
484 | dJointSetPRParam (joint,dParamLoStop,-0.5); |
---|
485 | dJointSetPRParam (joint,dParamHiStop,0.5); |
---|
486 | dJointSetPRParam (joint,dParamLoStop2,0); |
---|
487 | dJointSetPRParam (joint,dParamHiStop2,0); |
---|
488 | return 1; |
---|
489 | case 803: // 2 bodies with spring force and prismatic NOT fixed |
---|
490 | case 804: // 2 bodies with torque force and prismatic NOT fixed |
---|
491 | case 805: // 2 bodies with force only on first body |
---|
492 | constructWorldForTest (0, 2, |
---|
493 | -1.0, 0.0, 1.0, |
---|
494 | 1.0, 0.0, 1.0, |
---|
495 | 1,0,0, 1,0,0, |
---|
496 | 0, 0); |
---|
497 | joint = dJointCreatePR (world, 0); |
---|
498 | dJointAttach (joint, body[0], body[1]); |
---|
499 | dJointSetPRAnchor (joint,-0.5, 0.0, 1.0); |
---|
500 | dJointSetPRAxis1 (joint, 0, 1, 0); |
---|
501 | dJointSetPRAxis2 (joint, 1, 0, 0); |
---|
502 | dJointSetPRParam (joint,dParamLoStop,-0.5); |
---|
503 | dJointSetPRParam (joint,dParamHiStop,0.5); |
---|
504 | dJointSetPRParam (joint,dParamLoStop2,-0.5); |
---|
505 | dJointSetPRParam (joint,dParamHiStop2,0.5); |
---|
506 | return 1; |
---|
507 | } |
---|
508 | return 0; |
---|
509 | } |
---|
510 | |
---|
511 | |
---|
512 | // do stuff specific to this test each iteration. you can check some |
---|
513 | // invariants for the test -- the return value is some scaled error measurement |
---|
514 | // that must be less than 1. |
---|
515 | // return a dInfinity if error is not measured for this n. |
---|
516 | |
---|
517 | dReal doStuffAndGetError (int n) |
---|
518 | { |
---|
519 | switch (n) { |
---|
520 | |
---|
521 | // ********** fixed joint |
---|
522 | |
---|
523 | case 0: { // 2 body |
---|
524 | addOscillatingTorque (0.1); |
---|
525 | dampRotationalMotion (0.1); |
---|
526 | // check the orientations are the same |
---|
527 | const dReal *R1 = dBodyGetRotation (body[0]); |
---|
528 | const dReal *R2 = dBodyGetRotation (body[1]); |
---|
529 | dReal err1 = dMaxDifference (R1,R2,3,3); |
---|
530 | // check the body offset is correct |
---|
531 | dVector3 p,pp; |
---|
532 | const dReal *p1 = dBodyGetPosition (body[0]); |
---|
533 | const dReal *p2 = dBodyGetPosition (body[1]); |
---|
534 | for (int i=0; i<3; i++) p[i] = p2[i] - p1[i]; |
---|
535 | dMULTIPLY1_331 (pp,R1,p); |
---|
536 | pp[0] += 0.5; |
---|
537 | pp[1] += 0.5; |
---|
538 | return (err1 + length (pp)) * 300; |
---|
539 | } |
---|
540 | |
---|
541 | case 1: { // 1 body to static env |
---|
542 | addOscillatingTorque (0.1); |
---|
543 | |
---|
544 | // check the orientation is the identity |
---|
545 | dReal err1 = cmpIdentity (dBodyGetRotation (body[0])); |
---|
546 | |
---|
547 | // check the body offset is correct |
---|
548 | dVector3 p; |
---|
549 | const dReal *p1 = dBodyGetPosition (body[0]); |
---|
550 | for (int i=0; i<3; i++) p[i] = p1[i]; |
---|
551 | p[0] -= 0.25; |
---|
552 | p[1] -= 0.25; |
---|
553 | p[2] -= 1; |
---|
554 | return (err1 + length (p)) * 1e6; |
---|
555 | } |
---|
556 | |
---|
557 | case 2: { // 2 body |
---|
558 | addOscillatingTorque (0.1); |
---|
559 | dampRotationalMotion (0.1); |
---|
560 | // check the body offset is correct |
---|
561 | // Should really check body rotation too. Oh well. |
---|
562 | const dReal *R1 = dBodyGetRotation (body[0]); |
---|
563 | dVector3 p,pp; |
---|
564 | const dReal *p1 = dBodyGetPosition (body[0]); |
---|
565 | const dReal *p2 = dBodyGetPosition (body[1]); |
---|
566 | for (int i=0; i<3; i++) p[i] = p2[i] - p1[i]; |
---|
567 | dMULTIPLY1_331 (pp,R1,p); |
---|
568 | pp[0] += 0.5; |
---|
569 | pp[1] += 0.5; |
---|
570 | return length(pp) * 300; |
---|
571 | } |
---|
572 | |
---|
573 | case 3: { // 1 body to static env with relative rotation |
---|
574 | addOscillatingTorque (0.1); |
---|
575 | |
---|
576 | // check the body offset is correct |
---|
577 | dVector3 p; |
---|
578 | const dReal *p1 = dBodyGetPosition (body[0]); |
---|
579 | for (int i=0; i<3; i++) p[i] = p1[i]; |
---|
580 | p[0] -= 0.25; |
---|
581 | p[1] -= 0.25; |
---|
582 | p[2] -= 1; |
---|
583 | return length (p) * 1e6; |
---|
584 | } |
---|
585 | |
---|
586 | |
---|
587 | // ********** hinge joint |
---|
588 | |
---|
589 | case 200: // 2 body |
---|
590 | addOscillatingTorque (0.1); |
---|
591 | dampRotationalMotion (0.1); |
---|
592 | return dInfinity; |
---|
593 | |
---|
594 | case 220: // hinge angle polarity test |
---|
595 | dBodyAddTorque (body[0],0,0,0.01); |
---|
596 | dBodyAddTorque (body[1],0,0,-0.01); |
---|
597 | if (iteration == 40) { |
---|
598 | dReal a = dJointGetHingeAngle (joint); |
---|
599 | if (a > 0.5 && a < 1) return 0; else return 10; |
---|
600 | } |
---|
601 | return 0; |
---|
602 | |
---|
603 | case 221: { // hinge angle rate test |
---|
604 | static dReal last_angle = 0; |
---|
605 | dBodyAddTorque (body[0],0,0,0.01); |
---|
606 | dBodyAddTorque (body[1],0,0,-0.01); |
---|
607 | dReal a = dJointGetHingeAngle (joint); |
---|
608 | dReal r = dJointGetHingeAngleRate (joint); |
---|
609 | dReal er = (a-last_angle)/STEPSIZE; // estimated rate |
---|
610 | last_angle = a; |
---|
611 | return fabs(r-er) * 4e4; |
---|
612 | } |
---|
613 | |
---|
614 | case 230: // hinge motor rate (and polarity) test |
---|
615 | case 231: { // ...with stops |
---|
616 | static dReal a = 0; |
---|
617 | dReal r = dJointGetHingeAngleRate (joint); |
---|
618 | dReal err = fabs (cos(a) - r); |
---|
619 | if (a==0) err = 0; |
---|
620 | a += 0.03; |
---|
621 | dJointSetHingeParam (joint,dParamVel,cos(a)); |
---|
622 | if (n==231) return dInfinity; |
---|
623 | return err * 1e6; |
---|
624 | } |
---|
625 | |
---|
626 | // ********** slider joint |
---|
627 | |
---|
628 | case 300: // 2 body |
---|
629 | addOscillatingTorque (0.05); |
---|
630 | dampRotationalMotion (0.1); |
---|
631 | addSpringForce (0.5); |
---|
632 | return dInfinity; |
---|
633 | |
---|
634 | case 320: // slider angle polarity test |
---|
635 | dBodyAddForce (body[0],0,0,0.1); |
---|
636 | dBodyAddForce (body[1],0,0,-0.1); |
---|
637 | if (iteration == 40) { |
---|
638 | dReal a = dJointGetSliderPosition (joint); |
---|
639 | if (a > 0.2 && a < 0.5) return 0; else return 10; |
---|
640 | return a; |
---|
641 | } |
---|
642 | return 0; |
---|
643 | |
---|
644 | case 321: { // slider angle rate test |
---|
645 | static dReal last_pos = 0; |
---|
646 | dBodyAddForce (body[0],0,0,0.1); |
---|
647 | dBodyAddForce (body[1],0,0,-0.1); |
---|
648 | dReal p = dJointGetSliderPosition (joint); |
---|
649 | dReal r = dJointGetSliderPositionRate (joint); |
---|
650 | dReal er = (p-last_pos)/STEPSIZE; // estimated rate (almost exact) |
---|
651 | last_pos = p; |
---|
652 | return fabs(r-er) * 1e9; |
---|
653 | } |
---|
654 | |
---|
655 | case 330: // slider motor rate (and polarity) test |
---|
656 | case 331: { // ...with stops |
---|
657 | static dReal a = 0; |
---|
658 | dReal r = dJointGetSliderPositionRate (joint); |
---|
659 | dReal err = fabs (0.7*cos(a) - r); |
---|
660 | if (a < 0.04) err = 0; |
---|
661 | a += 0.03; |
---|
662 | dJointSetSliderParam (joint,dParamVel,0.7*cos(a)); |
---|
663 | if (n==331) return dInfinity; |
---|
664 | return err * 1e6; |
---|
665 | } |
---|
666 | |
---|
667 | // ********** hinge-2 joint |
---|
668 | |
---|
669 | case 420: // hinge-2 steering angle polarity test |
---|
670 | dBodyAddTorque (body[0],0,0,0.01); |
---|
671 | dBodyAddTorque (body[1],0,0,-0.01); |
---|
672 | if (iteration == 40) { |
---|
673 | dReal a = dJointGetHinge2Angle1 (joint); |
---|
674 | if (a > 0.5 && a < 0.6) return 0; else return 10; |
---|
675 | } |
---|
676 | return 0; |
---|
677 | |
---|
678 | case 421: { // hinge-2 steering angle rate test |
---|
679 | static dReal last_angle = 0; |
---|
680 | dBodyAddTorque (body[0],0,0,0.01); |
---|
681 | dBodyAddTorque (body[1],0,0,-0.01); |
---|
682 | dReal a = dJointGetHinge2Angle1 (joint); |
---|
683 | dReal r = dJointGetHinge2Angle1Rate (joint); |
---|
684 | dReal er = (a-last_angle)/STEPSIZE; // estimated rate |
---|
685 | last_angle = a; |
---|
686 | return fabs(r-er)*2e4; |
---|
687 | } |
---|
688 | |
---|
689 | case 430: // hinge 2 steering motor rate (+polarity) test |
---|
690 | case 431: { // ...with stops |
---|
691 | static dReal a = 0; |
---|
692 | dReal r = dJointGetHinge2Angle1Rate (joint); |
---|
693 | dReal err = fabs (cos(a) - r); |
---|
694 | if (a==0) err = 0; |
---|
695 | a += 0.03; |
---|
696 | dJointSetHinge2Param (joint,dParamVel,cos(a)); |
---|
697 | if (n==431) return dInfinity; |
---|
698 | return err * 1e6; |
---|
699 | } |
---|
700 | |
---|
701 | case 432: { // hinge 2 wheel motor rate (+polarity) test |
---|
702 | static dReal a = 0; |
---|
703 | dReal r = dJointGetHinge2Angle2Rate (joint); |
---|
704 | dReal err = fabs (cos(a) - r); |
---|
705 | if (a==0) err = 0; |
---|
706 | a += 0.03; |
---|
707 | dJointSetHinge2Param (joint,dParamVel2,cos(a)); |
---|
708 | return err * 1e6; |
---|
709 | } |
---|
710 | |
---|
711 | // ********** angular motor joint |
---|
712 | |
---|
713 | case 600: { // test euler angle calculations |
---|
714 | // desired euler angles from last iteration |
---|
715 | static dReal a1,a2,a3; |
---|
716 | |
---|
717 | // find actual euler angles |
---|
718 | dReal aa1 = dJointGetAMotorAngle (joint,0); |
---|
719 | dReal aa2 = dJointGetAMotorAngle (joint,1); |
---|
720 | dReal aa3 = dJointGetAMotorAngle (joint,2); |
---|
721 | // printf ("actual = %.4f %.4f %.4f\n\n",aa1,aa2,aa3); |
---|
722 | |
---|
723 | dReal err = dInfinity; |
---|
724 | if (iteration > 0) { |
---|
725 | err = dFabs(aa1-a1) + dFabs(aa2-a2) + dFabs(aa3-a3); |
---|
726 | err *= 1e10; |
---|
727 | } |
---|
728 | |
---|
729 | // get random base rotation for both bodies |
---|
730 | dMatrix3 Rbase; |
---|
731 | dRFromAxisAndAngle (Rbase, 3*(dRandReal()-0.5), 3*(dRandReal()-0.5), |
---|
732 | 3*(dRandReal()-0.5), 3*(dRandReal()-0.5)); |
---|
733 | dBodySetRotation (body[0],Rbase); |
---|
734 | |
---|
735 | // rotate body 2 by random euler angles w.r.t. body 1 |
---|
736 | a1 = 3.14 * 2 * (dRandReal()-0.5); |
---|
737 | a2 = 1.57 * 2 * (dRandReal()-0.5); |
---|
738 | a3 = 3.14 * 2 * (dRandReal()-0.5); |
---|
739 | dMatrix3 R1,R2,R3,Rtmp1,Rtmp2; |
---|
740 | dRFromAxisAndAngle (R1,0,0,1,-a1); |
---|
741 | dRFromAxisAndAngle (R2,0,1,0,a2); |
---|
742 | dRFromAxisAndAngle (R3,1,0,0,-a3); |
---|
743 | dMultiply0 (Rtmp1,R2,R3,3,3,3); |
---|
744 | dMultiply0 (Rtmp2,R1,Rtmp1,3,3,3); |
---|
745 | dMultiply0 (Rtmp1,Rbase,Rtmp2,3,3,3); |
---|
746 | dBodySetRotation (body[1],Rtmp1); |
---|
747 | // printf ("desired = %.4f %.4f %.4f\n",a1,a2,a3); |
---|
748 | |
---|
749 | return err; |
---|
750 | } |
---|
751 | |
---|
752 | // ********** universal joint |
---|
753 | |
---|
754 | case 700: { // 2 body: joint constraint |
---|
755 | dVector3 ax1, ax2; |
---|
756 | |
---|
757 | addOscillatingTorque (0.1); |
---|
758 | dampRotationalMotion (0.1); |
---|
759 | dJointGetUniversalAxis1(joint, ax1); |
---|
760 | dJointGetUniversalAxis2(joint, ax2); |
---|
761 | return fabs(10*dDOT(ax1, ax2)); |
---|
762 | } |
---|
763 | |
---|
764 | case 701: { // 2 body: angle 1 rate |
---|
765 | static dReal last_angle = 0; |
---|
766 | addOscillatingTorque (0.1); |
---|
767 | dampRotationalMotion (0.1); |
---|
768 | dReal a = dJointGetUniversalAngle1(joint); |
---|
769 | dReal r = dJointGetUniversalAngle1Rate(joint); |
---|
770 | dReal diff = a - last_angle; |
---|
771 | if (diff > M_PI) diff -= 2*M_PI; |
---|
772 | if (diff < -M_PI) diff += 2*M_PI; |
---|
773 | dReal er = diff / STEPSIZE; // estimated rate |
---|
774 | last_angle = a; |
---|
775 | // I'm not sure why the error is so large here. |
---|
776 | return fabs(r - er) * 1e1; |
---|
777 | } |
---|
778 | |
---|
779 | case 702: { // 2 body: angle 2 rate |
---|
780 | static dReal last_angle = 0; |
---|
781 | addOscillatingTorque (0.1); |
---|
782 | dampRotationalMotion (0.1); |
---|
783 | dReal a = dJointGetUniversalAngle2(joint); |
---|
784 | dReal r = dJointGetUniversalAngle2Rate(joint); |
---|
785 | dReal diff = a - last_angle; |
---|
786 | if (diff > M_PI) diff -= 2*M_PI; |
---|
787 | if (diff < -M_PI) diff += 2*M_PI; |
---|
788 | dReal er = diff / STEPSIZE; // estimated rate |
---|
789 | last_angle = a; |
---|
790 | // I'm not sure why the error is so large here. |
---|
791 | return fabs(r - er) * 1e1; |
---|
792 | } |
---|
793 | |
---|
794 | case 720: { // universal transmit torque test: constraint error |
---|
795 | dVector3 ax1, ax2; |
---|
796 | addOscillatingTorqueAbout (0.1, 1, 1, 0); |
---|
797 | dampRotationalMotion (0.1); |
---|
798 | dJointGetUniversalAxis1(joint, ax1); |
---|
799 | dJointGetUniversalAxis2(joint, ax2); |
---|
800 | return fabs(10*dDOT(ax1, ax2)); |
---|
801 | } |
---|
802 | |
---|
803 | case 721: { // universal transmit torque test: angle1 rate |
---|
804 | static dReal last_angle = 0; |
---|
805 | addOscillatingTorqueAbout (0.1, 1, 1, 0); |
---|
806 | dampRotationalMotion (0.1); |
---|
807 | dReal a = dJointGetUniversalAngle1(joint); |
---|
808 | dReal r = dJointGetUniversalAngle1Rate(joint); |
---|
809 | dReal diff = a - last_angle; |
---|
810 | if (diff > M_PI) diff -= 2*M_PI; |
---|
811 | if (diff < -M_PI) diff += 2*M_PI; |
---|
812 | dReal er = diff / STEPSIZE; // estimated rate |
---|
813 | last_angle = a; |
---|
814 | return fabs(r - er) * 1e10; |
---|
815 | } |
---|
816 | |
---|
817 | case 722: { // universal transmit torque test: angle2 rate |
---|
818 | static dReal last_angle = 0; |
---|
819 | addOscillatingTorqueAbout (0.1, 1, 1, 0); |
---|
820 | dampRotationalMotion (0.1); |
---|
821 | dReal a = dJointGetUniversalAngle2(joint); |
---|
822 | dReal r = dJointGetUniversalAngle2Rate(joint); |
---|
823 | dReal diff = a - last_angle; |
---|
824 | if (diff > M_PI) diff -= 2*M_PI; |
---|
825 | if (diff < -M_PI) diff += 2*M_PI; |
---|
826 | dReal er = diff / STEPSIZE; // estimated rate |
---|
827 | last_angle = a; |
---|
828 | return fabs(r - er) * 1e10; |
---|
829 | } |
---|
830 | |
---|
831 | case 730:{ |
---|
832 | dVector3 ax1, ax2; |
---|
833 | dJointGetUniversalAxis1(joint, ax1); |
---|
834 | dJointGetUniversalAxis2(joint, ax2); |
---|
835 | addOscillatingTorqueAbout (0.1, ax1[0], ax1[1], ax1[2]); |
---|
836 | dampRotationalMotion (0.1); |
---|
837 | return fabs(10*dDOT(ax1, ax2)); |
---|
838 | } |
---|
839 | |
---|
840 | case 731:{ |
---|
841 | dVector3 ax1; |
---|
842 | static dReal last_angle = 0; |
---|
843 | dJointGetUniversalAxis1(joint, ax1); |
---|
844 | addOscillatingTorqueAbout (0.1, ax1[0], ax1[1], ax1[2]); |
---|
845 | dampRotationalMotion (0.1); |
---|
846 | dReal a = dJointGetUniversalAngle1(joint); |
---|
847 | dReal r = dJointGetUniversalAngle1Rate(joint); |
---|
848 | dReal diff = a - last_angle; |
---|
849 | if (diff > M_PI) diff -= 2*M_PI; |
---|
850 | if (diff < -M_PI) diff += 2*M_PI; |
---|
851 | dReal er = diff / STEPSIZE; // estimated rate |
---|
852 | last_angle = a; |
---|
853 | return fabs(r - er) * 2e3; |
---|
854 | } |
---|
855 | |
---|
856 | case 732:{ |
---|
857 | dVector3 ax1; |
---|
858 | static dReal last_angle = 0; |
---|
859 | dJointGetUniversalAxis1(joint, ax1); |
---|
860 | addOscillatingTorqueAbout (0.1, ax1[0], ax1[1], ax1[2]); |
---|
861 | dampRotationalMotion (0.1); |
---|
862 | dReal a = dJointGetUniversalAngle2(joint); |
---|
863 | dReal r = dJointGetUniversalAngle2Rate(joint); |
---|
864 | dReal diff = a - last_angle; |
---|
865 | if (diff > M_PI) diff -= 2*M_PI; |
---|
866 | if (diff < -M_PI) diff += 2*M_PI; |
---|
867 | dReal er = diff / STEPSIZE; // estimated rate |
---|
868 | last_angle = a; |
---|
869 | return fabs(r - er) * 1e10; |
---|
870 | } |
---|
871 | |
---|
872 | case 740:{ |
---|
873 | dVector3 ax1, ax2; |
---|
874 | dJointGetUniversalAxis1(joint, ax1); |
---|
875 | dJointGetUniversalAxis2(joint, ax2); |
---|
876 | addOscillatingTorqueAbout (0.1, ax2[0], ax2[1], ax2[2]); |
---|
877 | dampRotationalMotion (0.1); |
---|
878 | return fabs(10*dDOT(ax1, ax2)); |
---|
879 | } |
---|
880 | |
---|
881 | case 741:{ |
---|
882 | dVector3 ax2; |
---|
883 | static dReal last_angle = 0; |
---|
884 | dJointGetUniversalAxis2(joint, ax2); |
---|
885 | addOscillatingTorqueAbout (0.1, ax2[0], ax2[1], ax2[2]); |
---|
886 | dampRotationalMotion (0.1); |
---|
887 | dReal a = dJointGetUniversalAngle1(joint); |
---|
888 | dReal r = dJointGetUniversalAngle1Rate(joint); |
---|
889 | dReal diff = a - last_angle; |
---|
890 | if (diff > M_PI) diff -= 2*M_PI; |
---|
891 | if (diff < -M_PI) diff += 2*M_PI; |
---|
892 | dReal er = diff / STEPSIZE; // estimated rate |
---|
893 | last_angle = a; |
---|
894 | return fabs(r - er) * 1e10; |
---|
895 | } |
---|
896 | |
---|
897 | case 742:{ |
---|
898 | dVector3 ax2; |
---|
899 | static dReal last_angle = 0; |
---|
900 | dJointGetUniversalAxis2(joint, ax2); |
---|
901 | addOscillatingTorqueAbout (0.1, ax2[0], ax2[1], ax2[2]); |
---|
902 | dampRotationalMotion (0.1); |
---|
903 | dReal a = dJointGetUniversalAngle2(joint); |
---|
904 | dReal r = dJointGetUniversalAngle2Rate(joint); |
---|
905 | dReal diff = a - last_angle; |
---|
906 | if (diff > M_PI) diff -= 2*M_PI; |
---|
907 | if (diff < -M_PI) diff += 2*M_PI; |
---|
908 | dReal er = diff / STEPSIZE; // estimated rate |
---|
909 | last_angle = a; |
---|
910 | return fabs(r - er) * 1e4; |
---|
911 | } |
---|
912 | |
---|
913 | // ********** slider joint |
---|
914 | case 801: |
---|
915 | case 803: |
---|
916 | addSpringForce (0.25); |
---|
917 | return dInfinity; |
---|
918 | |
---|
919 | case 802: |
---|
920 | case 804: { |
---|
921 | static dReal a = 0; |
---|
922 | dBodyAddTorque (body[0], 0, 0.01*cos(1.5708*a), 0); |
---|
923 | a += 0.01; |
---|
924 | return dInfinity; |
---|
925 | } |
---|
926 | |
---|
927 | case 805: |
---|
928 | addOscillatingForce (0.1); |
---|
929 | return dInfinity; |
---|
930 | } |
---|
931 | |
---|
932 | |
---|
933 | return dInfinity; |
---|
934 | } |
---|
935 | |
---|
936 | //**************************************************************************** |
---|
937 | // simulation stuff common to all the tests |
---|
938 | |
---|
939 | // start simulation - set viewpoint |
---|
940 | |
---|
941 | static void start() |
---|
942 | { |
---|
943 | static float xyz[3] = {1.0382f,-1.0811f,1.4700f}; |
---|
944 | static float hpr[3] = {135.0000f,-19.5000f,0.0000f}; |
---|
945 | dsSetViewpoint (xyz,hpr); |
---|
946 | } |
---|
947 | |
---|
948 | |
---|
949 | // simulation loop |
---|
950 | |
---|
951 | static void simLoop (int pause) |
---|
952 | { |
---|
953 | // stop after a given number of iterations, as long as we are not in |
---|
954 | // interactive mode |
---|
955 | if (cmd_graphics && !cmd_interactive && |
---|
956 | (iteration >= max_iterations)) { |
---|
957 | dsStop(); |
---|
958 | return; |
---|
959 | } |
---|
960 | iteration++; |
---|
961 | |
---|
962 | if (!pause) { |
---|
963 | // do stuff for this test and check to see if the joint is behaving well |
---|
964 | dReal error = doStuffAndGetError (test_num); |
---|
965 | if (error > max_error) max_error = error; |
---|
966 | if (cmd_interactive && error < dInfinity) { |
---|
967 | printf ("scaled error = %.4e\n",error); |
---|
968 | } |
---|
969 | |
---|
970 | // take a step |
---|
971 | dWorldStep (world,STEPSIZE); |
---|
972 | |
---|
973 | // occasionally re-orient the first body to create a deliberate error. |
---|
974 | if (cmd_occasional_error) { |
---|
975 | static int count = 0; |
---|
976 | if ((count % 20)==0) { |
---|
977 | // randomly adjust orientation of body[0] |
---|
978 | const dReal *R1; |
---|
979 | dMatrix3 R2,R3; |
---|
980 | R1 = dBodyGetRotation (body[0]); |
---|
981 | dRFromAxisAndAngle (R2,dRandReal()-0.5,dRandReal()-0.5, |
---|
982 | dRandReal()-0.5,dRandReal()-0.5); |
---|
983 | dMultiply0 (R3,R1,R2,3,3,3); |
---|
984 | dBodySetRotation (body[0],R3); |
---|
985 | |
---|
986 | // randomly adjust position of body[0] |
---|
987 | const dReal *pos = dBodyGetPosition (body[0]); |
---|
988 | dBodySetPosition (body[0], |
---|
989 | pos[0]+0.2*(dRandReal()-0.5), |
---|
990 | pos[1]+0.2*(dRandReal()-0.5), |
---|
991 | pos[2]+0.2*(dRandReal()-0.5)); |
---|
992 | } |
---|
993 | count++; |
---|
994 | } |
---|
995 | } |
---|
996 | |
---|
997 | if (cmd_graphics) { |
---|
998 | dReal sides1[3] = {SIDE,SIDE,SIDE}; |
---|
999 | dReal sides2[3] = {SIDE*0.99f,SIDE*0.99f,SIDE*0.99f}; |
---|
1000 | dsSetTexture (DS_WOOD); |
---|
1001 | dsSetColor (1,1,0); |
---|
1002 | dsDrawBox (dBodyGetPosition(body[0]),dBodyGetRotation(body[0]),sides1); |
---|
1003 | if (body[1]) { |
---|
1004 | dsSetColor (0,1,1); |
---|
1005 | dsDrawBox (dBodyGetPosition(body[1]),dBodyGetRotation(body[1]),sides2); |
---|
1006 | } |
---|
1007 | } |
---|
1008 | } |
---|
1009 | |
---|
1010 | //**************************************************************************** |
---|
1011 | // conduct a specific test, and report the results |
---|
1012 | |
---|
1013 | void doTest (int argc, char **argv, int n, int fatal_if_bad_n) |
---|
1014 | { |
---|
1015 | test_num = n; |
---|
1016 | iteration = 0; |
---|
1017 | max_iterations = 300; |
---|
1018 | max_error = 0; |
---|
1019 | |
---|
1020 | if (! setupTest (n)) { |
---|
1021 | if (fatal_if_bad_n) dError (0,"bad test number"); |
---|
1022 | return; |
---|
1023 | } |
---|
1024 | |
---|
1025 | // setup pointers to drawstuff callback functions |
---|
1026 | dsFunctions fn; |
---|
1027 | fn.version = DS_VERSION; |
---|
1028 | fn.start = &start; |
---|
1029 | fn.step = &simLoop; |
---|
1030 | fn.command = 0; |
---|
1031 | fn.stop = 0; |
---|
1032 | if (cmd_path_to_textures) |
---|
1033 | fn.path_to_textures = cmd_path_to_textures; |
---|
1034 | else |
---|
1035 | fn.path_to_textures = "../../drawstuff/textures"; |
---|
1036 | |
---|
1037 | // run simulation |
---|
1038 | if (cmd_graphics) { |
---|
1039 | dsSimulationLoop (argc,argv,352,288,&fn); |
---|
1040 | } |
---|
1041 | else { |
---|
1042 | for (int i=0; i < max_iterations; i++) simLoop (0); |
---|
1043 | } |
---|
1044 | dWorldDestroy (world); |
---|
1045 | body[0] = 0; |
---|
1046 | body[1] = 0; |
---|
1047 | joint = 0; |
---|
1048 | |
---|
1049 | // print results |
---|
1050 | printf ("test %d: ",n); |
---|
1051 | if (max_error == dInfinity) printf ("error not computed\n"); |
---|
1052 | else { |
---|
1053 | printf ("max scaled error = %.4e",max_error); |
---|
1054 | if (max_error < 1) printf (" - passed\n"); |
---|
1055 | else printf (" - FAILED\n"); |
---|
1056 | } |
---|
1057 | } |
---|
1058 | |
---|
1059 | //**************************************************************************** |
---|
1060 | // main |
---|
1061 | |
---|
1062 | int main (int argc, char **argv) |
---|
1063 | { |
---|
1064 | int i; |
---|
1065 | dInitODE(); |
---|
1066 | |
---|
1067 | // process the command line args. anything that starts with `-' is assumed |
---|
1068 | // to be a drawstuff argument. |
---|
1069 | for (i=1; i<argc; i++) { |
---|
1070 | if ( argv[i][0]=='-' && argv[i][1]=='i' && argv[i][2]==0) cmd_interactive = 1; |
---|
1071 | else if ( argv[i][0]=='-' && argv[i][1]=='g' && argv[i][2]==0) cmd_graphics = 0; |
---|
1072 | else if ( argv[i][0]=='-' && argv[i][1]=='e' && argv[i][2]==0) cmd_graphics = 0; |
---|
1073 | else if ( argv[i][0]=='-' && argv[i][1]=='n' && isdigit(argv[i][2]) ) { |
---|
1074 | char *endptr; |
---|
1075 | long int n = strtol (&(argv[i][2]),&endptr,10); |
---|
1076 | if (*endptr == 0) cmd_test_num = n; |
---|
1077 | } |
---|
1078 | else |
---|
1079 | cmd_path_to_textures = argv[i]; |
---|
1080 | } |
---|
1081 | |
---|
1082 | // do the tests |
---|
1083 | if (cmd_test_num == -1) { |
---|
1084 | for (i=0; i<NUM_JOINTS*100; i++) doTest (argc,argv,i,0); |
---|
1085 | } |
---|
1086 | else { |
---|
1087 | doTest (argc,argv,cmd_test_num,1); |
---|
1088 | } |
---|
1089 | |
---|
1090 | dCloseODE(); |
---|
1091 | return 0; |
---|
1092 | } |
---|