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