aboutsummaryrefslogtreecommitdiffstatshomepage
path: root/libraries/ode-0.9/contrib/BreakableJoints/stepfast.cpp
diff options
context:
space:
mode:
Diffstat (limited to 'libraries/ode-0.9/contrib/BreakableJoints/stepfast.cpp')
-rwxr-xr-xlibraries/ode-0.9/contrib/BreakableJoints/stepfast.cpp1212
1 files changed, 0 insertions, 1212 deletions
diff --git a/libraries/ode-0.9/contrib/BreakableJoints/stepfast.cpp b/libraries/ode-0.9/contrib/BreakableJoints/stepfast.cpp
deleted file mode 100755
index a0eb9ae..0000000
--- a/libraries/ode-0.9/contrib/BreakableJoints/stepfast.cpp
+++ /dev/null
@@ -1,1212 +0,0 @@
1/*************************************************************************
2 * *
3 * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. *
4 * All rights reserved. Email: russ@q12.org Web: www.q12.org *
5 * *
6 * 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
55static int autoEnableDepth = 2;
56
57void dWorldSetAutoEnableDepthSF1 (dxWorld *world, int autodepth)
58{
59 if (autodepth > 0)
60 autoEnableDepth = autodepth;
61 else
62 autoEnableDepth = 0;
63}
64
65int dWorldGetAutoEnableDepthSF1 (dxWorld *world)
66{
67 return autoEnableDepth;
68}
69
70//little bit of math.... the _sym_ functions assume the return matrix will be symmetric
71static void
72Multiply2_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
101static void
102MultiplyAdd2_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
135static void
136Multiply0_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
157static void
158MultiplyAdd0_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
179static void
180Multiply1_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
217static inline dReal
218sinc (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
233static inline void
234moveAndRotateBody (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
329void
330dInternalStepFast (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
716void
717dInternalStepIslandFast (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.
992static void
993processIslandsFast (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
1038static void
1039processIslandsFast (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
1207void 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}
generated by cgit v1.1 at 2024-11-10 16:58:12 +0000