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 "objects.h" |
---|
25 | #include "joint.h" |
---|
26 | #include "util.h" |
---|
27 | |
---|
28 | #define ALLOCA dALLOCA16 |
---|
29 | |
---|
30 | //**************************************************************************** |
---|
31 | // Auto disabling |
---|
32 | |
---|
33 | void dInternalHandleAutoDisabling (dxWorld *world, dReal stepsize) |
---|
34 | { |
---|
35 | dxBody *bb; |
---|
36 | for ( bb=world->firstbody; bb; bb=(dxBody*)bb->next ) |
---|
37 | { |
---|
38 | // don't freeze objects mid-air (patch 1586738) |
---|
39 | if ( bb->firstjoint == NULL ) continue; |
---|
40 | |
---|
41 | // nothing to do unless this body is currently enabled and has |
---|
42 | // the auto-disable flag set |
---|
43 | if ( (bb->flags & (dxBodyAutoDisable|dxBodyDisabled)) != dxBodyAutoDisable ) continue; |
---|
44 | |
---|
45 | // if sampling / threshold testing is disabled, we can never sleep. |
---|
46 | if ( bb->adis.average_samples == 0 ) continue; |
---|
47 | |
---|
48 | // |
---|
49 | // see if the body is idle |
---|
50 | // |
---|
51 | |
---|
52 | #ifndef dNODEBUG |
---|
53 | // sanity check |
---|
54 | if ( bb->average_counter >= bb->adis.average_samples ) |
---|
55 | { |
---|
56 | dUASSERT( bb->average_counter < bb->adis.average_samples, "buffer overflow" ); |
---|
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 = dReal( 1.0 ) / dReal( bb->adis.average_samples ); |
---|
114 | |
---|
115 | average_lvel[0] *= r1; |
---|
116 | average_avel[0] *= r1; |
---|
117 | average_lvel[1] *= r1; |
---|
118 | average_avel[1] *= r1; |
---|
119 | average_lvel[2] *= r1; |
---|
120 | 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) < 1.0e-4) return REAL(1.0) - x*x*REAL(0.166666666666666666667); |
---|
182 | else return 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] += 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] = b->finite_rot_axis[0] * k; |
---|
206 | frv[1] = b->finite_rot_axis[1] * k; |
---|
207 | frv[2] = 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 *= REAL(0.5); |
---|
215 | dReal theta = k * h; |
---|
216 | q[0] = dCos(theta); |
---|
217 | dReal s = sinc(theta) * h; |
---|
218 | q[1] = frv[0] * s; |
---|
219 | q[2] = frv[1] * s; |
---|
220 | q[3] = frv[2] * s; |
---|
221 | } |
---|
222 | else { |
---|
223 | // make a rotation quaternion q that corresponds to w * h |
---|
224 | dReal wlen = dSqrt (b->avel[0]*b->avel[0] + b->avel[1]*b->avel[1] + |
---|
225 | b->avel[2]*b->avel[2]); |
---|
226 | h *= REAL(0.5); |
---|
227 | dReal theta = wlen * h; |
---|
228 | q[0] = dCos(theta); |
---|
229 | dReal s = sinc(theta) * h; |
---|
230 | q[1] = b->avel[0] * s; |
---|
231 | q[2] = b->avel[1] * s; |
---|
232 | q[3] = 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] += 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] += 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**) ALLOCA (world->nb * sizeof(dxBody*)); |
---|
290 | joint = (dxJoint**) ALLOCA (world->nj * sizeof(dxJoint*)); |
---|
291 | int bcount = 0; // number of bodies in `body' |
---|
292 | int jcount = 0; // number of joints in `joint' |
---|
293 | |
---|
294 | // set all body/joint tags to 0 |
---|
295 | for (b=world->firstbody; b; b=(dxBody*)b->next) b->tag = 0; |
---|
296 | for (j=world->firstjoint; j; j=(dxJoint*)j->next) j->tag = 0; |
---|
297 | |
---|
298 | // allocate a stack of unvisited bodies in the island. the maximum size of |
---|
299 | // the stack can be the lesser of the number of bodies or joints, because |
---|
300 | // new bodies are only ever added to the stack by going through untagged |
---|
301 | // joints. all the bodies in the stack must be tagged! |
---|
302 | int stackalloc = (world->nj < world->nb) ? world->nj : world->nb; |
---|
303 | dxBody **stack = (dxBody**) ALLOCA (stackalloc * sizeof(dxBody*)); |
---|
304 | |
---|
305 | for (bb=world->firstbody; bb; bb=(dxBody*)bb->next) { |
---|
306 | // get bb = the next enabled, untagged body, and tag it |
---|
307 | if (bb->tag || (bb->flags & dxBodyDisabled)) continue; |
---|
308 | bb->tag = 1; |
---|
309 | |
---|
310 | // tag all bodies and joints starting from bb. |
---|
311 | int stacksize = 0; |
---|
312 | b = bb; |
---|
313 | body[0] = bb; |
---|
314 | bcount = 1; |
---|
315 | jcount = 0; |
---|
316 | goto quickstart; |
---|
317 | while (stacksize > 0) { |
---|
318 | b = stack[--stacksize]; // pop body off stack |
---|
319 | body[bcount++] = b; // put body on body list |
---|
320 | quickstart: |
---|
321 | |
---|
322 | // traverse and tag all body's joints, add untagged connected bodies |
---|
323 | // to stack |
---|
324 | for (dxJointNode *n=b->firstjoint; n; n=n->next) { |
---|
325 | if (!n->joint->tag) { |
---|
326 | n->joint->tag = 1; |
---|
327 | joint[jcount++] = n->joint; |
---|
328 | if (n->body && !n->body->tag) { |
---|
329 | n->body->tag = 1; |
---|
330 | stack[stacksize++] = n->body; |
---|
331 | } |
---|
332 | } |
---|
333 | } |
---|
334 | dIASSERT(stacksize <= world->nb); |
---|
335 | dIASSERT(stacksize <= world->nj); |
---|
336 | } |
---|
337 | |
---|
338 | // now do something with body and joint lists |
---|
339 | stepper (world,body,bcount,joint,jcount,stepsize); |
---|
340 | |
---|
341 | // what we've just done may have altered the body/joint tag values. |
---|
342 | // we must make sure that these tags are nonzero. |
---|
343 | // also make sure all bodies are in the enabled state. |
---|
344 | int i; |
---|
345 | for (i=0; i<bcount; i++) { |
---|
346 | body[i]->tag = 1; |
---|
347 | body[i]->flags &= ~dxBodyDisabled; |
---|
348 | } |
---|
349 | for (i=0; i<jcount; i++) joint[i]->tag = 1; |
---|
350 | } |
---|
351 | |
---|
352 | // if debugging, check that all objects (except for disabled bodies, |
---|
353 | // unconnected joints, and joints that are connected to disabled bodies) |
---|
354 | // were tagged. |
---|
355 | # ifndef dNODEBUG |
---|
356 | for (b=world->firstbody; b; b=(dxBody*)b->next) { |
---|
357 | if (b->flags & dxBodyDisabled) { |
---|
358 | if (b->tag) dDebug (0,"disabled body tagged"); |
---|
359 | } |
---|
360 | else { |
---|
361 | if (!b->tag) dDebug (0,"enabled body not tagged"); |
---|
362 | } |
---|
363 | } |
---|
364 | for (j=world->firstjoint; j; j=(dxJoint*)j->next) { |
---|
365 | if ((j->node[0].body && (j->node[0].body->flags & dxBodyDisabled)==0) || |
---|
366 | (j->node[1].body && (j->node[1].body->flags & dxBodyDisabled)==0)) { |
---|
367 | if (!j->tag) dDebug (0,"attached enabled joint not tagged"); |
---|
368 | } |
---|
369 | else { |
---|
370 | if (j->tag) dDebug (0,"unattached or disabled joint tagged"); |
---|
371 | } |
---|
372 | } |
---|
373 | # endif |
---|
374 | } |
---|