diff options
Diffstat (limited to 'libraries/ode-0.9/ode/demo/demo_I.cpp')
-rw-r--r-- | libraries/ode-0.9/ode/demo/demo_I.cpp | 254 |
1 files changed, 0 insertions, 254 deletions
diff --git a/libraries/ode-0.9/ode/demo/demo_I.cpp b/libraries/ode-0.9/ode/demo/demo_I.cpp deleted file mode 100644 index b4f8d6e..0000000 --- a/libraries/ode-0.9/ode/demo/demo_I.cpp +++ /dev/null | |||
@@ -1,254 +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 | |||
25 | test that the rotational physics is correct. | ||
26 | |||
27 | an "anchor body" has a number of other randomly positioned bodies | ||
28 | ("particles") attached to it by ball-and-socket joints, giving it some | ||
29 | random effective inertia tensor. the effective inertia matrix is calculated, | ||
30 | and then this inertia is assigned to another "test" body. a random torque is | ||
31 | applied to both bodies and the difference in angular velocity and orientation | ||
32 | is observed after a number of iterations. | ||
33 | |||
34 | typical errors for each test cycle are about 1e-5 ... 1e-4. | ||
35 | |||
36 | */ | ||
37 | |||
38 | |||
39 | #include <time.h> | ||
40 | #include <ode/ode.h> | ||
41 | #include <drawstuff/drawstuff.h> | ||
42 | |||
43 | #ifdef _MSC_VER | ||
44 | #pragma warning(disable:4244 4305) // for VC++, no precision loss complaints | ||
45 | #endif | ||
46 | |||
47 | // select correct drawing functions | ||
48 | |||
49 | #ifdef dDOUBLE | ||
50 | #define dsDrawBox dsDrawBoxD | ||
51 | #define dsDrawSphere dsDrawSphereD | ||
52 | #define dsDrawCylinder dsDrawCylinderD | ||
53 | #define dsDrawCapsule dsDrawCapsuleD | ||
54 | #endif | ||
55 | |||
56 | |||
57 | // some constants | ||
58 | |||
59 | #define NUM 10 // number of particles | ||
60 | #define SIDE 0.1 // visual size of the particles | ||
61 | |||
62 | |||
63 | // dynamics objects an globals | ||
64 | |||
65 | static dWorldID world=0; | ||
66 | static dBodyID anchor_body,particle[NUM],test_body; | ||
67 | static dJointID particle_joint[NUM]; | ||
68 | static dReal torque[3]; | ||
69 | static int iteration; | ||
70 | |||
71 | |||
72 | // start simulation - set viewpoint | ||
73 | |||
74 | static void start() | ||
75 | { | ||
76 | static float xyz[3] = {1.5572f,-1.8886f,1.5700f}; | ||
77 | static float hpr[3] = {118.5000f,-17.0000f,0.0000f}; | ||
78 | dsSetViewpoint (xyz,hpr); | ||
79 | } | ||
80 | |||
81 | |||
82 | // compute the mass parameters of a particle set. q = particle positions, | ||
83 | // pm = particle masses | ||
84 | |||
85 | #define _I(i,j) I[(i)*4+(j)] | ||
86 | |||
87 | void computeMassParams (dMass *m, dReal q[NUM][3], dReal pm[NUM]) | ||
88 | { | ||
89 | int i,j; | ||
90 | dMassSetZero (m); | ||
91 | for (i=0; i<NUM; i++) { | ||
92 | m->mass += pm[i]; | ||
93 | for (j=0; j<3; j++) m->c[j] += pm[i]*q[i][j]; | ||
94 | m->_I(0,0) += pm[i]*(q[i][1]*q[i][1] + q[i][2]*q[i][2]); | ||
95 | m->_I(1,1) += pm[i]*(q[i][0]*q[i][0] + q[i][2]*q[i][2]); | ||
96 | m->_I(2,2) += pm[i]*(q[i][0]*q[i][0] + q[i][1]*q[i][1]); | ||
97 | m->_I(0,1) -= pm[i]*(q[i][0]*q[i][1]); | ||
98 | m->_I(0,2) -= pm[i]*(q[i][0]*q[i][2]); | ||
99 | m->_I(1,2) -= pm[i]*(q[i][1]*q[i][2]); | ||
100 | } | ||
101 | for (j=0; j<3; j++) m->c[j] /= m->mass; | ||
102 | m->_I(1,0) = m->_I(0,1); | ||
103 | m->_I(2,0) = m->_I(0,2); | ||
104 | m->_I(2,1) = m->_I(1,2); | ||
105 | } | ||
106 | |||
107 | |||
108 | void reset_test() | ||
109 | { | ||
110 | int i; | ||
111 | dMass m,anchor_m; | ||
112 | dReal q[NUM][3], pm[NUM]; // particle positions and masses | ||
113 | dReal pos1[3] = {1,0,1}; // point of reference (POR) | ||
114 | dReal pos2[3] = {-1,0,1}; // point of reference (POR) | ||
115 | |||
116 | // make random particle positions (relative to POR) and masses | ||
117 | for (i=0; i<NUM; i++) { | ||
118 | pm[i] = dRandReal()+0.1; | ||
119 | q[i][0] = dRandReal()-0.5; | ||
120 | q[i][1] = dRandReal()-0.5; | ||
121 | q[i][2] = dRandReal()-0.5; | ||
122 | } | ||
123 | |||
124 | // adjust particle positions so centor of mass = POR | ||
125 | computeMassParams (&m,q,pm); | ||
126 | for (i=0; i<NUM; i++) { | ||
127 | q[i][0] -= m.c[0]; | ||
128 | q[i][1] -= m.c[1]; | ||
129 | q[i][2] -= m.c[2]; | ||
130 | } | ||
131 | |||
132 | if (world) dWorldDestroy (world); | ||
133 | world = dWorldCreate(); | ||
134 | |||
135 | anchor_body = dBodyCreate (world); | ||
136 | dBodySetPosition (anchor_body,pos1[0],pos1[1],pos1[2]); | ||
137 | dMassSetBox (&anchor_m,1,SIDE,SIDE,SIDE); | ||
138 | dMassAdjust (&anchor_m,0.1); | ||
139 | dBodySetMass (anchor_body,&anchor_m); | ||
140 | |||
141 | for (i=0; i<NUM; i++) { | ||
142 | particle[i] = dBodyCreate (world); | ||
143 | dBodySetPosition (particle[i], | ||
144 | pos1[0]+q[i][0],pos1[1]+q[i][1],pos1[2]+q[i][2]); | ||
145 | dMassSetBox (&m,1,SIDE,SIDE,SIDE); | ||
146 | dMassAdjust (&m,pm[i]); | ||
147 | dBodySetMass (particle[i],&m); | ||
148 | } | ||
149 | |||
150 | for (i=0; i < NUM; i++) { | ||
151 | particle_joint[i] = dJointCreateBall (world,0); | ||
152 | dJointAttach (particle_joint[i],anchor_body,particle[i]); | ||
153 | const dReal *p = dBodyGetPosition (particle[i]); | ||
154 | dJointSetBallAnchor (particle_joint[i],p[0],p[1],p[2]); | ||
155 | } | ||
156 | |||
157 | // make test_body with the same mass and inertia of the anchor_body plus | ||
158 | // all the particles | ||
159 | |||
160 | test_body = dBodyCreate (world); | ||
161 | dBodySetPosition (test_body,pos2[0],pos2[1],pos2[2]); | ||
162 | computeMassParams (&m,q,pm); | ||
163 | m.mass += anchor_m.mass; | ||
164 | for (i=0; i<12; i++) m.I[i] = m.I[i] + anchor_m.I[i]; | ||
165 | dBodySetMass (test_body,&m); | ||
166 | |||
167 | // rotate the test and anchor bodies by a random amount | ||
168 | dQuaternion qrot; | ||
169 | for (i=0; i<4; i++) qrot[i] = dRandReal()-0.5; | ||
170 | dNormalize4 (qrot); | ||
171 | dBodySetQuaternion (anchor_body,qrot); | ||
172 | dBodySetQuaternion (test_body,qrot); | ||
173 | dMatrix3 R; | ||
174 | dQtoR (qrot,R); | ||
175 | for (i=0; i<NUM; i++) { | ||
176 | dVector3 v; | ||
177 | dMultiply0 (v,R,&q[i][0],3,3,1); | ||
178 | dBodySetPosition (particle[i],pos1[0]+v[0],pos1[1]+v[1],pos1[2]+v[2]); | ||
179 | } | ||
180 | |||
181 | // set random torque | ||
182 | for (i=0; i<3; i++) torque[i] = (dRandReal()-0.5) * 0.1; | ||
183 | |||
184 | |||
185 | iteration=0; | ||
186 | } | ||
187 | |||
188 | |||
189 | // simulation loop | ||
190 | |||
191 | static void simLoop (int pause) | ||
192 | { | ||
193 | if (!pause) { | ||
194 | dBodyAddTorque (anchor_body,torque[0],torque[1],torque[2]); | ||
195 | dBodyAddTorque (test_body,torque[0],torque[1],torque[2]); | ||
196 | dWorldStep (world,0.03); | ||
197 | |||
198 | iteration++; | ||
199 | if (iteration >= 100) { | ||
200 | // measure the difference between the anchor and test bodies | ||
201 | const dReal *w1 = dBodyGetAngularVel (anchor_body); | ||
202 | const dReal *w2 = dBodyGetAngularVel (test_body); | ||
203 | const dReal *q1 = dBodyGetQuaternion (anchor_body); | ||
204 | const dReal *q2 = dBodyGetQuaternion (test_body); | ||
205 | dReal maxdiff = dMaxDifference (w1,w2,1,3); | ||
206 | printf ("w-error = %.4e (%.2f,%.2f,%.2f) and (%.2f,%.2f,%.2f)\n", | ||
207 | maxdiff,w1[0],w1[1],w1[2],w2[0],w2[1],w2[2]); | ||
208 | maxdiff = dMaxDifference (q1,q2,1,4); | ||
209 | printf ("q-error = %.4e\n",maxdiff); | ||
210 | reset_test(); | ||
211 | } | ||
212 | } | ||
213 | |||
214 | dReal sides[3] = {SIDE,SIDE,SIDE}; | ||
215 | dReal sides2[3] = {6*SIDE,6*SIDE,6*SIDE}; | ||
216 | dReal sides3[3] = {3*SIDE,3*SIDE,3*SIDE}; | ||
217 | dsSetColor (1,1,1); | ||
218 | dsDrawBox (dBodyGetPosition(anchor_body), dBodyGetRotation(anchor_body), | ||
219 | sides3); | ||
220 | dsSetColor (1,0,0); | ||
221 | dsDrawBox (dBodyGetPosition(test_body), dBodyGetRotation(test_body), sides2); | ||
222 | dsSetColor (1,1,0); | ||
223 | for (int i=0; i<NUM; i++) | ||
224 | dsDrawBox (dBodyGetPosition (particle[i]), | ||
225 | dBodyGetRotation (particle[i]), sides); | ||
226 | } | ||
227 | |||
228 | |||
229 | int main (int argc, char **argv) | ||
230 | { | ||
231 | // setup pointers to drawstuff callback functions | ||
232 | dsFunctions fn; | ||
233 | fn.version = DS_VERSION; | ||
234 | fn.start = &start; | ||
235 | fn.step = &simLoop; | ||
236 | fn.command = 0; | ||
237 | fn.stop = 0; | ||
238 | fn.path_to_textures = "../../drawstuff/textures"; | ||
239 | if(argc==2) | ||
240 | { | ||
241 | fn.path_to_textures = argv[1]; | ||
242 | } | ||
243 | |||
244 | dInitODE(); | ||
245 | dRandSetSeed (time(0)); | ||
246 | reset_test(); | ||
247 | |||
248 | // run simulation | ||
249 | dsSimulationLoop (argc,argv,352,288,&fn); | ||
250 | |||
251 | dWorldDestroy (world); | ||
252 | dCloseODE(); | ||
253 | return 0; | ||
254 | } | ||