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 | * Fast iterative solver, David Whittaker. Email: david@csworkbench.com * |
---|
7 | * * |
---|
8 | * This library is free software; you can redistribute it and/or * |
---|
9 | * modify it under the terms of EITHER: * |
---|
10 | * (1) The GNU Lesser General Public License as published by the Free * |
---|
11 | * Software Foundation; either version 2.1 of the License, or (at * |
---|
12 | * your option) any later version. The text of the GNU Lesser * |
---|
13 | * General Public License is included with this library in the * |
---|
14 | * file LICENSE.TXT. * |
---|
15 | * (2) The BSD-style license that is included with this library in * |
---|
16 | * the file LICENSE-BSD.TXT. * |
---|
17 | * * |
---|
18 | * This library is distributed in the hope that it will be useful, * |
---|
19 | * but WITHOUT ANY WARRANTY; without even the implied warranty of * |
---|
20 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * |
---|
21 | * LICENSE.TXT and LICENSE-BSD.TXT for more details. * |
---|
22 | * * |
---|
23 | *************************************************************************/ |
---|
24 | |
---|
25 | // This is the StepFast code by David Whittaker. This code is faster, but |
---|
26 | // sometimes less stable than, the original "big matrix" code. |
---|
27 | // Refer to the user's manual for more information. |
---|
28 | // Note that this source file duplicates a lot of stuff from step.cpp, |
---|
29 | // eventually we should move the common code to a third file. |
---|
30 | |
---|
31 | #include "objects.h" |
---|
32 | #include "joint.h" |
---|
33 | #include <ode/config.h> |
---|
34 | #include <ode/objects.h> |
---|
35 | #include <ode/odemath.h> |
---|
36 | #include <ode/rotation.h> |
---|
37 | #include <ode/timer.h> |
---|
38 | #include <ode/error.h> |
---|
39 | #include <ode/matrix.h> |
---|
40 | #include <ode/misc.h> |
---|
41 | #include "lcp.h" |
---|
42 | #include "step.h" |
---|
43 | #include "util.h" |
---|
44 | |
---|
45 | |
---|
46 | // misc defines |
---|
47 | |
---|
48 | #define ALLOCA dALLOCA16 |
---|
49 | |
---|
50 | #define RANDOM_JOINT_ORDER |
---|
51 | //#define FAST_FACTOR //use a factorization approximation to the LCP solver (fast, theoretically less accurate) |
---|
52 | #define SLOW_LCP //use the old LCP solver |
---|
53 | //#define NO_ISLANDS //does not perform island creation code (3~4% of simulation time), body disabling doesn't work |
---|
54 | //#define TIMING |
---|
55 | |
---|
56 | |
---|
57 | static int autoEnableDepth = 2; |
---|
58 | |
---|
59 | void dWorldSetAutoEnableDepthSF1 (dxWorld *world, int autodepth) |
---|
60 | { |
---|
61 | if (autodepth > 0) |
---|
62 | autoEnableDepth = autodepth; |
---|
63 | else |
---|
64 | autoEnableDepth = 0; |
---|
65 | } |
---|
66 | |
---|
67 | int dWorldGetAutoEnableDepthSF1 (dxWorld *world) |
---|
68 | { |
---|
69 | return autoEnableDepth; |
---|
70 | } |
---|
71 | |
---|
72 | //little bit of math.... the _sym_ functions assume the return matrix will be symmetric |
---|
73 | static void |
---|
74 | Multiply2_sym_p8p (dReal * A, dReal * B, dReal * C, int p, int Askip) |
---|
75 | { |
---|
76 | int i, j; |
---|
77 | dReal sum, *aa, *ad, *bb, *cc; |
---|
78 | dIASSERT (p > 0 && A && B && C); |
---|
79 | bb = B; |
---|
80 | for (i = 0; i < p; i++) |
---|
81 | { |
---|
82 | //aa is going accross the matrix, ad down |
---|
83 | aa = ad = A; |
---|
84 | cc = C; |
---|
85 | for (j = i; j < p; j++) |
---|
86 | { |
---|
87 | sum = bb[0] * cc[0]; |
---|
88 | sum += bb[1] * cc[1]; |
---|
89 | sum += bb[2] * cc[2]; |
---|
90 | sum += bb[4] * cc[4]; |
---|
91 | sum += bb[5] * cc[5]; |
---|
92 | sum += bb[6] * cc[6]; |
---|
93 | *(aa++) = *ad = sum; |
---|
94 | ad += Askip; |
---|
95 | cc += 8; |
---|
96 | } |
---|
97 | bb += 8; |
---|
98 | A += Askip + 1; |
---|
99 | C += 8; |
---|
100 | } |
---|
101 | } |
---|
102 | |
---|
103 | static void |
---|
104 | MultiplyAdd2_sym_p8p (dReal * A, dReal * B, dReal * C, int p, int Askip) |
---|
105 | { |
---|
106 | int i, j; |
---|
107 | dReal sum, *aa, *ad, *bb, *cc; |
---|
108 | dIASSERT (p > 0 && A && B && C); |
---|
109 | bb = B; |
---|
110 | for (i = 0; i < p; i++) |
---|
111 | { |
---|
112 | //aa is going accross the matrix, ad down |
---|
113 | aa = ad = A; |
---|
114 | cc = C; |
---|
115 | for (j = i; j < p; j++) |
---|
116 | { |
---|
117 | sum = bb[0] * cc[0]; |
---|
118 | sum += bb[1] * cc[1]; |
---|
119 | sum += bb[2] * cc[2]; |
---|
120 | sum += bb[4] * cc[4]; |
---|
121 | sum += bb[5] * cc[5]; |
---|
122 | sum += bb[6] * cc[6]; |
---|
123 | *(aa++) += sum; |
---|
124 | *ad += sum; |
---|
125 | ad += Askip; |
---|
126 | cc += 8; |
---|
127 | } |
---|
128 | bb += 8; |
---|
129 | A += Askip + 1; |
---|
130 | C += 8; |
---|
131 | } |
---|
132 | } |
---|
133 | |
---|
134 | |
---|
135 | // this assumes the 4th and 8th rows of B are zero. |
---|
136 | |
---|
137 | static void |
---|
138 | Multiply0_p81 (dReal * A, dReal * B, dReal * C, int p) |
---|
139 | { |
---|
140 | int i; |
---|
141 | dIASSERT (p > 0 && A && B && C); |
---|
142 | dReal sum; |
---|
143 | for (i = p; i; i--) |
---|
144 | { |
---|
145 | sum = B[0] * C[0]; |
---|
146 | sum += B[1] * C[1]; |
---|
147 | sum += B[2] * C[2]; |
---|
148 | sum += B[4] * C[4]; |
---|
149 | sum += B[5] * C[5]; |
---|
150 | sum += B[6] * C[6]; |
---|
151 | *(A++) = sum; |
---|
152 | B += 8; |
---|
153 | } |
---|
154 | } |
---|
155 | |
---|
156 | |
---|
157 | // this assumes the 4th and 8th rows of B are zero. |
---|
158 | |
---|
159 | static void |
---|
160 | MultiplyAdd0_p81 (dReal * A, dReal * B, dReal * C, int p) |
---|
161 | { |
---|
162 | int i; |
---|
163 | dIASSERT (p > 0 && A && B && C); |
---|
164 | dReal sum; |
---|
165 | for (i = p; i; i--) |
---|
166 | { |
---|
167 | sum = B[0] * C[0]; |
---|
168 | sum += B[1] * C[1]; |
---|
169 | sum += B[2] * C[2]; |
---|
170 | sum += B[4] * C[4]; |
---|
171 | sum += B[5] * C[5]; |
---|
172 | sum += B[6] * C[6]; |
---|
173 | *(A++) += sum; |
---|
174 | B += 8; |
---|
175 | } |
---|
176 | } |
---|
177 | |
---|
178 | |
---|
179 | // this assumes the 4th and 8th rows of B are zero. |
---|
180 | |
---|
181 | static void |
---|
182 | Multiply1_8q1 (dReal * A, dReal * B, dReal * C, int q) |
---|
183 | { |
---|
184 | int k; |
---|
185 | dReal sum; |
---|
186 | dIASSERT (q > 0 && A && B && C); |
---|
187 | sum = 0; |
---|
188 | for (k = 0; k < q; k++) |
---|
189 | sum += B[k * 8] * C[k]; |
---|
190 | A[0] = sum; |
---|
191 | sum = 0; |
---|
192 | for (k = 0; k < q; k++) |
---|
193 | sum += B[1 + k * 8] * C[k]; |
---|
194 | A[1] = sum; |
---|
195 | sum = 0; |
---|
196 | for (k = 0; k < q; k++) |
---|
197 | sum += B[2 + k * 8] * C[k]; |
---|
198 | A[2] = sum; |
---|
199 | sum = 0; |
---|
200 | for (k = 0; k < q; k++) |
---|
201 | sum += B[4 + k * 8] * C[k]; |
---|
202 | A[4] = sum; |
---|
203 | sum = 0; |
---|
204 | for (k = 0; k < q; k++) |
---|
205 | sum += B[5 + k * 8] * C[k]; |
---|
206 | A[5] = sum; |
---|
207 | sum = 0; |
---|
208 | for (k = 0; k < q; k++) |
---|
209 | sum += B[6 + k * 8] * C[k]; |
---|
210 | A[6] = sum; |
---|
211 | } |
---|
212 | |
---|
213 | //**************************************************************************** |
---|
214 | // body rotation |
---|
215 | |
---|
216 | // return sin(x)/x. this has a singularity at 0 so special handling is needed |
---|
217 | // for small arguments. |
---|
218 | |
---|
219 | static inline dReal |
---|
220 | sinc (dReal x) |
---|
221 | { |
---|
222 | // if |x| < 1e-4 then use a taylor series expansion. this two term expansion |
---|
223 | // is actually accurate to one LS bit within this range if double precision |
---|
224 | // is being used - so don't worry! |
---|
225 | if (dFabs (x) < 1.0e-4) |
---|
226 | return REAL (1.0) - x * x * REAL (0.166666666666666666667); |
---|
227 | else |
---|
228 | return dSin (x) / x; |
---|
229 | } |
---|
230 | |
---|
231 | |
---|
232 | // given a body b, apply its linear and angular rotation over the time |
---|
233 | // interval h, thereby adjusting its position and orientation. |
---|
234 | |
---|
235 | static inline void |
---|
236 | moveAndRotateBody (dxBody * b, dReal h) |
---|
237 | { |
---|
238 | int j; |
---|
239 | |
---|
240 | // handle linear velocity |
---|
241 | for (j = 0; j < 3; j++) |
---|
242 | b->posr.pos[j] += h * b->lvel[j]; |
---|
243 | |
---|
244 | if (b->flags & dxBodyFlagFiniteRotation) |
---|
245 | { |
---|
246 | dVector3 irv; // infitesimal rotation vector |
---|
247 | dQuaternion q; // quaternion for finite rotation |
---|
248 | |
---|
249 | if (b->flags & dxBodyFlagFiniteRotationAxis) |
---|
250 | { |
---|
251 | // split the angular velocity vector into a component along the finite |
---|
252 | // rotation axis, and a component orthogonal to it. |
---|
253 | dVector3 frv, irv; // finite rotation vector |
---|
254 | dReal k = dDOT (b->finite_rot_axis, b->avel); |
---|
255 | frv[0] = b->finite_rot_axis[0] * k; |
---|
256 | frv[1] = b->finite_rot_axis[1] * k; |
---|
257 | frv[2] = b->finite_rot_axis[2] * k; |
---|
258 | irv[0] = b->avel[0] - frv[0]; |
---|
259 | irv[1] = b->avel[1] - frv[1]; |
---|
260 | irv[2] = b->avel[2] - frv[2]; |
---|
261 | |
---|
262 | // make a rotation quaternion q that corresponds to frv * h. |
---|
263 | // compare this with the full-finite-rotation case below. |
---|
264 | h *= REAL (0.5); |
---|
265 | dReal theta = k * h; |
---|
266 | q[0] = dCos (theta); |
---|
267 | dReal s = sinc (theta) * h; |
---|
268 | q[1] = frv[0] * s; |
---|
269 | q[2] = frv[1] * s; |
---|
270 | q[3] = frv[2] * s; |
---|
271 | } |
---|
272 | else |
---|
273 | { |
---|
274 | // make a rotation quaternion q that corresponds to w * h |
---|
275 | dReal wlen = dSqrt (b->avel[0] * b->avel[0] + b->avel[1] * b->avel[1] + b->avel[2] * b->avel[2]); |
---|
276 | h *= REAL (0.5); |
---|
277 | dReal theta = wlen * h; |
---|
278 | q[0] = dCos (theta); |
---|
279 | dReal s = sinc (theta) * h; |
---|
280 | q[1] = b->avel[0] * s; |
---|
281 | q[2] = b->avel[1] * s; |
---|
282 | q[3] = b->avel[2] * s; |
---|
283 | } |
---|
284 | |
---|
285 | // do the finite rotation |
---|
286 | dQuaternion q2; |
---|
287 | dQMultiply0 (q2, q, b->q); |
---|
288 | for (j = 0; j < 4; j++) |
---|
289 | b->q[j] = q2[j]; |
---|
290 | |
---|
291 | // do the infitesimal rotation if required |
---|
292 | if (b->flags & dxBodyFlagFiniteRotationAxis) |
---|
293 | { |
---|
294 | dReal dq[4]; |
---|
295 | dWtoDQ (irv, b->q, dq); |
---|
296 | for (j = 0; j < 4; j++) |
---|
297 | b->q[j] += h * dq[j]; |
---|
298 | } |
---|
299 | } |
---|
300 | else |
---|
301 | { |
---|
302 | // the normal way - do an infitesimal rotation |
---|
303 | dReal dq[4]; |
---|
304 | dWtoDQ (b->avel, b->q, dq); |
---|
305 | for (j = 0; j < 4; j++) |
---|
306 | b->q[j] += h * dq[j]; |
---|
307 | } |
---|
308 | |
---|
309 | // normalize the quaternion and convert it to a rotation matrix |
---|
310 | dNormalize4 (b->q); |
---|
311 | dQtoR (b->q, b->posr.R); |
---|
312 | |
---|
313 | // notify all attached geoms that this body has moved |
---|
314 | for (dxGeom * geom = b->geom; geom; geom = dGeomGetBodyNext (geom)) |
---|
315 | dGeomMoved (geom); |
---|
316 | } |
---|
317 | |
---|
318 | //**************************************************************************** |
---|
319 | //This is an implementation of the iterated/relaxation algorithm. |
---|
320 | //Here is a quick overview of the algorithm per Sergi Valverde's posts to the |
---|
321 | //mailing list: |
---|
322 | // |
---|
323 | // for i=0..N-1 do |
---|
324 | // for c = 0..C-1 do |
---|
325 | // Solve constraint c-th |
---|
326 | // Apply forces to constraint bodies |
---|
327 | // next |
---|
328 | // next |
---|
329 | // Integrate bodies |
---|
330 | |
---|
331 | void |
---|
332 | dInternalStepFast (dxWorld * world, dxBody * body[2], dReal * GI[2], dReal * GinvI[2], dxJoint * joint, dxJoint::Info1 info, dxJoint::Info2 Jinfo, dReal stepsize) |
---|
333 | { |
---|
334 | int i, j, k; |
---|
335 | # ifdef TIMING |
---|
336 | dTimerNow ("constraint preprocessing"); |
---|
337 | # endif |
---|
338 | |
---|
339 | dReal stepsize1 = dRecip (stepsize); |
---|
340 | |
---|
341 | int m = info.m; |
---|
342 | // nothing to do if no constraints. |
---|
343 | if (m <= 0) |
---|
344 | return; |
---|
345 | |
---|
346 | int nub = 0; |
---|
347 | if (info.nub == info.m) |
---|
348 | nub = m; |
---|
349 | |
---|
350 | // compute A = J*invM*J'. first compute JinvM = J*invM. this has the same |
---|
351 | // format as J so we just go through the constraints in J multiplying by |
---|
352 | // the appropriate scalars and matrices. |
---|
353 | # ifdef TIMING |
---|
354 | dTimerNow ("compute A"); |
---|
355 | # endif |
---|
356 | dReal JinvM[2 * 6 * 8]; |
---|
357 | //dSetZero (JinvM, 2 * m * 8); |
---|
358 | |
---|
359 | dReal *Jsrc = Jinfo.J1l; |
---|
360 | dReal *Jdst = JinvM; |
---|
361 | if (body[0]) |
---|
362 | { |
---|
363 | for (j = m - 1; j >= 0; j--) |
---|
364 | { |
---|
365 | for (k = 0; k < 3; k++) |
---|
366 | Jdst[k] = Jsrc[k] * body[0]->invMass; |
---|
367 | dMULTIPLY0_133 (Jdst + 4, Jsrc + 4, GinvI[0]); |
---|
368 | Jsrc += 8; |
---|
369 | Jdst += 8; |
---|
370 | } |
---|
371 | } |
---|
372 | if (body[1]) |
---|
373 | { |
---|
374 | Jsrc = Jinfo.J2l; |
---|
375 | Jdst = JinvM + 8 * m; |
---|
376 | for (j = m - 1; j >= 0; j--) |
---|
377 | { |
---|
378 | for (k = 0; k < 3; k++) |
---|
379 | Jdst[k] = Jsrc[k] * body[1]->invMass; |
---|
380 | dMULTIPLY0_133 (Jdst + 4, Jsrc + 4, GinvI[1]); |
---|
381 | Jsrc += 8; |
---|
382 | Jdst += 8; |
---|
383 | } |
---|
384 | } |
---|
385 | |
---|
386 | |
---|
387 | // now compute A = JinvM * J'. |
---|
388 | int mskip = dPAD (m); |
---|
389 | dReal A[6 * 8]; |
---|
390 | //dSetZero (A, 6 * 8); |
---|
391 | |
---|
392 | if (body[0]) { |
---|
393 | Multiply2_sym_p8p (A, JinvM, Jinfo.J1l, m, mskip); |
---|
394 | if (body[1]) |
---|
395 | MultiplyAdd2_sym_p8p (A, JinvM + 8 * m, Jinfo.J2l, |
---|
396 | m, mskip); |
---|
397 | } else { |
---|
398 | if (body[1]) |
---|
399 | Multiply2_sym_p8p (A, JinvM + 8 * m, Jinfo.J2l, |
---|
400 | m, mskip); |
---|
401 | } |
---|
402 | |
---|
403 | // add cfm to the diagonal of A |
---|
404 | for (i = 0; i < m; i++) |
---|
405 | A[i * mskip + i] += Jinfo.cfm[i] * stepsize1; |
---|
406 | |
---|
407 | // compute the right hand side `rhs' |
---|
408 | # ifdef TIMING |
---|
409 | dTimerNow ("compute rhs"); |
---|
410 | # endif |
---|
411 | dReal tmp1[16]; |
---|
412 | //dSetZero (tmp1, 16); |
---|
413 | // put v/h + invM*fe into tmp1 |
---|
414 | for (i = 0; i < 2; i++) |
---|
415 | { |
---|
416 | if (!body[i]) |
---|
417 | continue; |
---|
418 | for (j = 0; j < 3; j++) |
---|
419 | tmp1[i * 8 + j] = body[i]->facc[j] * body[i]->invMass + body[i]->lvel[j] * stepsize1; |
---|
420 | dMULTIPLY0_331 (tmp1 + i * 8 + 4, GinvI[i], body[i]->tacc); |
---|
421 | for (j = 0; j < 3; j++) |
---|
422 | tmp1[i * 8 + 4 + j] += body[i]->avel[j] * stepsize1; |
---|
423 | } |
---|
424 | // put J*tmp1 into rhs |
---|
425 | dReal rhs[6]; |
---|
426 | //dSetZero (rhs, 6); |
---|
427 | |
---|
428 | if (body[0]) { |
---|
429 | Multiply0_p81 (rhs, Jinfo.J1l, tmp1, m); |
---|
430 | if (body[1]) |
---|
431 | MultiplyAdd0_p81 (rhs, Jinfo.J2l, tmp1 + 8, m); |
---|
432 | } else { |
---|
433 | if (body[1]) |
---|
434 | Multiply0_p81 (rhs, Jinfo.J2l, tmp1 + 8, m); |
---|
435 | } |
---|
436 | |
---|
437 | // complete rhs |
---|
438 | for (i = 0; i < m; i++) |
---|
439 | rhs[i] = Jinfo.c[i] * stepsize1 - rhs[i]; |
---|
440 | |
---|
441 | #ifdef SLOW_LCP |
---|
442 | // solve the LCP problem and get lambda. |
---|
443 | // this will destroy A but that's okay |
---|
444 | # ifdef TIMING |
---|
445 | dTimerNow ("solving LCP problem"); |
---|
446 | # endif |
---|
447 | dReal *lambda = (dReal *) ALLOCA (m * sizeof (dReal)); |
---|
448 | dReal *residual = (dReal *) ALLOCA (m * sizeof (dReal)); |
---|
449 | dReal lo[6], hi[6]; |
---|
450 | memcpy (lo, Jinfo.lo, m * sizeof (dReal)); |
---|
451 | memcpy (hi, Jinfo.hi, m * sizeof (dReal)); |
---|
452 | dSolveLCP (m, A, lambda, rhs, residual, nub, lo, hi, Jinfo.findex); |
---|
453 | #endif |
---|
454 | |
---|
455 | // LCP Solver replacement: |
---|
456 | // This algorithm goes like this: |
---|
457 | // Do a straightforward LDLT factorization of the matrix A, solving for |
---|
458 | // A*x = rhs |
---|
459 | // For each x[i] that is outside of the bounds of lo[i] and hi[i], |
---|
460 | // clamp x[i] into that range. |
---|
461 | // Substitute into A the now known x's |
---|
462 | // subtract the residual away from the rhs. |
---|
463 | // Remove row and column i from L, updating the factorization |
---|
464 | // place the known x's at the end of the array, keeping up with location in p |
---|
465 | // Repeat until all constraints have been clamped or all are within bounds |
---|
466 | // |
---|
467 | // This is probably only faster in the single joint case where only one repeat is |
---|
468 | // the norm. |
---|
469 | |
---|
470 | #ifdef FAST_FACTOR |
---|
471 | // factorize A (L*D*L'=A) |
---|
472 | # ifdef TIMING |
---|
473 | dTimerNow ("factorize A"); |
---|
474 | # endif |
---|
475 | dReal d[6]; |
---|
476 | dReal L[6 * 8]; |
---|
477 | memcpy (L, A, m * mskip * sizeof (dReal)); |
---|
478 | dFactorLDLT (L, d, m, mskip); |
---|
479 | |
---|
480 | // compute lambda |
---|
481 | # ifdef TIMING |
---|
482 | dTimerNow ("compute lambda"); |
---|
483 | # endif |
---|
484 | |
---|
485 | int left = m; //constraints left to solve. |
---|
486 | int remove[6]; |
---|
487 | dReal lambda[6]; |
---|
488 | dReal x[6]; |
---|
489 | int p[6]; |
---|
490 | for (i = 0; i < 6; i++) |
---|
491 | p[i] = i; |
---|
492 | while (true) |
---|
493 | { |
---|
494 | memcpy (x, rhs, left * sizeof (dReal)); |
---|
495 | dSolveLDLT (L, d, x, left, mskip); |
---|
496 | |
---|
497 | int fixed = 0; |
---|
498 | for (i = 0; i < left; i++) |
---|
499 | { |
---|
500 | j = p[i]; |
---|
501 | remove[i] = false; |
---|
502 | // This isn't the exact same use of findex as dSolveLCP.... since x[findex] |
---|
503 | // may change after I've already clamped x[i], but it should be close |
---|
504 | if (Jinfo.findex[j] > -1) |
---|
505 | { |
---|
506 | dReal f = fabs (Jinfo.hi[j] * x[p[Jinfo.findex[j]]]); |
---|
507 | if (x[i] > f) |
---|
508 | x[i] = f; |
---|
509 | else if (x[i] < -f) |
---|
510 | x[i] = -f; |
---|
511 | else |
---|
512 | continue; |
---|
513 | } |
---|
514 | else |
---|
515 | { |
---|
516 | if (x[i] > Jinfo.hi[j]) |
---|
517 | x[i] = Jinfo.hi[j]; |
---|
518 | else if (x[i] < Jinfo.lo[j]) |
---|
519 | x[i] = Jinfo.lo[j]; |
---|
520 | else |
---|
521 | continue; |
---|
522 | } |
---|
523 | remove[i] = true; |
---|
524 | fixed++; |
---|
525 | } |
---|
526 | if (fixed == 0 || fixed == left) //no change or all constraints solved |
---|
527 | break; |
---|
528 | |
---|
529 | for (i = 0; i < left; i++) //sub in to right hand side. |
---|
530 | if (remove[i]) |
---|
531 | for (j = 0; j < left; j++) |
---|
532 | if (!remove[j]) |
---|
533 | rhs[j] -= A[j * mskip + i] * x[i]; |
---|
534 | |
---|
535 | for (int r = left - 1; r >= 0; r--) //eliminate row/col for fixed variables |
---|
536 | { |
---|
537 | if (remove[r]) |
---|
538 | { |
---|
539 | //dRemoveLDLT adapted for use without row pointers. |
---|
540 | if (r == left - 1) |
---|
541 | { |
---|
542 | left--; |
---|
543 | continue; // deleting last row/col is easy |
---|
544 | } |
---|
545 | else if (r == 0) |
---|
546 | { |
---|
547 | dReal a[6]; |
---|
548 | for (i = 0; i < left; i++) |
---|
549 | a[i] = -A[i * mskip]; |
---|
550 | a[0] += REAL (1.0); |
---|
551 | dLDLTAddTL (L, d, a, left, mskip); |
---|
552 | } |
---|
553 | else |
---|
554 | { |
---|
555 | dReal t[6]; |
---|
556 | dReal a[6]; |
---|
557 | for (i = 0; i < r; i++) |
---|
558 | t[i] = L[r * mskip + i] / d[i]; |
---|
559 | for (i = 0; i < left - r; i++) |
---|
560 | a[i] = dDot (L + (r + i) * mskip, t, r) - A[(r + i) * mskip + r]; |
---|
561 | a[0] += REAL (1.0); |
---|
562 | dLDLTAddTL (L + r * mskip + r, d + r, a, left - r, mskip); |
---|
563 | } |
---|
564 | |
---|
565 | dRemoveRowCol (L, left, mskip, r); |
---|
566 | //end dRemoveLDLT |
---|
567 | |
---|
568 | left--; |
---|
569 | if (r < (left - 1)) |
---|
570 | { |
---|
571 | dReal tx = x[r]; |
---|
572 | memmove (d + r, d + r + 1, (left - r) * sizeof (dReal)); |
---|
573 | memmove (rhs + r, rhs + r + 1, (left - r) * sizeof (dReal)); |
---|
574 | //x will get written over by rhs anyway, no need to move it around |
---|
575 | //just store the fixed value we just discovered in it. |
---|
576 | x[left] = tx; |
---|
577 | for (i = 0; i < m; i++) |
---|
578 | if (p[i] > r && p[i] <= left) |
---|
579 | p[i]--; |
---|
580 | p[r] = left; |
---|
581 | } |
---|
582 | } |
---|
583 | } |
---|
584 | } |
---|
585 | |
---|
586 | for (i = 0; i < m; i++) |
---|
587 | lambda[i] = x[p[i]]; |
---|
588 | # endif |
---|
589 | // compute the constraint force `cforce' |
---|
590 | # ifdef TIMING |
---|
591 | dTimerNow ("compute constraint force"); |
---|
592 | #endif |
---|
593 | |
---|
594 | // compute cforce = J'*lambda |
---|
595 | dJointFeedback *fb = joint->feedback; |
---|
596 | dReal cforce[16]; |
---|
597 | //dSetZero (cforce, 16); |
---|
598 | |
---|
599 | if (fb) |
---|
600 | { |
---|
601 | // the user has requested feedback on the amount of force that this |
---|
602 | // joint is applying to the bodies. we use a slightly slower |
---|
603 | // computation that splits out the force components and puts them |
---|
604 | // in the feedback structure. |
---|
605 | dReal data1[8], data2[8]; |
---|
606 | if (body[0]) |
---|
607 | { |
---|
608 | Multiply1_8q1 (data1, Jinfo.J1l, lambda, m); |
---|
609 | dReal *cf1 = cforce; |
---|
610 | cf1[0] = (fb->f1[0] = data1[0]); |
---|
611 | cf1[1] = (fb->f1[1] = data1[1]); |
---|
612 | cf1[2] = (fb->f1[2] = data1[2]); |
---|
613 | cf1[4] = (fb->t1[0] = data1[4]); |
---|
614 | cf1[5] = (fb->t1[1] = data1[5]); |
---|
615 | cf1[6] = (fb->t1[2] = data1[6]); |
---|
616 | } |
---|
617 | if (body[1]) |
---|
618 | { |
---|
619 | Multiply1_8q1 (data2, Jinfo.J2l, lambda, m); |
---|
620 | dReal *cf2 = cforce + 8; |
---|
621 | cf2[0] = (fb->f2[0] = data2[0]); |
---|
622 | cf2[1] = (fb->f2[1] = data2[1]); |
---|
623 | cf2[2] = (fb->f2[2] = data2[2]); |
---|
624 | cf2[4] = (fb->t2[0] = data2[4]); |
---|
625 | cf2[5] = (fb->t2[1] = data2[5]); |
---|
626 | cf2[6] = (fb->t2[2] = data2[6]); |
---|
627 | } |
---|
628 | } |
---|
629 | else |
---|
630 | { |
---|
631 | // no feedback is required, let's compute cforce the faster way |
---|
632 | if (body[0]) |
---|
633 | Multiply1_8q1 (cforce, Jinfo.J1l, lambda, m); |
---|
634 | if (body[1]) |
---|
635 | Multiply1_8q1 (cforce + 8, Jinfo.J2l, lambda, m); |
---|
636 | } |
---|
637 | |
---|
638 | for (i = 0; i < 2; i++) |
---|
639 | { |
---|
640 | if (!body[i]) |
---|
641 | continue; |
---|
642 | for (j = 0; j < 3; j++) |
---|
643 | { |
---|
644 | body[i]->facc[j] += cforce[i * 8 + j]; |
---|
645 | body[i]->tacc[j] += cforce[i * 8 + 4 + j]; |
---|
646 | } |
---|
647 | } |
---|
648 | } |
---|
649 | |
---|
650 | void |
---|
651 | dInternalStepIslandFast (dxWorld * world, dxBody * const *bodies, int nb, dxJoint * const *_joints, int nj, dReal stepsize, int maxiterations) |
---|
652 | { |
---|
653 | # ifdef TIMING |
---|
654 | dTimerNow ("preprocessing"); |
---|
655 | # endif |
---|
656 | dxBody *bodyPair[2], *body; |
---|
657 | dReal *GIPair[2], *GinvIPair[2]; |
---|
658 | dxJoint *joint; |
---|
659 | int iter, b, j, i; |
---|
660 | dReal ministep = stepsize / maxiterations; |
---|
661 | |
---|
662 | // make a local copy of the joint array, because we might want to modify it. |
---|
663 | // (the "dxJoint *const*" declaration says we're allowed to modify the joints |
---|
664 | // but not the joint array, because the caller might need it unchanged). |
---|
665 | dxJoint **joints = (dxJoint **) ALLOCA (nj * sizeof (dxJoint *)); |
---|
666 | memcpy (joints, _joints, nj * sizeof (dxJoint *)); |
---|
667 | |
---|
668 | // get m = total constraint dimension, nub = number of unbounded variables. |
---|
669 | // create constraint offset array and number-of-rows array for all joints. |
---|
670 | // the constraints are re-ordered as follows: the purely unbounded |
---|
671 | // constraints, the mixed unbounded + LCP constraints, and last the purely |
---|
672 | // LCP constraints. this assists the LCP solver to put all unbounded |
---|
673 | // variables at the start for a quick factorization. |
---|
674 | // |
---|
675 | // joints with m=0 are inactive and are removed from the joints array |
---|
676 | // entirely, so that the code that follows does not consider them. |
---|
677 | // also number all active joints in the joint list (set their tag values). |
---|
678 | // inactive joints receive a tag value of -1. |
---|
679 | |
---|
680 | int m = 0; |
---|
681 | dxJoint::Info1 * info = (dxJoint::Info1 *) ALLOCA (nj * sizeof (dxJoint::Info1)); |
---|
682 | int *ofs = (int *) ALLOCA (nj * sizeof (int)); |
---|
683 | for (i = 0, j = 0; j < nj; j++) |
---|
684 | { // i=dest, j=src |
---|
685 | joints[j]->vtable->getInfo1 (joints[j], info + i); |
---|
686 | dIASSERT (info[i].m >= 0 && info[i].m <= 6 && info[i].nub >= 0 && info[i].nub <= info[i].m); |
---|
687 | if (info[i].m > 0) |
---|
688 | { |
---|
689 | joints[i] = joints[j]; |
---|
690 | joints[i]->tag = i; |
---|
691 | i++; |
---|
692 | } |
---|
693 | else |
---|
694 | { |
---|
695 | joints[j]->tag = -1; |
---|
696 | } |
---|
697 | } |
---|
698 | nj = i; |
---|
699 | |
---|
700 | // the purely unbounded constraints |
---|
701 | for (i = 0; i < nj; i++) |
---|
702 | { |
---|
703 | ofs[i] = m; |
---|
704 | m += info[i].m; |
---|
705 | } |
---|
706 | dReal *c = NULL; |
---|
707 | dReal *cfm = NULL; |
---|
708 | dReal *lo = NULL; |
---|
709 | dReal *hi = NULL; |
---|
710 | int *findex = NULL; |
---|
711 | |
---|
712 | dReal *J = NULL; |
---|
713 | dxJoint::Info2 * Jinfo = NULL; |
---|
714 | |
---|
715 | if (m) |
---|
716 | { |
---|
717 | // create a constraint equation right hand side vector `c', a constraint |
---|
718 | // force mixing vector `cfm', and LCP low and high bound vectors, and an |
---|
719 | // 'findex' vector. |
---|
720 | c = (dReal *) ALLOCA (m * sizeof (dReal)); |
---|
721 | cfm = (dReal *) ALLOCA (m * sizeof (dReal)); |
---|
722 | lo = (dReal *) ALLOCA (m * sizeof (dReal)); |
---|
723 | hi = (dReal *) ALLOCA (m * sizeof (dReal)); |
---|
724 | findex = (int *) ALLOCA (m * sizeof (int)); |
---|
725 | dSetZero (c, m); |
---|
726 | dSetValue (cfm, m, world->global_cfm); |
---|
727 | dSetValue (lo, m, -dInfinity); |
---|
728 | dSetValue (hi, m, dInfinity); |
---|
729 | for (i = 0; i < m; i++) |
---|
730 | findex[i] = -1; |
---|
731 | |
---|
732 | // get jacobian data from constraints. a (2*m)x8 matrix will be created |
---|
733 | // to store the two jacobian blocks from each constraint. it has this |
---|
734 | // format: |
---|
735 | // |
---|
736 | // l l l 0 a a a 0 \ . |
---|
737 | // l l l 0 a a a 0 }-- jacobian body 1 block for joint 0 (3 rows) |
---|
738 | // l l l 0 a a a 0 / |
---|
739 | // l l l 0 a a a 0 \ . |
---|
740 | // l l l 0 a a a 0 }-- jacobian body 2 block for joint 0 (3 rows) |
---|
741 | // l l l 0 a a a 0 / |
---|
742 | // l l l 0 a a a 0 }--- jacobian body 1 block for joint 1 (1 row) |
---|
743 | // l l l 0 a a a 0 }--- jacobian body 2 block for joint 1 (1 row) |
---|
744 | // etc... |
---|
745 | // |
---|
746 | // (lll) = linear jacobian data |
---|
747 | // (aaa) = angular jacobian data |
---|
748 | // |
---|
749 | # ifdef TIMING |
---|
750 | dTimerNow ("create J"); |
---|
751 | # endif |
---|
752 | J = (dReal *) ALLOCA (2 * m * 8 * sizeof (dReal)); |
---|
753 | dSetZero (J, 2 * m * 8); |
---|
754 | Jinfo = (dxJoint::Info2 *) ALLOCA (nj * sizeof (dxJoint::Info2)); |
---|
755 | for (i = 0; i < nj; i++) |
---|
756 | { |
---|
757 | Jinfo[i].rowskip = 8; |
---|
758 | Jinfo[i].fps = dRecip (stepsize); |
---|
759 | Jinfo[i].erp = world->global_erp; |
---|
760 | Jinfo[i].J1l = J + 2 * 8 * ofs[i]; |
---|
761 | Jinfo[i].J1a = Jinfo[i].J1l + 4; |
---|
762 | Jinfo[i].J2l = Jinfo[i].J1l + 8 * info[i].m; |
---|
763 | Jinfo[i].J2a = Jinfo[i].J2l + 4; |
---|
764 | Jinfo[i].c = c + ofs[i]; |
---|
765 | Jinfo[i].cfm = cfm + ofs[i]; |
---|
766 | Jinfo[i].lo = lo + ofs[i]; |
---|
767 | Jinfo[i].hi = hi + ofs[i]; |
---|
768 | Jinfo[i].findex = findex + ofs[i]; |
---|
769 | //joints[i]->vtable->getInfo2 (joints[i], Jinfo+i); |
---|
770 | } |
---|
771 | |
---|
772 | } |
---|
773 | |
---|
774 | dReal *saveFacc = (dReal *) ALLOCA (nb * 4 * sizeof (dReal)); |
---|
775 | dReal *saveTacc = (dReal *) ALLOCA (nb * 4 * sizeof (dReal)); |
---|
776 | dReal *globalI = (dReal *) ALLOCA (nb * 12 * sizeof (dReal)); |
---|
777 | dReal *globalInvI = (dReal *) ALLOCA (nb * 12 * sizeof (dReal)); |
---|
778 | for (b = 0; b < nb; b++) |
---|
779 | { |
---|
780 | for (i = 0; i < 4; i++) |
---|
781 | { |
---|
782 | saveFacc[b * 4 + i] = bodies[b]->facc[i]; |
---|
783 | saveTacc[b * 4 + i] = bodies[b]->tacc[i]; |
---|
784 | } |
---|
785 | bodies[b]->tag = b; |
---|
786 | } |
---|
787 | |
---|
788 | for (iter = 0; iter < maxiterations; iter++) |
---|
789 | { |
---|
790 | # ifdef TIMING |
---|
791 | dTimerNow ("applying inertia and gravity"); |
---|
792 | # endif |
---|
793 | dReal tmp[12] = { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 }; |
---|
794 | |
---|
795 | for (b = 0; b < nb; b++) |
---|
796 | { |
---|
797 | body = bodies[b]; |
---|
798 | |
---|
799 | // for all bodies, compute the inertia tensor and its inverse in the global |
---|
800 | // frame, and compute the rotational force and add it to the torque |
---|
801 | // accumulator. I and invI are vertically stacked 3x4 matrices, one per body. |
---|
802 | // @@@ check computation of rotational force. |
---|
803 | |
---|
804 | // compute inertia tensor in global frame |
---|
805 | dMULTIPLY2_333 (tmp, body->mass.I, body->posr.R); |
---|
806 | dMULTIPLY0_333 (globalI + b * 12, body->posr.R, tmp); |
---|
807 | // compute inverse inertia tensor in global frame |
---|
808 | dMULTIPLY2_333 (tmp, body->invI, body->posr.R); |
---|
809 | dMULTIPLY0_333 (globalInvI + b * 12, body->posr.R, tmp); |
---|
810 | |
---|
811 | for (i = 0; i < 4; i++) |
---|
812 | body->tacc[i] = saveTacc[b * 4 + i]; |
---|
813 | #ifdef dGYROSCOPIC |
---|
814 | // compute rotational force |
---|
815 | dMULTIPLY0_331 (tmp, globalI + b * 12, body->avel); |
---|
816 | dCROSS (body->tacc, -=, body->avel, tmp); |
---|
817 | #endif |
---|
818 | |
---|
819 | // add the gravity force to all bodies |
---|
820 | if ((body->flags & dxBodyNoGravity) == 0) |
---|
821 | { |
---|
822 | body->facc[0] = saveFacc[b * 4 + 0] + body->mass.mass * world->gravity[0]; |
---|
823 | body->facc[1] = saveFacc[b * 4 + 1] + body->mass.mass * world->gravity[1]; |
---|
824 | body->facc[2] = saveFacc[b * 4 + 2] + body->mass.mass * world->gravity[2]; |
---|
825 | body->facc[3] = 0; |
---|
826 | } else { |
---|
827 | body->facc[0] = saveFacc[b * 4 + 0]; |
---|
828 | body->facc[1] = saveFacc[b * 4 + 1]; |
---|
829 | body->facc[2] = saveFacc[b * 4 + 2]; |
---|
830 | body->facc[3] = 0; |
---|
831 | } |
---|
832 | |
---|
833 | } |
---|
834 | |
---|
835 | #ifdef RANDOM_JOINT_ORDER |
---|
836 | #ifdef TIMING |
---|
837 | dTimerNow ("randomizing joint order"); |
---|
838 | #endif |
---|
839 | //randomize the order of the joints by looping through the array |
---|
840 | //and swapping the current joint pointer with a random one before it. |
---|
841 | for (j = 0; j < nj; j++) |
---|
842 | { |
---|
843 | joint = joints[j]; |
---|
844 | dxJoint::Info1 i1 = info[j]; |
---|
845 | dxJoint::Info2 i2 = Jinfo[j]; |
---|
846 | const int r = dRandInt(j+1); |
---|
847 | dIASSERT (r < nj); |
---|
848 | joints[j] = joints[r]; |
---|
849 | info[j] = info[r]; |
---|
850 | Jinfo[j] = Jinfo[r]; |
---|
851 | joints[r] = joint; |
---|
852 | info[r] = i1; |
---|
853 | Jinfo[r] = i2; |
---|
854 | } |
---|
855 | #endif |
---|
856 | |
---|
857 | //now iterate through the random ordered joint array we created. |
---|
858 | for (j = 0; j < nj; j++) |
---|
859 | { |
---|
860 | #ifdef TIMING |
---|
861 | dTimerNow ("setting up joint"); |
---|
862 | #endif |
---|
863 | joint = joints[j]; |
---|
864 | bodyPair[0] = joint->node[0].body; |
---|
865 | bodyPair[1] = joint->node[1].body; |
---|
866 | |
---|
867 | if (bodyPair[0] && (bodyPair[0]->flags & dxBodyDisabled)) |
---|
868 | bodyPair[0] = 0; |
---|
869 | if (bodyPair[1] && (bodyPair[1]->flags & dxBodyDisabled)) |
---|
870 | bodyPair[1] = 0; |
---|
871 | |
---|
872 | //if this joint is not connected to any enabled bodies, skip it. |
---|
873 | if (!bodyPair[0] && !bodyPair[1]) |
---|
874 | continue; |
---|
875 | |
---|
876 | if (bodyPair[0]) |
---|
877 | { |
---|
878 | GIPair[0] = globalI + bodyPair[0]->tag * 12; |
---|
879 | GinvIPair[0] = globalInvI + bodyPair[0]->tag * 12; |
---|
880 | } |
---|
881 | if (bodyPair[1]) |
---|
882 | { |
---|
883 | GIPair[1] = globalI + bodyPair[1]->tag * 12; |
---|
884 | GinvIPair[1] = globalInvI + bodyPair[1]->tag * 12; |
---|
885 | } |
---|
886 | |
---|
887 | joints[j]->vtable->getInfo2 (joints[j], Jinfo + j); |
---|
888 | |
---|
889 | //dInternalStepIslandFast is an exact copy of the old routine with one |
---|
890 | //modification: the calculated forces are added back to the facc and tacc |
---|
891 | //vectors instead of applying them to the bodies and moving them. |
---|
892 | if (info[j].m > 0) |
---|
893 | { |
---|
894 | dInternalStepFast (world, bodyPair, GIPair, GinvIPair, joint, info[j], Jinfo[j], ministep); |
---|
895 | } |
---|
896 | } |
---|
897 | // } |
---|
898 | # ifdef TIMING |
---|
899 | dTimerNow ("moving bodies"); |
---|
900 | # endif |
---|
901 | //Now we can simulate all the free floating bodies, and move them. |
---|
902 | for (b = 0; b < nb; b++) |
---|
903 | { |
---|
904 | body = bodies[b]; |
---|
905 | |
---|
906 | for (i = 0; i < 4; i++) |
---|
907 | { |
---|
908 | body->facc[i] *= ministep; |
---|
909 | body->tacc[i] *= ministep; |
---|
910 | } |
---|
911 | |
---|
912 | //apply torque |
---|
913 | dMULTIPLYADD0_331 (body->avel, globalInvI + b * 12, body->tacc); |
---|
914 | |
---|
915 | //apply force |
---|
916 | for (i = 0; i < 3; i++) |
---|
917 | body->lvel[i] += body->invMass * body->facc[i]; |
---|
918 | |
---|
919 | //move It! |
---|
920 | moveAndRotateBody (body, ministep); |
---|
921 | } |
---|
922 | } |
---|
923 | for (b = 0; b < nb; b++) |
---|
924 | for (j = 0; j < 4; j++) |
---|
925 | bodies[b]->facc[j] = bodies[b]->tacc[j] = 0; |
---|
926 | } |
---|
927 | |
---|
928 | |
---|
929 | #ifdef NO_ISLANDS |
---|
930 | |
---|
931 | // Since the iterative algorithm doesn't care about islands of bodies, this is a |
---|
932 | // faster algorithm that just sends it all the joints and bodies in one array. |
---|
933 | // It's downfall is it's inability to handle disabled bodies as well as the old one. |
---|
934 | static void |
---|
935 | processIslandsFast (dxWorld * world, dReal stepsize, int maxiterations) |
---|
936 | { |
---|
937 | // nothing to do if no bodies |
---|
938 | if (world->nb <= 0) |
---|
939 | return; |
---|
940 | |
---|
941 | dInternalHandleAutoDisabling (world,stepsize); |
---|
942 | |
---|
943 | # ifdef TIMING |
---|
944 | dTimerStart ("creating joint and body arrays"); |
---|
945 | # endif |
---|
946 | dxBody **bodies, *body; |
---|
947 | dxJoint **joints, *joint; |
---|
948 | joints = (dxJoint **) ALLOCA (world->nj * sizeof (dxJoint *)); |
---|
949 | bodies = (dxBody **) ALLOCA (world->nb * sizeof (dxBody *)); |
---|
950 | |
---|
951 | int nj = 0; |
---|
952 | for (joint = world->firstjoint; joint; joint = (dxJoint *) joint->next) |
---|
953 | joints[nj++] = joint; |
---|
954 | |
---|
955 | int nb = 0; |
---|
956 | for (body = world->firstbody; body; body = (dxBody *) body->next) |
---|
957 | bodies[nb++] = body; |
---|
958 | |
---|
959 | dInternalStepIslandFast (world, bodies, nb, joints, nj, stepsize, maxiterations); |
---|
960 | # ifdef TIMING |
---|
961 | dTimerEnd (); |
---|
962 | dTimerReport (stdout, 1); |
---|
963 | # endif |
---|
964 | } |
---|
965 | |
---|
966 | #else |
---|
967 | |
---|
968 | //**************************************************************************** |
---|
969 | // island processing |
---|
970 | |
---|
971 | // this groups all joints and bodies in a world into islands. all objects |
---|
972 | // in an island are reachable by going through connected bodies and joints. |
---|
973 | // each island can be simulated separately. |
---|
974 | // note that joints that are not attached to anything will not be included |
---|
975 | // in any island, an so they do not affect the simulation. |
---|
976 | // |
---|
977 | // this function starts new island from unvisited bodies. however, it will |
---|
978 | // never start a new islands from a disabled body. thus islands of disabled |
---|
979 | // bodies will not be included in the simulation. disabled bodies are |
---|
980 | // re-enabled if they are found to be part of an active island. |
---|
981 | |
---|
982 | static void |
---|
983 | processIslandsFast (dxWorld * world, dReal stepsize, int maxiterations) |
---|
984 | { |
---|
985 | #ifdef TIMING |
---|
986 | dTimerStart ("Island Setup"); |
---|
987 | #endif |
---|
988 | dxBody *b, *bb, **body; |
---|
989 | dxJoint *j, **joint; |
---|
990 | |
---|
991 | // nothing to do if no bodies |
---|
992 | if (world->nb <= 0) |
---|
993 | return; |
---|
994 | |
---|
995 | dInternalHandleAutoDisabling (world,stepsize); |
---|
996 | |
---|
997 | // make arrays for body and joint lists (for a single island) to go into |
---|
998 | body = (dxBody **) ALLOCA (world->nb * sizeof (dxBody *)); |
---|
999 | joint = (dxJoint **) ALLOCA (world->nj * sizeof (dxJoint *)); |
---|
1000 | int bcount = 0; // number of bodies in `body' |
---|
1001 | int jcount = 0; // number of joints in `joint' |
---|
1002 | int tbcount = 0; |
---|
1003 | int tjcount = 0; |
---|
1004 | |
---|
1005 | // set all body/joint tags to 0 |
---|
1006 | for (b = world->firstbody; b; b = (dxBody *) b->next) |
---|
1007 | b->tag = 0; |
---|
1008 | for (j = world->firstjoint; j; j = (dxJoint *) j->next) |
---|
1009 | j->tag = 0; |
---|
1010 | |
---|
1011 | // allocate a stack of unvisited bodies in the island. the maximum size of |
---|
1012 | // the stack can be the lesser of the number of bodies or joints, because |
---|
1013 | // new bodies are only ever added to the stack by going through untagged |
---|
1014 | // joints. all the bodies in the stack must be tagged! |
---|
1015 | int stackalloc = (world->nj < world->nb) ? world->nj : world->nb; |
---|
1016 | dxBody **stack = (dxBody **) ALLOCA (stackalloc * sizeof (dxBody *)); |
---|
1017 | int *autostack = (int *) ALLOCA (stackalloc * sizeof (int)); |
---|
1018 | |
---|
1019 | for (bb = world->firstbody; bb; bb = (dxBody *) bb->next) |
---|
1020 | { |
---|
1021 | #ifdef TIMING |
---|
1022 | dTimerNow ("Island Processing"); |
---|
1023 | #endif |
---|
1024 | // get bb = the next enabled, untagged body, and tag it |
---|
1025 | if (bb->tag || (bb->flags & dxBodyDisabled)) |
---|
1026 | continue; |
---|
1027 | bb->tag = 1; |
---|
1028 | |
---|
1029 | // tag all bodies and joints starting from bb. |
---|
1030 | int stacksize = 0; |
---|
1031 | int autoDepth = autoEnableDepth; |
---|
1032 | b = bb; |
---|
1033 | body[0] = bb; |
---|
1034 | bcount = 1; |
---|
1035 | jcount = 0; |
---|
1036 | goto quickstart; |
---|
1037 | while (stacksize > 0) |
---|
1038 | { |
---|
1039 | b = stack[--stacksize]; // pop body off stack |
---|
1040 | autoDepth = autostack[stacksize]; |
---|
1041 | body[bcount++] = b; // put body on body list |
---|
1042 | quickstart: |
---|
1043 | |
---|
1044 | // traverse and tag all body's joints, add untagged connected bodies |
---|
1045 | // to stack |
---|
1046 | for (dxJointNode * n = b->firstjoint; n; n = n->next) |
---|
1047 | { |
---|
1048 | if (!n->joint->tag) |
---|
1049 | { |
---|
1050 | int thisDepth = autoEnableDepth; |
---|
1051 | n->joint->tag = 1; |
---|
1052 | joint[jcount++] = n->joint; |
---|
1053 | if (n->body && !n->body->tag) |
---|
1054 | { |
---|
1055 | if (n->body->flags & dxBodyDisabled) |
---|
1056 | thisDepth = autoDepth - 1; |
---|
1057 | if (thisDepth < 0) |
---|
1058 | continue; |
---|
1059 | n->body->flags &= ~dxBodyDisabled; |
---|
1060 | n->body->tag = 1; |
---|
1061 | autostack[stacksize] = thisDepth; |
---|
1062 | stack[stacksize++] = n->body; |
---|
1063 | } |
---|
1064 | } |
---|
1065 | } |
---|
1066 | dIASSERT (stacksize <= world->nb); |
---|
1067 | dIASSERT (stacksize <= world->nj); |
---|
1068 | } |
---|
1069 | |
---|
1070 | // now do something with body and joint lists |
---|
1071 | dInternalStepIslandFast (world, body, bcount, joint, jcount, stepsize, maxiterations); |
---|
1072 | |
---|
1073 | // what we've just done may have altered the body/joint tag values. |
---|
1074 | // we must make sure that these tags are nonzero. |
---|
1075 | // also make sure all bodies are in the enabled state. |
---|
1076 | int i; |
---|
1077 | for (i = 0; i < bcount; i++) |
---|
1078 | { |
---|
1079 | body[i]->tag = 1; |
---|
1080 | body[i]->flags &= ~dxBodyDisabled; |
---|
1081 | } |
---|
1082 | for (i = 0; i < jcount; i++) |
---|
1083 | joint[i]->tag = 1; |
---|
1084 | |
---|
1085 | tbcount += bcount; |
---|
1086 | tjcount += jcount; |
---|
1087 | } |
---|
1088 | |
---|
1089 | #ifdef TIMING |
---|
1090 | dMessage(0, "Total joints processed: %i, bodies: %i", tjcount, tbcount); |
---|
1091 | #endif |
---|
1092 | |
---|
1093 | // if debugging, check that all objects (except for disabled bodies, |
---|
1094 | // unconnected joints, and joints that are connected to disabled bodies) |
---|
1095 | // were tagged. |
---|
1096 | # ifndef dNODEBUG |
---|
1097 | for (b = world->firstbody; b; b = (dxBody *) b->next) |
---|
1098 | { |
---|
1099 | if (b->flags & dxBodyDisabled) |
---|
1100 | { |
---|
1101 | if (b->tag) |
---|
1102 | dDebug (0, "disabled body tagged"); |
---|
1103 | } |
---|
1104 | else |
---|
1105 | { |
---|
1106 | if (!b->tag) |
---|
1107 | dDebug (0, "enabled body not tagged"); |
---|
1108 | } |
---|
1109 | } |
---|
1110 | for (j = world->firstjoint; j; j = (dxJoint *) j->next) |
---|
1111 | { |
---|
1112 | if ((j->node[0].body && (j->node[0].body->flags & dxBodyDisabled) == 0) || (j->node[1].body && (j->node[1].body->flags & dxBodyDisabled) == 0)) |
---|
1113 | { |
---|
1114 | if (!j->tag) |
---|
1115 | dDebug (0, "attached enabled joint not tagged"); |
---|
1116 | } |
---|
1117 | else |
---|
1118 | { |
---|
1119 | if (j->tag) |
---|
1120 | dDebug (0, "unattached or disabled joint tagged"); |
---|
1121 | } |
---|
1122 | } |
---|
1123 | # endif |
---|
1124 | |
---|
1125 | # ifdef TIMING |
---|
1126 | dTimerEnd (); |
---|
1127 | dTimerReport (stdout, 1); |
---|
1128 | # endif |
---|
1129 | } |
---|
1130 | |
---|
1131 | #endif |
---|
1132 | |
---|
1133 | |
---|
1134 | void dWorldStepFast1 (dWorldID w, dReal stepsize, int maxiterations) |
---|
1135 | { |
---|
1136 | dUASSERT (w, "bad world argument"); |
---|
1137 | dUASSERT (stepsize > 0, "stepsize must be > 0"); |
---|
1138 | processIslandsFast (w, stepsize, maxiterations); |
---|
1139 | } |
---|