|
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 #include <ode/ode.h> |
|
24 #include "object.h" |
|
25 #include "joint.h" |
|
26 #include "util.h" |
|
27 #include <ode/lookup_tables.h> |
|
28 |
|
29 //#define ALLOCA dALLOCA16 |
|
30 |
|
31 //**************************************************************************** |
|
32 // Auto disabling |
|
33 |
|
34 void dInternalHandleAutoDisabling (dxWorld *world, dReal stepsize) |
|
35 { |
|
36 dxBody *bb; |
|
37 for ( bb=world->firstbody; bb; bb=(dxBody*)bb->next ) |
|
38 { |
|
39 // don't freeze objects mid-air (patch 1586738) |
|
40 if ( bb->firstjoint == NULL ) continue; |
|
41 |
|
42 // nothing to do unless this body is currently enabled and has |
|
43 // the auto-disable flag set |
|
44 if ( (bb->flags & (dxBodyAutoDisable|dxBodyDisabled)) != dxBodyAutoDisable ) continue; |
|
45 |
|
46 // if sampling / threshold testing is disabled, we can never sleep. |
|
47 if ( bb->adis.average_samples == 0 ) continue; |
|
48 |
|
49 // |
|
50 // see if the body is idle |
|
51 // |
|
52 |
|
53 #ifndef dNODEBUG |
|
54 // sanity check |
|
55 if ( bb->average_counter >= bb->adis.average_samples ) |
|
56 { |
|
57 |
|
58 // something is going wrong, reset the average-calculations |
|
59 bb->average_ready = 0; // not ready for average calculation |
|
60 bb->average_counter = 0; // reset the buffer index |
|
61 } |
|
62 #endif // dNODEBUG |
|
63 |
|
64 // sample the linear and angular velocity |
|
65 bb->average_lvel_buffer[bb->average_counter][0] = bb->lvel[0]; |
|
66 bb->average_lvel_buffer[bb->average_counter][1] = bb->lvel[1]; |
|
67 bb->average_lvel_buffer[bb->average_counter][2] = bb->lvel[2]; |
|
68 bb->average_avel_buffer[bb->average_counter][0] = bb->avel[0]; |
|
69 bb->average_avel_buffer[bb->average_counter][1] = bb->avel[1]; |
|
70 bb->average_avel_buffer[bb->average_counter][2] = bb->avel[2]; |
|
71 bb->average_counter++; |
|
72 |
|
73 // buffer ready test |
|
74 if ( bb->average_counter >= bb->adis.average_samples ) |
|
75 { |
|
76 bb->average_counter = 0; // fill the buffer from the beginning |
|
77 bb->average_ready = 1; // this body is ready now for average calculation |
|
78 } |
|
79 |
|
80 int idle = 0; // Assume it's in motion unless we have samples to disprove it. |
|
81 |
|
82 // enough samples? |
|
83 if ( bb->average_ready ) |
|
84 { |
|
85 idle = 1; // Initial assumption: IDLE |
|
86 |
|
87 // the sample buffers are filled and ready for calculation |
|
88 dVector3 average_lvel, average_avel; |
|
89 |
|
90 // Store first velocity samples |
|
91 average_lvel[0] = bb->average_lvel_buffer[0][0]; |
|
92 average_avel[0] = bb->average_avel_buffer[0][0]; |
|
93 average_lvel[1] = bb->average_lvel_buffer[0][1]; |
|
94 average_avel[1] = bb->average_avel_buffer[0][1]; |
|
95 average_lvel[2] = bb->average_lvel_buffer[0][2]; |
|
96 average_avel[2] = bb->average_avel_buffer[0][2]; |
|
97 |
|
98 // If we're not in "instantaneous mode" |
|
99 if ( bb->adis.average_samples > 1 ) |
|
100 { |
|
101 // add remaining velocities together |
|
102 for ( unsigned int i = 1; i < bb->adis.average_samples; ++i ) |
|
103 { |
|
104 average_lvel[0] += bb->average_lvel_buffer[i][0]; |
|
105 average_avel[0] += bb->average_avel_buffer[i][0]; |
|
106 average_lvel[1] += bb->average_lvel_buffer[i][1]; |
|
107 average_avel[1] += bb->average_avel_buffer[i][1]; |
|
108 average_lvel[2] += bb->average_lvel_buffer[i][2]; |
|
109 average_avel[2] += bb->average_avel_buffer[i][2]; |
|
110 } |
|
111 |
|
112 // make average |
|
113 dReal r1 = dDIV(REAL( 1.0 ),REAL( bb->adis.average_samples )); |
|
114 |
|
115 average_lvel[0] = dMUL(average_lvel[0],r1); |
|
116 average_avel[0] = dMUL(average_avel[0],r1); |
|
117 average_lvel[1] = dMUL(average_lvel[1],r1); |
|
118 average_avel[1] = dMUL(average_avel[1],r1); |
|
119 average_lvel[2] = dMUL(average_lvel[2],r1); |
|
120 average_avel[2] = dMUL(average_avel[2],r1); |
|
121 } |
|
122 |
|
123 // threshold test |
|
124 dReal av_lspeed, av_aspeed; |
|
125 av_lspeed = dDOT( average_lvel, average_lvel ); |
|
126 if ( av_lspeed > bb->adis.linear_average_threshold ) |
|
127 { |
|
128 idle = 0; // average linear velocity is too high for idle |
|
129 } |
|
130 else |
|
131 { |
|
132 av_aspeed = dDOT( average_avel, average_avel ); |
|
133 if ( av_aspeed > bb->adis.angular_average_threshold ) |
|
134 { |
|
135 idle = 0; // average angular velocity is too high for idle |
|
136 } |
|
137 } |
|
138 } |
|
139 |
|
140 // if it's idle, accumulate steps and time. |
|
141 // these counters won't overflow because this code doesn't run for disabled bodies. |
|
142 if (idle) { |
|
143 bb->adis_stepsleft--; |
|
144 bb->adis_timeleft -= stepsize; |
|
145 } |
|
146 else { |
|
147 // Reset countdowns |
|
148 bb->adis_stepsleft = bb->adis.idle_steps; |
|
149 bb->adis_timeleft = bb->adis.idle_time; |
|
150 } |
|
151 |
|
152 // disable the body if it's idle for a long enough time |
|
153 if ( bb->adis_stepsleft <= 0 && bb->adis_timeleft <= 0 ) |
|
154 { |
|
155 bb->flags |= dxBodyDisabled; // set the disable flag |
|
156 |
|
157 // disabling bodies should also include resetting the velocity |
|
158 // should prevent jittering in big "islands" |
|
159 bb->lvel[0] = 0; |
|
160 bb->lvel[1] = 0; |
|
161 bb->lvel[2] = 0; |
|
162 bb->avel[0] = 0; |
|
163 bb->avel[1] = 0; |
|
164 bb->avel[2] = 0; |
|
165 } |
|
166 } |
|
167 } |
|
168 |
|
169 |
|
170 //**************************************************************************** |
|
171 // body rotation |
|
172 |
|
173 // return sin(x)/x. this has a singularity at 0 so special handling is needed |
|
174 // for small arguments. |
|
175 |
|
176 static inline dReal sinc (dReal x) |
|
177 { |
|
178 // if |x| < 1e-4 then use a taylor series expansion. this two term expansion |
|
179 // is actually accurate to one LS bit within this range if double precision |
|
180 // is being used - so don't worry! |
|
181 if (dFabs(x) < REAL(1.0e-4)) return REAL(1.0) - dMUL(dMUL(x,x),REAL(0.166666666666666666667)); |
|
182 else return dDIV(dSin(x),x); |
|
183 } |
|
184 |
|
185 |
|
186 // given a body b, apply its linear and angular rotation over the time |
|
187 // interval h, thereby adjusting its position and orientation. |
|
188 |
|
189 void dxStepBody (dxBody *b, dReal h) |
|
190 { |
|
191 int j; |
|
192 |
|
193 // handle linear velocity |
|
194 for (j=0; j<3; j++) b->posr.pos[j] += dMUL(h,b->lvel[j]); |
|
195 |
|
196 if (b->flags & dxBodyFlagFiniteRotation) { |
|
197 dVector3 irv; // infitesimal rotation vector |
|
198 dQuaternion q; // quaternion for finite rotation |
|
199 |
|
200 if (b->flags & dxBodyFlagFiniteRotationAxis) { |
|
201 // split the angular velocity vector into a component along the finite |
|
202 // rotation axis, and a component orthogonal to it. |
|
203 dVector3 frv; // finite rotation vector |
|
204 dReal k = dDOT (b->finite_rot_axis,b->avel); |
|
205 frv[0] = dMUL(b->finite_rot_axis[0],k); |
|
206 frv[1] = dMUL(b->finite_rot_axis[1],k); |
|
207 frv[2] = dMUL(b->finite_rot_axis[2],k); |
|
208 irv[0] = b->avel[0] - frv[0]; |
|
209 irv[1] = b->avel[1] - frv[1]; |
|
210 irv[2] = b->avel[2] - frv[2]; |
|
211 |
|
212 // make a rotation quaternion q that corresponds to frv * h. |
|
213 // compare this with the full-finite-rotation case below. |
|
214 h = dMUL(h,REAL(0.5)); |
|
215 dReal theta = dMUL(k,h); |
|
216 q[0] = dCos(theta); |
|
217 dReal s = dMUL(sinc(theta),h); |
|
218 q[1] = dMUL(frv[0],s); |
|
219 q[2] = dMUL(frv[1],s); |
|
220 q[3] = dMUL(frv[2],s); |
|
221 } |
|
222 else { |
|
223 // make a rotation quaternion q that corresponds to w * h |
|
224 dReal wlen = dSqrt (dMUL(b->avel[0],b->avel[0]) + dMUL(b->avel[1],b->avel[1]) + |
|
225 dMUL(b->avel[2],b->avel[2])); |
|
226 h = dMUL(h,REAL(0.5)); |
|
227 dReal theta = dMUL(wlen,h); |
|
228 q[0] = dCos(theta); |
|
229 dReal s = dMUL(sinc(theta),h); |
|
230 q[1] = dMUL(b->avel[0],s); |
|
231 q[2] = dMUL(b->avel[1],s); |
|
232 q[3] = dMUL(b->avel[2],s); |
|
233 } |
|
234 |
|
235 // do the finite rotation |
|
236 dQuaternion q2; |
|
237 dQMultiply0 (q2,q,b->q); |
|
238 for (j=0; j<4; j++) b->q[j] = q2[j]; |
|
239 |
|
240 // do the infitesimal rotation if required |
|
241 if (b->flags & dxBodyFlagFiniteRotationAxis) { |
|
242 dReal dq[4]; |
|
243 dWtoDQ (irv,b->q,dq); |
|
244 for (j=0; j<4; j++) b->q[j] += dMUL(h,dq[j]); |
|
245 } |
|
246 } |
|
247 else { |
|
248 // the normal way - do an infitesimal rotation |
|
249 dReal dq[4]; |
|
250 dWtoDQ (b->avel,b->q,dq); |
|
251 for (j=0; j<4; j++) b->q[j] += dMUL(h,dq[j]); |
|
252 } |
|
253 |
|
254 // normalize the quaternion and convert it to a rotation matrix |
|
255 dNormalize4 (b->q); |
|
256 dQtoR (b->q,b->posr.R); |
|
257 |
|
258 // notify all attached geoms that this body has moved |
|
259 for (dxGeom *geom = b->geom; geom; geom = dGeomGetBodyNext (geom)) |
|
260 dGeomMoved (geom); |
|
261 } |
|
262 |
|
263 //**************************************************************************** |
|
264 // island processing |
|
265 |
|
266 // this groups all joints and bodies in a world into islands. all objects |
|
267 // in an island are reachable by going through connected bodies and joints. |
|
268 // each island can be simulated separately. |
|
269 // note that joints that are not attached to anything will not be included |
|
270 // in any island, an so they do not affect the simulation. |
|
271 // |
|
272 // this function starts new island from unvisited bodies. however, it will |
|
273 // never start a new islands from a disabled body. thus islands of disabled |
|
274 // bodies will not be included in the simulation. disabled bodies are |
|
275 // re-enabled if they are found to be part of an active island. |
|
276 |
|
277 void dxProcessIslands (dxWorld *world, dReal stepsize, dstepper_fn_t stepper) |
|
278 { |
|
279 dxBody *b,*bb,**body; |
|
280 dxJoint *j,**joint; |
|
281 |
|
282 // nothing to do if no bodies |
|
283 if (world->nb <= 0) return; |
|
284 |
|
285 // handle auto-disabling of bodies |
|
286 dInternalHandleAutoDisabling (world,stepsize); |
|
287 |
|
288 // make arrays for body and joint lists (for a single island) to go into |
|
289 body = (dxBody**) malloc (world->nb * sizeof(dxBody*)); |
|
290 if(body == NULL){ |
|
291 return; |
|
292 } |
|
293 joint = (dxJoint**) malloc (world->nj * sizeof(dxJoint*)); |
|
294 if(joint == NULL){ |
|
295 free(body); |
|
296 return; |
|
297 } |
|
298 int bcount = 0; // number of bodies in `body' |
|
299 int jcount = 0; // number of joints in `joint' |
|
300 |
|
301 // set all body/joint tags to 0 |
|
302 for (b=world->firstbody; b; b=(dxBody*)b->next) b->tag = 0; |
|
303 for (j=world->firstjoint; j; j=(dxJoint*)j->next) j->tag = 0; |
|
304 |
|
305 // allocate a stack of unvisited bodies in the island. the maximum size of |
|
306 // the stack can be the lesser of the number of bodies or joints, because |
|
307 // new bodies are only ever added to the stack by going through untagged |
|
308 // joints. all the bodies in the stack must be tagged! |
|
309 int stackalloc = (world->nj < world->nb) ? world->nj : world->nb; |
|
310 dxBody **stack = (dxBody**) malloc (stackalloc * sizeof(dxBody*)); |
|
311 if(stack == NULL){ |
|
312 free(body); |
|
313 free(joint); |
|
314 return; |
|
315 } |
|
316 |
|
317 for (bb=world->firstbody; bb; bb=(dxBody*)bb->next) { |
|
318 // get bb = the next enabled, untagged body, and tag it |
|
319 if (bb->tag || (bb->flags & dxBodyDisabled)) continue; |
|
320 bb->tag = 1; |
|
321 |
|
322 // tag all bodies and joints starting from bb. |
|
323 int stacksize = 0; |
|
324 int firsttime = 1; |
|
325 |
|
326 b = bb; |
|
327 body[0] = bb; |
|
328 bcount = 1; |
|
329 jcount = 0; |
|
330 |
|
331 while (stacksize > 0 || firsttime) |
|
332 { |
|
333 if (!firsttime) |
|
334 { |
|
335 b = stack[--stacksize]; // pop body off stack |
|
336 body[bcount++] = b; // put body on body list |
|
337 } |
|
338 else |
|
339 { |
|
340 firsttime = 0; |
|
341 } |
|
342 |
|
343 // traverse and tag all body's joints, add untagged connected bodies |
|
344 // to stack |
|
345 for (dxJointNode *n=b->firstjoint; n; n=n->next) { |
|
346 if (!n->joint->tag) { |
|
347 n->joint->tag = 1; |
|
348 joint[jcount++] = n->joint; |
|
349 if (n->body && !n->body->tag) { |
|
350 n->body->tag = 1; |
|
351 stack[stacksize++] = n->body; |
|
352 } |
|
353 } |
|
354 } |
|
355 } |
|
356 |
|
357 // now do something with body and joint lists |
|
358 stepper (world,body,bcount,joint,jcount,stepsize); |
|
359 |
|
360 // what we've just done may have altered the body/joint tag values. |
|
361 // we must make sure that these tags are nonzero. |
|
362 // also make sure all bodies are in the enabled state. |
|
363 int i; |
|
364 for (i=0; i<bcount; i++) { |
|
365 body[i]->tag = 1; |
|
366 body[i]->flags &= ~dxBodyDisabled; |
|
367 } |
|
368 for (i=0; i<jcount; i++) joint[i]->tag = 1; |
|
369 } |
|
370 |
|
371 free(body); |
|
372 free(joint); |
|
373 free(stack); |
|
374 |
|
375 } |