aboutsummaryrefslogtreecommitdiffstatshomepage
path: root/libraries/ode-0.9/ode/src/step.cpp
diff options
context:
space:
mode:
Diffstat (limited to 'libraries/ode-0.9/ode/src/step.cpp')
-rw-r--r--libraries/ode-0.9/ode/src/step.cpp1795
1 files changed, 1795 insertions, 0 deletions
diff --git a/libraries/ode-0.9/ode/src/step.cpp b/libraries/ode-0.9/ode/src/step.cpp
new file mode 100644
index 0000000..19d0473
--- /dev/null
+++ b/libraries/ode-0.9/ode/src/step.cpp
@@ -0,0 +1,1795 @@
1/*************************************************************************
2 * *
3 * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. *
4 * All rights reserved. Email: russ@q12.org Web: www.q12.org *
5 * *
6 * This library is free software; you can redistribute it and/or *
7 * modify it under the terms of EITHER: *
8 * (1) The GNU Lesser General Public License as published by the Free *
9 * Software Foundation; either version 2.1 of the License, or (at *
10 * your option) any later version. The text of the GNU Lesser *
11 * General Public License is included with this library in the *
12 * file LICENSE.TXT. *
13 * (2) The BSD-style license that is included with this library in *
14 * the file LICENSE-BSD.TXT. *
15 * *
16 * This library is distributed in the hope that it will be useful, *
17 * but WITHOUT ANY WARRANTY; without even the implied warranty of *
18 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files *
19 * LICENSE.TXT and LICENSE-BSD.TXT for more details. *
20 * *
21 *************************************************************************/
22
23#include "objects.h"
24#include "joint.h"
25#include <ode/config.h>
26#include <ode/odemath.h>
27#include <ode/rotation.h>
28#include <ode/timer.h>
29#include <ode/error.h>
30#include <ode/matrix.h>
31#include "lcp.h"
32#include "util.h"
33
34//****************************************************************************
35// misc defines
36
37#define FAST_FACTOR
38//#define TIMING
39
40// memory allocation system
41#ifdef dUSE_MALLOC_FOR_ALLOCA
42unsigned int dMemoryFlag;
43#define REPORT_OUT_OF_MEMORY fprintf(stderr, "Insufficient memory to complete rigid body simulation. Results will not be accurate.\n")
44
45#define ALLOCA(t,v,s) t* v=(t*)malloc(s)
46#define UNALLOCA(t) free(t)
47
48#else
49#define ALLOCA(t,v,s) t* v=(t*)dALLOCA16(s)
50#define UNALLOCA(t) /* nothing */
51#endif
52
53
54//****************************************************************************
55// debugging - comparison of various vectors and matrices produced by the
56// slow and fast versions of the stepper.
57
58//#define COMPARE_METHODS
59
60#ifdef COMPARE_METHODS
61#include "testing.h"
62dMatrixComparison comparator;
63#endif
64
65// undef to use the fast decomposition
66#define DIRECT_CHOLESKY
67#undef REPORT_ERROR
68
69//****************************************************************************
70// special matrix multipliers
71
72// this assumes the 4th and 8th rows of B and C are zero.
73
74static void Multiply2_p8r (dReal *A, dReal *B, dReal *C,
75 int p, int r, int Askip)
76{
77 int i,j;
78 dReal sum,*bb,*cc;
79 dIASSERT (p>0 && r>0 && A && B && C);
80 bb = B;
81 for (i=p; i; i--) {
82 cc = C;
83 for (j=r; j; j--) {
84 sum = bb[0]*cc[0];
85 sum += bb[1]*cc[1];
86 sum += bb[2]*cc[2];
87 sum += bb[4]*cc[4];
88 sum += bb[5]*cc[5];
89 sum += bb[6]*cc[6];
90 *(A++) = sum;
91 cc += 8;
92 }
93 A += Askip - r;
94 bb += 8;
95 }
96}
97
98
99// this assumes the 4th and 8th rows of B and C are zero.
100
101static void MultiplyAdd2_p8r (dReal *A, dReal *B, dReal *C,
102 int p, int r, int Askip)
103{
104 int i,j;
105 dReal sum,*bb,*cc;
106 dIASSERT (p>0 && r>0 && A && B && C);
107 bb = B;
108 for (i=p; i; i--) {
109 cc = C;
110 for (j=r; j; j--) {
111 sum = bb[0]*cc[0];
112 sum += bb[1]*cc[1];
113 sum += bb[2]*cc[2];
114 sum += bb[4]*cc[4];
115 sum += bb[5]*cc[5];
116 sum += bb[6]*cc[6];
117 *(A++) += sum;
118 cc += 8;
119 }
120 A += Askip - r;
121 bb += 8;
122 }
123}
124
125
126// this assumes the 4th and 8th rows of B are zero.
127
128static void Multiply0_p81 (dReal *A, dReal *B, dReal *C, int p)
129{
130 int i;
131 dIASSERT (p>0 && A && B && C);
132 dReal sum;
133 for (i=p; i; i--) {
134 sum = B[0]*C[0];
135 sum += B[1]*C[1];
136 sum += B[2]*C[2];
137 sum += B[4]*C[4];
138 sum += B[5]*C[5];
139 sum += B[6]*C[6];
140 *(A++) = sum;
141 B += 8;
142 }
143}
144
145
146// this assumes the 4th and 8th rows of B are zero.
147
148static void MultiplyAdd0_p81 (dReal *A, dReal *B, dReal *C, int p)
149{
150 int i;
151 dIASSERT (p>0 && A && B && C);
152 dReal sum;
153 for (i=p; i; i--) {
154 sum = B[0]*C[0];
155 sum += B[1]*C[1];
156 sum += B[2]*C[2];
157 sum += B[4]*C[4];
158 sum += B[5]*C[5];
159 sum += B[6]*C[6];
160 *(A++) += sum;
161 B += 8;
162 }
163}
164
165
166// this assumes the 4th and 8th rows of B are zero.
167
168static void MultiplyAdd1_8q1 (dReal *A, dReal *B, dReal *C, int q)
169{
170 int k;
171 dReal sum;
172 dIASSERT (q>0 && A && B && C);
173 sum = 0;
174 for (k=0; k<q; k++) sum += B[k*8] * C[k];
175 A[0] += sum;
176 sum = 0;
177 for (k=0; k<q; k++) sum += B[1+k*8] * C[k];
178 A[1] += sum;
179 sum = 0;
180 for (k=0; k<q; k++) sum += B[2+k*8] * C[k];
181 A[2] += sum;
182 sum = 0;
183 for (k=0; k<q; k++) sum += B[4+k*8] * C[k];
184 A[4] += sum;
185 sum = 0;
186 for (k=0; k<q; k++) sum += B[5+k*8] * C[k];
187 A[5] += sum;
188 sum = 0;
189 for (k=0; k<q; k++) sum += B[6+k*8] * C[k];
190 A[6] += sum;
191}
192
193
194// this assumes the 4th and 8th rows of B are zero.
195
196static void Multiply1_8q1 (dReal *A, dReal *B, dReal *C, int q)
197{
198 int k;
199 dReal sum;
200 dIASSERT (q>0 && A && B && C);
201 sum = 0;
202 for (k=0; k<q; k++) sum += B[k*8] * C[k];
203 A[0] = sum;
204 sum = 0;
205 for (k=0; k<q; k++) sum += B[1+k*8] * C[k];
206 A[1] = sum;
207 sum = 0;
208 for (k=0; k<q; k++) sum += B[2+k*8] * C[k];
209 A[2] = sum;
210 sum = 0;
211 for (k=0; k<q; k++) sum += B[4+k*8] * C[k];
212 A[4] = sum;
213 sum = 0;
214 for (k=0; k<q; k++) sum += B[5+k*8] * C[k];
215 A[5] = sum;
216 sum = 0;
217 for (k=0; k<q; k++) sum += B[6+k*8] * C[k];
218 A[6] = sum;
219}
220
221//****************************************************************************
222// the slow, but sure way
223// note that this does not do any joint feedback!
224
225// given lists of bodies and joints that form an island, perform a first
226// order timestep.
227//
228// `body' is the body array, `nb' is the size of the array.
229// `_joint' is the body array, `nj' is the size of the array.
230
231void dInternalStepIsland_x1 (dxWorld *world, dxBody * const *body, int nb,
232 dxJoint * const *_joint, int nj, dReal stepsize)
233{
234 int i,j,k;
235 int n6 = 6*nb;
236
237#ifdef TIMING
238 dTimerStart("preprocessing");
239#endif
240
241 // number all bodies in the body list - set their tag values
242 for (i=0; i<nb; i++) body[i]->tag = i;
243
244 // make a local copy of the joint array, because we might want to modify it.
245 // (the "dxJoint *const*" declaration says we're allowed to modify the joints
246 // but not the joint array, because the caller might need it unchanged).
247 ALLOCA(dxJoint*,joint,nj*sizeof(dxJoint*));
248#ifdef dUSE_MALLOC_FOR_ALLOCA
249 if (joint == NULL) {
250 dMemoryFlag = d_MEMORY_OUT_OF_MEMORY;
251 return;
252 }
253#endif
254 memcpy (joint,_joint,nj * sizeof(dxJoint*));
255
256 // for all bodies, compute the inertia tensor and its inverse in the global
257 // frame, and compute the rotational force and add it to the torque
258 // accumulator.
259 // @@@ check computation of rotational force.
260 ALLOCA(dReal,I,3*nb*4*sizeof(dReal));
261#ifdef dUSE_MALLOC_FOR_ALLOCA
262 if (I == NULL) {
263 UNALLOCA(joint);
264 dMemoryFlag = d_MEMORY_OUT_OF_MEMORY;
265 return;
266 }
267#endif
268 ALLOCA(dReal,invI,3*nb*4*sizeof(dReal));
269#ifdef dUSE_MALLOC_FOR_ALLOCA
270 if (invI == NULL) {
271 UNALLOCA(I);
272 UNALLOCA(joint);
273 dMemoryFlag = d_MEMORY_OUT_OF_MEMORY;
274 return;
275 }
276#endif
277
278 //dSetZero (I,3*nb*4);
279 //dSetZero (invI,3*nb*4);
280 for (i=0; i<nb; i++) {
281 dReal tmp[12];
282 // compute inertia tensor in global frame
283 dMULTIPLY2_333 (tmp,body[i]->mass.I,body[i]->posr.R);
284 dMULTIPLY0_333 (I+i*12,body[i]->posr.R,tmp);
285 // compute inverse inertia tensor in global frame
286 dMULTIPLY2_333 (tmp,body[i]->invI,body[i]->posr.R);
287 dMULTIPLY0_333 (invI+i*12,body[i]->posr.R,tmp);
288 // compute rotational force
289 dMULTIPLY0_331 (tmp,I+i*12,body[i]->avel);
290 dCROSS (body[i]->tacc,-=,body[i]->avel,tmp);
291 }
292
293 // add the gravity force to all bodies
294 for (i=0; i<nb; i++) {
295 if ((body[i]->flags & dxBodyNoGravity)==0) {
296 body[i]->facc[0] += body[i]->mass.mass * world->gravity[0];
297 body[i]->facc[1] += body[i]->mass.mass * world->gravity[1];
298 body[i]->facc[2] += body[i]->mass.mass * world->gravity[2];
299 }
300 }
301
302 // get m = total constraint dimension, nub = number of unbounded variables.
303 // create constraint offset array and number-of-rows array for all joints.
304 // the constraints are re-ordered as follows: the purely unbounded
305 // constraints, the mixed unbounded + LCP constraints, and last the purely
306 // LCP constraints.
307 //
308 // joints with m=0 are inactive and are removed from the joints array
309 // entirely, so that the code that follows does not consider them.
310 int m = 0;
311 ALLOCA(dxJoint::Info1,info,nj*sizeof(dxJoint::Info1));
312#ifdef dUSE_MALLOC_FOR_ALLOCA
313 if (info == NULL) {
314 UNALLOCA(invI);
315 UNALLOCA(I);
316 UNALLOCA(joint);
317 dMemoryFlag = d_MEMORY_OUT_OF_MEMORY;
318 return;
319 }
320#endif
321
322 ALLOCA(int,ofs,nj*sizeof(int));
323#ifdef dUSE_MALLOC_FOR_ALLOCA
324 if (ofs == NULL) {
325 UNALLOCA(info);
326 UNALLOCA(invI);
327 UNALLOCA(I);
328 UNALLOCA(joint);
329 dMemoryFlag = d_MEMORY_OUT_OF_MEMORY;
330 return;
331 }
332#endif
333
334 for (i=0, j=0; j<nj; j++) { // i=dest, j=src
335 joint[j]->vtable->getInfo1 (joint[j],info+i);
336 dIASSERT (info[i].m >= 0 && info[i].m <= 6 &&
337 info[i].nub >= 0 && info[i].nub <= info[i].m);
338 if (info[i].m > 0) {
339 joint[i] = joint[j];
340 i++;
341 }
342 }
343 nj = i;
344
345 // the purely unbounded constraints
346 for (i=0; i<nj; i++) if (info[i].nub == info[i].m) {
347 ofs[i] = m;
348 m += info[i].m;
349 }
350 int nub = m;
351 // the mixed unbounded + LCP constraints
352 for (i=0; i<nj; i++) if (info[i].nub > 0 && info[i].nub < info[i].m) {
353 ofs[i] = m;
354 m += info[i].m;
355 }
356 // the purely LCP constraints
357 for (i=0; i<nj; i++) if (info[i].nub == 0) {
358 ofs[i] = m;
359 m += info[i].m;
360 }
361
362 // create (6*nb,6*nb) inverse mass matrix `invM', and fill it with mass
363 // parameters
364#ifdef TIMING
365 dTimerNow ("create mass matrix");
366#endif
367 int nskip = dPAD (n6);
368 ALLOCA(dReal, invM, n6*nskip*sizeof(dReal));
369#ifdef dUSE_MALLOC_FOR_ALLOCA
370 if (invM == NULL) {
371 UNALLOCA(ofs);
372 UNALLOCA(info);
373 UNALLOCA(invI);
374 UNALLOCA(I);
375 UNALLOCA(joint);
376 dMemoryFlag = d_MEMORY_OUT_OF_MEMORY;
377 return;
378 }
379#endif
380
381 dSetZero (invM,n6*nskip);
382 for (i=0; i<nb; i++) {
383 dReal *MM = invM+(i*6)*nskip+(i*6);
384 MM[0] = body[i]->invMass;
385 MM[nskip+1] = body[i]->invMass;
386 MM[2*nskip+2] = body[i]->invMass;
387 MM += 3*nskip+3;
388 for (j=0; j<3; j++) for (k=0; k<3; k++) {
389 MM[j*nskip+k] = invI[i*12+j*4+k];
390 }
391 }
392
393 // assemble some body vectors: fe = external forces, v = velocities
394 ALLOCA(dReal,fe,n6*sizeof(dReal));
395#ifdef dUSE_MALLOC_FOR_ALLOCA
396 if (fe == NULL) {
397 UNALLOCA(invM);
398 UNALLOCA(ofs);
399 UNALLOCA(info);
400 UNALLOCA(invI);
401 UNALLOCA(I);
402 UNALLOCA(joint);
403 dMemoryFlag = d_MEMORY_OUT_OF_MEMORY;
404 return;
405 }
406#endif
407
408 ALLOCA(dReal,v,n6*sizeof(dReal));
409#ifdef dUSE_MALLOC_FOR_ALLOCA
410 if (v == NULL) {
411 UNALLOCA(fe);
412 UNALLOCA(invM);
413 UNALLOCA(ofs);
414 UNALLOCA(info);
415 UNALLOCA(invI);
416 UNALLOCA(I);
417 UNALLOCA(joint);
418 dMemoryFlag = d_MEMORY_OUT_OF_MEMORY;
419 return;
420 }
421#endif
422
423 //dSetZero (fe,n6);
424 //dSetZero (v,n6);
425 for (i=0; i<nb; i++) {
426 for (j=0; j<3; j++) fe[i*6+j] = body[i]->facc[j];
427 for (j=0; j<3; j++) fe[i*6+3+j] = body[i]->tacc[j];
428 for (j=0; j<3; j++) v[i*6+j] = body[i]->lvel[j];
429 for (j=0; j<3; j++) v[i*6+3+j] = body[i]->avel[j];
430 }
431
432 // this will be set to the velocity update
433 ALLOCA(dReal,vnew,n6*sizeof(dReal));
434#ifdef dUSE_MALLOC_FOR_ALLOCA
435 if (vnew == NULL) {
436 UNALLOCA(v);
437 UNALLOCA(fe);
438 UNALLOCA(invM);
439 UNALLOCA(ofs);
440 UNALLOCA(info);
441 UNALLOCA(invI);
442 UNALLOCA(I);
443 UNALLOCA(joint);
444 dMemoryFlag = d_MEMORY_OUT_OF_MEMORY;
445 return;
446 }
447#endif
448 dSetZero (vnew,n6);
449
450 // if there are constraints, compute cforce
451 if (m > 0) {
452 // create a constraint equation right hand side vector `c', a constraint
453 // force mixing vector `cfm', and LCP low and high bound vectors, and an
454 // 'findex' vector.
455 ALLOCA(dReal,c,m*sizeof(dReal));
456#ifdef dUSE_MALLOC_FOR_ALLOCA
457 if (c == NULL) {
458 UNALLOCA(vnew);
459 UNALLOCA(v);
460 UNALLOCA(fe);
461 UNALLOCA(invM);
462 UNALLOCA(ofs);
463 UNALLOCA(info);
464 UNALLOCA(invI);
465 UNALLOCA(I);
466 UNALLOCA(joint);
467 dMemoryFlag = d_MEMORY_OUT_OF_MEMORY;
468 return;
469 }
470#endif
471 ALLOCA(dReal,cfm,m*sizeof(dReal));
472#ifdef dUSE_MALLOC_FOR_ALLOCA
473 if (cfm == NULL) {
474 UNALLOCA(c);
475 UNALLOCA(vnew);
476 UNALLOCA(v);
477 UNALLOCA(fe);
478 UNALLOCA(invM);
479 UNALLOCA(ofs);
480 UNALLOCA(info);
481 UNALLOCA(invI);
482 UNALLOCA(I);
483 UNALLOCA(joint);
484 dMemoryFlag = d_MEMORY_OUT_OF_MEMORY;
485 return;
486 }
487#endif
488 ALLOCA(dReal,lo,m*sizeof(dReal));
489#ifdef dUSE_MALLOC_FOR_ALLOCA
490 if (lo == NULL) {
491 UNALLOCA(cfm);
492 UNALLOCA(c);
493 UNALLOCA(vnew);
494 UNALLOCA(v);
495 UNALLOCA(fe);
496 UNALLOCA(invM);
497 UNALLOCA(ofs);
498 UNALLOCA(info);
499 UNALLOCA(invI);
500 UNALLOCA(I);
501 UNALLOCA(joint);
502 dMemoryFlag = d_MEMORY_OUT_OF_MEMORY;
503 return;
504 }
505#endif
506 ALLOCA(dReal,hi,m*sizeof(dReal));
507#ifdef dUSE_MALLOC_FOR_ALLOCA
508 if (hi == NULL) {
509 UNALLOCA(lo);
510 UNALLOCA(cfm);
511 UNALLOCA(c);
512 UNALLOCA(vnew);
513 UNALLOCA(v);
514 UNALLOCA(fe);
515 UNALLOCA(invM);
516 UNALLOCA(ofs);
517 UNALLOCA(info);
518 UNALLOCA(invI);
519 UNALLOCA(I);
520 UNALLOCA(joint);
521 dMemoryFlag = d_MEMORY_OUT_OF_MEMORY;
522 return;
523 }
524#endif
525 ALLOCA(int,findex,m*sizeof(int));
526#ifdef dUSE_MALLOC_FOR_ALLOCA
527 if (findex == NULL) {
528 UNALLOCA(hi);
529 UNALLOCA(lo);
530 UNALLOCA(cfm);
531 UNALLOCA(c);
532 UNALLOCA(vnew);
533 UNALLOCA(v);
534 UNALLOCA(fe);
535 UNALLOCA(invM);
536 UNALLOCA(ofs);
537 UNALLOCA(info);
538 UNALLOCA(invI);
539 UNALLOCA(I);
540 UNALLOCA(joint);
541 dMemoryFlag = d_MEMORY_OUT_OF_MEMORY;
542 return;
543 }
544#endif
545 dSetZero (c,m);
546 dSetValue (cfm,m,world->global_cfm);
547 dSetValue (lo,m,-dInfinity);
548 dSetValue (hi,m, dInfinity);
549 for (i=0; i<m; i++) findex[i] = -1;
550
551 // create (m,6*nb) jacobian mass matrix `J', and fill it with constraint
552 // data. also fill the c vector.
553# ifdef TIMING
554 dTimerNow ("create J");
555# endif
556 ALLOCA(dReal,J,m*nskip*sizeof(dReal));
557#ifdef dUSE_MALLOC_FOR_ALLOCA
558 if (J == NULL) {
559 UNALLOCA(findex);
560 UNALLOCA(hi);
561 UNALLOCA(lo);
562 UNALLOCA(cfm);
563 UNALLOCA(c);
564 UNALLOCA(vnew);
565 UNALLOCA(v);
566 UNALLOCA(fe);
567 UNALLOCA(invM);
568 UNALLOCA(ofs);
569 UNALLOCA(info);
570 UNALLOCA(invI);
571 UNALLOCA(I);
572 UNALLOCA(joint);
573 dMemoryFlag = d_MEMORY_OUT_OF_MEMORY;
574 return;
575 }
576#endif
577 dSetZero (J,m*nskip);
578 dxJoint::Info2 Jinfo;
579 Jinfo.rowskip = nskip;
580 Jinfo.fps = dRecip(stepsize);
581 Jinfo.erp = world->global_erp;
582 for (i=0; i<nj; i++) {
583 Jinfo.J1l = J + nskip*ofs[i] + 6*joint[i]->node[0].body->tag;
584 Jinfo.J1a = Jinfo.J1l + 3;
585 if (joint[i]->node[1].body) {
586 Jinfo.J2l = J + nskip*ofs[i] + 6*joint[i]->node[1].body->tag;
587 Jinfo.J2a = Jinfo.J2l + 3;
588 }
589 else {
590 Jinfo.J2l = 0;
591 Jinfo.J2a = 0;
592 }
593 Jinfo.c = c + ofs[i];
594 Jinfo.cfm = cfm + ofs[i];
595 Jinfo.lo = lo + ofs[i];
596 Jinfo.hi = hi + ofs[i];
597 Jinfo.findex = findex + ofs[i];
598 joint[i]->vtable->getInfo2 (joint[i],&Jinfo);
599 // adjust returned findex values for global index numbering
600 for (j=0; j<info[i].m; j++) {
601 if (findex[ofs[i] + j] >= 0) findex[ofs[i] + j] += ofs[i];
602 }
603 }
604
605 // compute A = J*invM*J'
606# ifdef TIMING
607 dTimerNow ("compute A");
608# endif
609 ALLOCA(dReal,JinvM,m*nskip*sizeof(dReal));
610#ifdef dUSE_MALLOC_FOR_ALLOCA
611 if (JinvM == NULL) {
612 UNALLOCA(J);
613 UNALLOCA(findex);
614 UNALLOCA(hi);
615 UNALLOCA(lo);
616 UNALLOCA(cfm);
617 UNALLOCA(c);
618 UNALLOCA(vnew);
619 UNALLOCA(v);
620 UNALLOCA(fe);
621 UNALLOCA(invM);
622 UNALLOCA(ofs);
623 UNALLOCA(info);
624 UNALLOCA(invI);
625 UNALLOCA(I);
626 UNALLOCA(joint);
627 dMemoryFlag = d_MEMORY_OUT_OF_MEMORY;
628 return;
629 }
630#endif
631 //dSetZero (JinvM,m*nskip);
632 dMultiply0 (JinvM,J,invM,m,n6,n6);
633 int mskip = dPAD(m);
634 ALLOCA(dReal,A,m*mskip*sizeof(dReal));
635#ifdef dUSE_MALLOC_FOR_ALLOCA
636 if (A == NULL) {
637 UNALLOCA(JinvM);
638 UNALLOCA(J);
639 UNALLOCA(findex);
640 UNALLOCA(hi);
641 UNALLOCA(lo);
642 UNALLOCA(cfm);
643 UNALLOCA(c);
644 UNALLOCA(vnew);
645 UNALLOCA(v);
646 UNALLOCA(fe);
647 UNALLOCA(invM);
648 UNALLOCA(ofs);
649 UNALLOCA(info);
650 UNALLOCA(invI);
651 UNALLOCA(I);
652 UNALLOCA(joint);
653 dMemoryFlag = d_MEMORY_OUT_OF_MEMORY;
654 return;
655 }
656#endif
657 //dSetZero (A,m*mskip);
658 dMultiply2 (A,JinvM,J,m,n6,m);
659
660 // add cfm to the diagonal of A
661 for (i=0; i<m; i++) A[i*mskip+i] += cfm[i] * Jinfo.fps;
662
663# ifdef COMPARE_METHODS
664 comparator.nextMatrix (A,m,m,1,"A");
665# endif
666
667 // compute `rhs', the right hand side of the equation J*a=c
668# ifdef TIMING
669 dTimerNow ("compute rhs");
670# endif
671 ALLOCA(dReal,tmp1,n6*sizeof(dReal));
672#ifdef dUSE_MALLOC_FOR_ALLOCA
673 if (tmp1 == NULL) {
674 UNALLOCA(A);
675 UNALLOCA(JinvM);
676 UNALLOCA(J);
677 UNALLOCA(findex);
678 UNALLOCA(hi);
679 UNALLOCA(lo);
680 UNALLOCA(cfm);
681 UNALLOCA(c);
682 UNALLOCA(vnew);
683 UNALLOCA(v);
684 UNALLOCA(fe);
685 UNALLOCA(invM);
686 UNALLOCA(ofs);
687 UNALLOCA(info);
688 UNALLOCA(invI);
689 UNALLOCA(I);
690 UNALLOCA(joint);
691 dMemoryFlag = d_MEMORY_OUT_OF_MEMORY;
692 return;
693 }
694#endif
695 //dSetZero (tmp1,n6);
696 dMultiply0 (tmp1,invM,fe,n6,n6,1);
697 for (i=0; i<n6; i++) tmp1[i] += v[i]/stepsize;
698 ALLOCA(dReal,rhs,m*sizeof(dReal));
699#ifdef dUSE_MALLOC_FOR_ALLOCA
700 if (rhs == NULL) {
701 UNALLOCA(tmp1);
702 UNALLOCA(A);
703 UNALLOCA(JinvM);
704 UNALLOCA(J);
705 UNALLOCA(findex);
706 UNALLOCA(hi);
707 UNALLOCA(lo);
708 UNALLOCA(cfm);
709 UNALLOCA(c);
710 UNALLOCA(vnew);
711 UNALLOCA(v);
712 UNALLOCA(fe);
713 UNALLOCA(invM);
714 UNALLOCA(ofs);
715 UNALLOCA(info);
716 UNALLOCA(invI);
717 UNALLOCA(I);
718 UNALLOCA(joint);
719 dMemoryFlag = d_MEMORY_OUT_OF_MEMORY;
720 return;
721 }
722#endif
723 //dSetZero (rhs,m);
724 dMultiply0 (rhs,J,tmp1,m,n6,1);
725 for (i=0; i<m; i++) rhs[i] = c[i]/stepsize - rhs[i];
726
727# ifdef COMPARE_METHODS
728 comparator.nextMatrix (c,m,1,0,"c");
729 comparator.nextMatrix (rhs,m,1,0,"rhs");
730# endif
731
732
733
734
735
736#ifndef DIRECT_CHOLESKY
737 // solve the LCP problem and get lambda.
738 // this will destroy A but that's okay
739# ifdef TIMING
740 dTimerNow ("solving LCP problem");
741# endif
742 ALLOCA(dReal,lambda,m*sizeof(dReal));
743#ifdef dUSE_MALLOC_FOR_ALLOCA
744 if (lambda == NULL) {
745 UNALLOCA(rhs);
746 UNALLOCA(tmp1);
747 UNALLOCA(A);
748 UNALLOCA(JinvM);
749 UNALLOCA(J);
750 UNALLOCA(findex);
751 UNALLOCA(hi);
752 UNALLOCA(lo);
753 UNALLOCA(cfm);
754 UNALLOCA(c);
755 UNALLOCA(vnew);
756 UNALLOCA(v);
757 UNALLOCA(fe);
758 UNALLOCA(invM);
759 UNALLOCA(ofs);
760 UNALLOCA(info);
761 UNALLOCA(invI);
762 UNALLOCA(I);
763 UNALLOCA(joint);
764 dMemoryFlag = d_MEMORY_OUT_OF_MEMORY;
765 return;
766 }
767#endif
768 ALLOCA(dReal,residual,m*sizeof(dReal));
769#ifdef dUSE_MALLOC_FOR_ALLOCA
770 if (residual == NULL) {
771 UNALLOCA(lambda);
772 UNALLOCA(rhs);
773 UNALLOCA(tmp1);
774 UNALLOCA(A);
775 UNALLOCA(JinvM);
776 UNALLOCA(J);
777 UNALLOCA(findex);
778 UNALLOCA(hi);
779 UNALLOCA(lo);
780 UNALLOCA(cfm);
781 UNALLOCA(c);
782 UNALLOCA(vnew);
783 UNALLOCA(v);
784 UNALLOCA(fe);
785 UNALLOCA(invM);
786 UNALLOCA(ofs);
787 UNALLOCA(info);
788 UNALLOCA(invI);
789 UNALLOCA(I);
790 UNALLOCA(joint);
791 dMemoryFlag = d_MEMORY_OUT_OF_MEMORY;
792 return;
793 }
794#endif
795 dSolveLCP (m,A,lambda,rhs,residual,nub,lo,hi,findex);
796 UNALLOCA(residual);
797 UNALLOCA(lambda);
798
799#ifdef dUSE_MALLOC_FOR_ALLOCA
800 if (dMemoryFlag == d_MEMORY_OUT_OF_MEMORY)
801 return;
802#endif
803
804
805#else
806
807 // OLD WAY - direct factor and solve
808
809 // factorize A (L*L'=A)
810# ifdef TIMING
811 dTimerNow ("factorize A");
812# endif
813 ALLOCA(dReal,L,m*mskip*sizeof(dReal));
814#ifdef dUSE_MALLOC_FOR_ALLOCA
815 if (L == NULL) {
816 UNALLOCA(rhs);
817 UNALLOCA(tmp1);
818 UNALLOCA(A);
819 UNALLOCA(JinvM);
820 UNALLOCA(J);
821 UNALLOCA(findex);
822 UNALLOCA(hi);
823 UNALLOCA(lo);
824 UNALLOCA(cfm);
825 UNALLOCA(c);
826 UNALLOCA(vnew);
827 UNALLOCA(v);
828 UNALLOCA(fe);
829 UNALLOCA(invM);
830 UNALLOCA(ofs);
831 UNALLOCA(info);
832 UNALLOCA(invI);
833 UNALLOCA(I);
834 UNALLOCA(joint);
835 dMemoryFlag = d_MEMORY_OUT_OF_MEMORY;
836 return;
837 }
838#endif
839 memcpy (L,A,m*mskip*sizeof(dReal));
840 if (dFactorCholesky (L,m)==0) dDebug (0,"A is not positive definite");
841
842 // compute lambda
843# ifdef TIMING
844 dTimerNow ("compute lambda");
845# endif
846 ALLOCA(dReal,lambda,m*sizeof(dReal));
847#ifdef dUSE_MALLOC_FOR_ALLOCA
848 if (lambda == NULL) {
849 UNALLOCA(L);
850 UNALLOCA(rhs);
851 UNALLOCA(tmp1);
852 UNALLOCA(A);
853 UNALLOCA(JinvM);
854 UNALLOCA(J);
855 UNALLOCA(findex);
856 UNALLOCA(hi);
857 UNALLOCA(lo);
858 UNALLOCA(cfm);
859 UNALLOCA(c);
860 UNALLOCA(vnew);
861 UNALLOCA(v);
862 UNALLOCA(fe);
863 UNALLOCA(invM);
864 UNALLOCA(ofs);
865 UNALLOCA(info);
866 UNALLOCA(invI);
867 UNALLOCA(I);
868 UNALLOCA(joint);
869 dMemoryFlag = d_MEMORY_OUT_OF_MEMORY;
870 return;
871 }
872#endif
873 memcpy (lambda,rhs,m * sizeof(dReal));
874 dSolveCholesky (L,lambda,m);
875#endif
876
877# ifdef COMPARE_METHODS
878 comparator.nextMatrix (lambda,m,1,0,"lambda");
879# endif
880
881 // compute the velocity update `vnew'
882# ifdef TIMING
883 dTimerNow ("compute velocity update");
884# endif
885 dMultiply1 (tmp1,J,lambda,n6,m,1);
886 for (i=0; i<n6; i++) tmp1[i] += fe[i];
887 dMultiply0 (vnew,invM,tmp1,n6,n6,1);
888 for (i=0; i<n6; i++) vnew[i] = v[i] + stepsize*vnew[i];
889
890#ifdef REPORT_ERROR
891 // see if the constraint has worked: compute J*vnew and make sure it equals
892 // `c' (to within a certain tolerance).
893# ifdef TIMING
894 dTimerNow ("verify constraint equation");
895# endif
896 dMultiply0 (tmp1,J,vnew,m,n6,1);
897 dReal err = 0;
898 for (i=0; i<m; i++) {
899 err += dFabs(tmp1[i]-c[i]);
900 }
901 printf ("total constraint error=%.6e\n",err);
902#endif
903
904 UNALLOCA(c);
905 UNALLOCA(cfm);
906 UNALLOCA(lo);
907 UNALLOCA(hi);
908 UNALLOCA(findex);
909 UNALLOCA(J);
910 UNALLOCA(JinvM);
911 UNALLOCA(A);
912 UNALLOCA(tmp1);
913 UNALLOCA(rhs);
914 UNALLOCA(lambda);
915 UNALLOCA(L);
916 }
917 else {
918 // no constraints
919 dMultiply0 (vnew,invM,fe,n6,n6,1);
920 for (i=0; i<n6; i++) vnew[i] = v[i] + stepsize*vnew[i];
921 }
922
923#ifdef COMPARE_METHODS
924 comparator.nextMatrix (vnew,n6,1,0,"vnew");
925#endif
926
927 // apply the velocity update to the bodies
928#ifdef TIMING
929 dTimerNow ("update velocity");
930#endif
931 for (i=0; i<nb; i++) {
932 for (j=0; j<3; j++) body[i]->lvel[j] = vnew[i*6+j];
933 for (j=0; j<3; j++) body[i]->avel[j] = vnew[i*6+3+j];
934 }
935
936 // update the position and orientation from the new linear/angular velocity
937 // (over the given timestep)
938#ifdef TIMING
939 dTimerNow ("update position");
940#endif
941 for (i=0; i<nb; i++) dxStepBody (body[i],stepsize);
942
943#ifdef TIMING
944 dTimerNow ("tidy up");
945#endif
946
947 // zero all force accumulators
948 for (i=0; i<nb; i++) {
949 body[i]->facc[0] = 0;
950 body[i]->facc[1] = 0;
951 body[i]->facc[2] = 0;
952 body[i]->facc[3] = 0;
953 body[i]->tacc[0] = 0;
954 body[i]->tacc[1] = 0;
955 body[i]->tacc[2] = 0;
956 body[i]->tacc[3] = 0;
957 }
958
959#ifdef TIMING
960 dTimerEnd();
961 if (m > 0) dTimerReport (stdout,1);
962#endif
963
964 UNALLOCA(joint);
965 UNALLOCA(I);
966 UNALLOCA(invI);
967 UNALLOCA(info);
968 UNALLOCA(ofs);
969 UNALLOCA(invM);
970 UNALLOCA(fe);
971 UNALLOCA(v);
972 UNALLOCA(vnew);
973}
974
975//****************************************************************************
976// an optimized version of dInternalStepIsland1()
977
978void dInternalStepIsland_x2 (dxWorld *world, dxBody * const *body, int nb,
979 dxJoint * const *_joint, int nj, dReal stepsize)
980{
981 int i,j,k;
982#ifdef TIMING
983 dTimerStart("preprocessing");
984#endif
985
986 dReal stepsize1 = dRecip(stepsize);
987
988 // number all bodies in the body list - set their tag values
989 for (i=0; i<nb; i++) body[i]->tag = i;
990
991 // make a local copy of the joint array, because we might want to modify it.
992 // (the "dxJoint *const*" declaration says we're allowed to modify the joints
993 // but not the joint array, because the caller might need it unchanged).
994 ALLOCA(dxJoint*,joint,nj*sizeof(dxJoint*));
995#ifdef dUSE_MALLOC_FOR_ALLOCA
996 if (joint == NULL) {
997 dMemoryFlag = d_MEMORY_OUT_OF_MEMORY;
998 return;
999 }
1000#endif
1001 memcpy (joint,_joint,nj * sizeof(dxJoint*));
1002
1003 // for all bodies, compute the inertia tensor and its inverse in the global
1004 // frame, and compute the rotational force and add it to the torque
1005 // accumulator. I and invI are vertically stacked 3x4 matrices, one per body.
1006 // @@@ check computation of rotational force.
1007#ifdef dGYROSCOPIC
1008 ALLOCA(dReal,I,3*nb*4*sizeof(dReal));
1009# ifdef dUSE_MALLOC_FOR_ALLOCA
1010 if (I == NULL) {
1011 UNALLOCA(joint);
1012 dMemoryFlag = d_MEMORY_OUT_OF_MEMORY;
1013 return;
1014 }
1015# endif
1016#else
1017 dReal *I = NULL;
1018#endif // for dGYROSCOPIC
1019
1020 ALLOCA(dReal,invI,3*nb*4*sizeof(dReal));
1021#ifdef dUSE_MALLOC_FOR_ALLOCA
1022 if (invI == NULL) {
1023 UNALLOCA(I);
1024 UNALLOCA(joint);
1025 dMemoryFlag = d_MEMORY_OUT_OF_MEMORY;
1026 return;
1027 }
1028#endif
1029
1030 //dSetZero (I,3*nb*4);
1031 //dSetZero (invI,3*nb*4);
1032 for (i=0; i<nb; i++) {
1033 dReal tmp[12];
1034
1035 // compute inverse inertia tensor in global frame
1036 dMULTIPLY2_333 (tmp,body[i]->invI,body[i]->posr.R);
1037 dMULTIPLY0_333 (invI+i*12,body[i]->posr.R,tmp);
1038#ifdef dGYROSCOPIC
1039 // compute inertia tensor in global frame
1040 dMULTIPLY2_333 (tmp,body[i]->mass.I,body[i]->posr.R);
1041 dMULTIPLY0_333 (I+i*12,body[i]->posr.R,tmp);
1042
1043 // compute rotational force
1044 dMULTIPLY0_331 (tmp,I+i*12,body[i]->avel);
1045 dCROSS (body[i]->tacc,-=,body[i]->avel,tmp);
1046#endif
1047 }
1048
1049 // add the gravity force to all bodies
1050 for (i=0; i<nb; i++) {
1051 if ((body[i]->flags & dxBodyNoGravity)==0) {
1052 body[i]->facc[0] += body[i]->mass.mass * world->gravity[0];
1053 body[i]->facc[1] += body[i]->mass.mass * world->gravity[1];
1054 body[i]->facc[2] += body[i]->mass.mass * world->gravity[2];
1055 }
1056 }
1057
1058 // get m = total constraint dimension, nub = number of unbounded variables.
1059 // create constraint offset array and number-of-rows array for all joints.
1060 // the constraints are re-ordered as follows: the purely unbounded
1061 // constraints, the mixed unbounded + LCP constraints, and last the purely
1062 // LCP constraints. this assists the LCP solver to put all unbounded
1063 // variables at the start for a quick factorization.
1064 //
1065 // joints with m=0 are inactive and are removed from the joints array
1066 // entirely, so that the code that follows does not consider them.
1067 // also number all active joints in the joint list (set their tag values).
1068 // inactive joints receive a tag value of -1.
1069
1070 int m = 0;
1071 ALLOCA(dxJoint::Info1,info,nj*sizeof(dxJoint::Info1));
1072#ifdef dUSE_MALLOC_FOR_ALLOCA
1073 if (info == NULL) {
1074 UNALLOCA(invI);
1075 UNALLOCA(I);
1076 UNALLOCA(joint);
1077 dMemoryFlag = d_MEMORY_OUT_OF_MEMORY;
1078 return;
1079 }
1080#endif
1081 ALLOCA(int,ofs,nj*sizeof(int));
1082#ifdef dUSE_MALLOC_FOR_ALLOCA
1083 if (ofs == NULL) {
1084 UNALLOCA(info);
1085 UNALLOCA(invI);
1086 UNALLOCA(I);
1087 UNALLOCA(joint);
1088 dMemoryFlag = d_MEMORY_OUT_OF_MEMORY;
1089 return;
1090 }
1091#endif
1092 for (i=0, j=0; j<nj; j++) { // i=dest, j=src
1093 joint[j]->vtable->getInfo1 (joint[j],info+i);
1094 dIASSERT (info[i].m >= 0 && info[i].m <= 6 &&
1095 info[i].nub >= 0 && info[i].nub <= info[i].m);
1096 if (info[i].m > 0) {
1097 joint[i] = joint[j];
1098 joint[i]->tag = i;
1099 i++;
1100 }
1101 else {
1102 joint[j]->tag = -1;
1103 }
1104 }
1105 nj = i;
1106
1107 // the purely unbounded constraints
1108 for (i=0; i<nj; i++) if (info[i].nub == info[i].m) {
1109 ofs[i] = m;
1110 m += info[i].m;
1111 }
1112 int nub = m;
1113 // the mixed unbounded + LCP constraints
1114 for (i=0; i<nj; i++) if (info[i].nub > 0 && info[i].nub < info[i].m) {
1115 ofs[i] = m;
1116 m += info[i].m;
1117 }
1118 // the purely LCP constraints
1119 for (i=0; i<nj; i++) if (info[i].nub == 0) {
1120 ofs[i] = m;
1121 m += info[i].m;
1122 }
1123
1124 // this will be set to the force due to the constraints
1125 ALLOCA(dReal,cforce,nb*8*sizeof(dReal));
1126#ifdef dUSE_MALLOC_FOR_ALLOCA
1127 if (cforce == NULL) {
1128 UNALLOCA(ofs);
1129 UNALLOCA(info);
1130 UNALLOCA(invI);
1131 UNALLOCA(I);
1132 UNALLOCA(joint);
1133 dMemoryFlag = d_MEMORY_OUT_OF_MEMORY;
1134 return;
1135 }
1136#endif
1137 dSetZero (cforce,nb*8);
1138
1139 // if there are constraints, compute cforce
1140 if (m > 0) {
1141 // create a constraint equation right hand side vector `c', a constraint
1142 // force mixing vector `cfm', and LCP low and high bound vectors, and an
1143 // 'findex' vector.
1144 ALLOCA(dReal,c,m*sizeof(dReal));
1145#ifdef dUSE_MALLOC_FOR_ALLOCA
1146 if (c == NULL) {
1147 UNALLOCA(cforce);
1148 UNALLOCA(ofs);
1149 UNALLOCA(info);
1150 UNALLOCA(invI);
1151 UNALLOCA(I);
1152 UNALLOCA(joint);
1153 dMemoryFlag = d_MEMORY_OUT_OF_MEMORY;
1154 return;
1155 }
1156#endif
1157 ALLOCA(dReal,cfm,m*sizeof(dReal));
1158#ifdef dUSE_MALLOC_FOR_ALLOCA
1159 if (cfm == NULL) {
1160 UNALLOCA(c);
1161 UNALLOCA(cforce);
1162 UNALLOCA(ofs);
1163 UNALLOCA(info);
1164 UNALLOCA(invI);
1165 UNALLOCA(I);
1166 UNALLOCA(joint);
1167 dMemoryFlag = d_MEMORY_OUT_OF_MEMORY;
1168 return;
1169 }
1170#endif
1171 ALLOCA(dReal,lo,m*sizeof(dReal));
1172#ifdef dUSE_MALLOC_FOR_ALLOCA
1173 if (lo == NULL) {
1174 UNALLOCA(cfm);
1175 UNALLOCA(c);
1176 UNALLOCA(cforce);
1177 UNALLOCA(ofs);
1178 UNALLOCA(info);
1179 UNALLOCA(invI);
1180 UNALLOCA(I);
1181 UNALLOCA(joint);
1182 dMemoryFlag = d_MEMORY_OUT_OF_MEMORY;
1183 return;
1184 }
1185#endif
1186 ALLOCA(dReal,hi,m*sizeof(dReal));
1187#ifdef dUSE_MALLOC_FOR_ALLOCA
1188 if (hi == NULL) {
1189 UNALLOCA(lo);
1190 UNALLOCA(cfm);
1191 UNALLOCA(c);
1192 UNALLOCA(cforce);
1193 UNALLOCA(ofs);
1194 UNALLOCA(info);
1195 UNALLOCA(invI);
1196 UNALLOCA(I);
1197 UNALLOCA(joint);
1198 dMemoryFlag = d_MEMORY_OUT_OF_MEMORY;
1199 return;
1200 }
1201#endif
1202 ALLOCA(int,findex,m*sizeof(int));
1203#ifdef dUSE_MALLOC_FOR_ALLOCA
1204 if (findex == NULL) {
1205 UNALLOCA(hi);
1206 UNALLOCA(lo);
1207 UNALLOCA(cfm);
1208 UNALLOCA(c);
1209 UNALLOCA(cforce);
1210 UNALLOCA(ofs);
1211 UNALLOCA(info);
1212 UNALLOCA(invI);
1213 UNALLOCA(I);
1214 UNALLOCA(joint);
1215 dMemoryFlag = d_MEMORY_OUT_OF_MEMORY;
1216 return;
1217 }
1218#endif
1219 dSetZero (c,m);
1220 dSetValue (cfm,m,world->global_cfm);
1221 dSetValue (lo,m,-dInfinity);
1222 dSetValue (hi,m, dInfinity);
1223 for (i=0; i<m; i++) findex[i] = -1;
1224
1225 // get jacobian data from constraints. a (2*m)x8 matrix will be created
1226 // to store the two jacobian blocks from each constraint. it has this
1227 // format:
1228 //
1229 // l l l 0 a a a 0 \ .
1230 // l l l 0 a a a 0 }-- jacobian body 1 block for joint 0 (3 rows)
1231 // l l l 0 a a a 0 /
1232 // l l l 0 a a a 0 \ .
1233 // l l l 0 a a a 0 }-- jacobian body 2 block for joint 0 (3 rows)
1234 // l l l 0 a a a 0 /
1235 // l l l 0 a a a 0 }--- jacobian body 1 block for joint 1 (1 row)
1236 // l l l 0 a a a 0 }--- jacobian body 2 block for joint 1 (1 row)
1237 // etc...
1238 //
1239 // (lll) = linear jacobian data
1240 // (aaa) = angular jacobian data
1241 //
1242# ifdef TIMING
1243 dTimerNow ("create J");
1244# endif
1245 ALLOCA(dReal,J,2*m*8*sizeof(dReal));
1246#ifdef dUSE_MALLOC_FOR_ALLOCA
1247 if (J == NULL) {
1248 UNALLOCA(findex);
1249 UNALLOCA(hi);
1250 UNALLOCA(lo);
1251 UNALLOCA(cfm);
1252 UNALLOCA(c);
1253 UNALLOCA(cforce);
1254 UNALLOCA(ofs);
1255 UNALLOCA(info);
1256 UNALLOCA(invI);
1257 UNALLOCA(I);
1258 UNALLOCA(joint);
1259 dMemoryFlag = d_MEMORY_OUT_OF_MEMORY;
1260 return;
1261 }
1262#endif
1263 dSetZero (J,2*m*8);
1264 dxJoint::Info2 Jinfo;
1265 Jinfo.rowskip = 8;
1266 Jinfo.fps = stepsize1;
1267 Jinfo.erp = world->global_erp;
1268 for (i=0; i<nj; i++) {
1269 Jinfo.J1l = J + 2*8*ofs[i];
1270 Jinfo.J1a = Jinfo.J1l + 4;
1271 Jinfo.J2l = Jinfo.J1l + 8*info[i].m;
1272 Jinfo.J2a = Jinfo.J2l + 4;
1273 Jinfo.c = c + ofs[i];
1274 Jinfo.cfm = cfm + ofs[i];
1275 Jinfo.lo = lo + ofs[i];
1276 Jinfo.hi = hi + ofs[i];
1277 Jinfo.findex = findex + ofs[i];
1278 joint[i]->vtable->getInfo2 (joint[i],&Jinfo);
1279 // adjust returned findex values for global index numbering
1280 for (j=0; j<info[i].m; j++) {
1281 if (findex[ofs[i] + j] >= 0) findex[ofs[i] + j] += ofs[i];
1282 }
1283 }
1284
1285 // compute A = J*invM*J'. first compute JinvM = J*invM. this has the same
1286 // format as J so we just go through the constraints in J multiplying by
1287 // the appropriate scalars and matrices.
1288# ifdef TIMING
1289 dTimerNow ("compute A");
1290# endif
1291 ALLOCA(dReal,JinvM,2*m*8*sizeof(dReal));
1292#ifdef dUSE_MALLOC_FOR_ALLOCA
1293 if (JinvM == NULL) {
1294 UNALLOCA(J);
1295 UNALLOCA(findex);
1296 UNALLOCA(hi);
1297 UNALLOCA(lo);
1298 UNALLOCA(cfm);
1299 UNALLOCA(c);
1300 UNALLOCA(cforce);
1301 UNALLOCA(ofs);
1302 UNALLOCA(info);
1303 UNALLOCA(invI);
1304 UNALLOCA(I);
1305 UNALLOCA(joint);
1306 dMemoryFlag = d_MEMORY_OUT_OF_MEMORY;
1307 return;
1308 }
1309#endif
1310 dSetZero (JinvM,2*m*8);
1311 for (i=0; i<nj; i++) {
1312 int b = joint[i]->node[0].body->tag;
1313 dReal body_invMass = body[b]->invMass;
1314 dReal *body_invI = invI + b*12;
1315 dReal *Jsrc = J + 2*8*ofs[i];
1316 dReal *Jdst = JinvM + 2*8*ofs[i];
1317 for (j=info[i].m-1; j>=0; j--) {
1318 for (k=0; k<3; k++) Jdst[k] = Jsrc[k] * body_invMass;
1319 dMULTIPLY0_133 (Jdst+4,Jsrc+4,body_invI);
1320 Jsrc += 8;
1321 Jdst += 8;
1322 }
1323 if (joint[i]->node[1].body) {
1324 b = joint[i]->node[1].body->tag;
1325 body_invMass = body[b]->invMass;
1326 body_invI = invI + b*12;
1327 for (j=info[i].m-1; j>=0; j--) {
1328 for (k=0; k<3; k++) Jdst[k] = Jsrc[k] * body_invMass;
1329 dMULTIPLY0_133 (Jdst+4,Jsrc+4,body_invI);
1330 Jsrc += 8;
1331 Jdst += 8;
1332 }
1333 }
1334 }
1335
1336 // now compute A = JinvM * J'. A's rows and columns are grouped by joint,
1337 // i.e. in the same way as the rows of J. block (i,j) of A is only nonzero
1338 // if joints i and j have at least one body in common. this fact suggests
1339 // the algorithm used to fill A:
1340 //
1341 // for b = all bodies
1342 // n = number of joints attached to body b
1343 // for i = 1..n
1344 // for j = i+1..n
1345 // ii = actual joint number for i
1346 // jj = actual joint number for j
1347 // // (ii,jj) will be set to all pairs of joints around body b
1348 // compute blockwise: A(ii,jj) += JinvM(ii) * J(jj)'
1349 //
1350 // this algorithm catches all pairs of joints that have at least one body
1351 // in common. it does not compute the diagonal blocks of A however -
1352 // another similar algorithm does that.
1353
1354 int mskip = dPAD(m);
1355 ALLOCA(dReal,A,m*mskip*sizeof(dReal));
1356#ifdef dUSE_MALLOC_FOR_ALLOCA
1357 if (A == NULL) {
1358 UNALLOCA(JinvM);
1359 UNALLOCA(J);
1360 UNALLOCA(findex);
1361 UNALLOCA(hi);
1362 UNALLOCA(lo);
1363 UNALLOCA(cfm);
1364 UNALLOCA(c);
1365 UNALLOCA(cforce);
1366 UNALLOCA(ofs);
1367 UNALLOCA(info);
1368 UNALLOCA(invI);
1369 UNALLOCA(I);
1370 UNALLOCA(joint);
1371 dMemoryFlag = d_MEMORY_OUT_OF_MEMORY;
1372 return;
1373 }
1374#endif
1375 dSetZero (A,m*mskip);
1376 for (i=0; i<nb; i++) {
1377 for (dxJointNode *n1=body[i]->firstjoint; n1; n1=n1->next) {
1378 for (dxJointNode *n2=n1->next; n2; n2=n2->next) {
1379 // get joint numbers and ensure ofs[j1] >= ofs[j2]
1380 int j1 = n1->joint->tag;
1381 int j2 = n2->joint->tag;
1382 if (ofs[j1] < ofs[j2]) {
1383 int tmp = j1;
1384 j1 = j2;
1385 j2 = tmp;
1386 }
1387
1388 // if either joint was tagged as -1 then it is an inactive (m=0)
1389 // joint that should not be considered
1390 if (j1==-1 || j2==-1) continue;
1391
1392 // determine if body i is the 1st or 2nd body of joints j1 and j2
1393 int jb1 = (joint[j1]->node[1].body == body[i]);
1394 int jb2 = (joint[j2]->node[1].body == body[i]);
1395 // jb1/jb2 must be 0 for joints with only one body
1396 dIASSERT(joint[j1]->node[1].body || jb1==0);
1397 dIASSERT(joint[j2]->node[1].body || jb2==0);
1398
1399 // set block of A
1400 MultiplyAdd2_p8r (A + ofs[j1]*mskip + ofs[j2],
1401 JinvM + 2*8*ofs[j1] + jb1*8*info[j1].m,
1402 J + 2*8*ofs[j2] + jb2*8*info[j2].m,
1403 info[j1].m,info[j2].m, mskip);
1404 }
1405 }
1406 }
1407 // compute diagonal blocks of A
1408 for (i=0; i<nj; i++) {
1409 Multiply2_p8r (A + ofs[i]*(mskip+1),
1410 JinvM + 2*8*ofs[i],
1411 J + 2*8*ofs[i],
1412 info[i].m,info[i].m, mskip);
1413 if (joint[i]->node[1].body) {
1414 MultiplyAdd2_p8r (A + ofs[i]*(mskip+1),
1415 JinvM + 2*8*ofs[i] + 8*info[i].m,
1416 J + 2*8*ofs[i] + 8*info[i].m,
1417 info[i].m,info[i].m, mskip);
1418 }
1419 }
1420
1421 // add cfm to the diagonal of A
1422 for (i=0; i<m; i++) A[i*mskip+i] += cfm[i] * stepsize1;
1423
1424# ifdef COMPARE_METHODS
1425 comparator.nextMatrix (A,m,m,1,"A");
1426# endif
1427
1428 // compute the right hand side `rhs'
1429# ifdef TIMING
1430 dTimerNow ("compute rhs");
1431# endif
1432 ALLOCA(dReal,tmp1,nb*8*sizeof(dReal));
1433#ifdef dUSE_MALLOC_FOR_ALLOCA
1434 if (tmp1 == NULL) {
1435 UNALLOCA(A);
1436 UNALLOCA(JinvM);
1437 UNALLOCA(J);
1438 UNALLOCA(findex);
1439 UNALLOCA(hi);
1440 UNALLOCA(lo);
1441 UNALLOCA(cfm);
1442 UNALLOCA(c);
1443 UNALLOCA(cforce);
1444 UNALLOCA(ofs);
1445 UNALLOCA(info);
1446 UNALLOCA(invI);
1447 UNALLOCA(I);
1448 UNALLOCA(joint);
1449 dMemoryFlag = d_MEMORY_OUT_OF_MEMORY;
1450 return;
1451 }
1452#endif
1453 //dSetZero (tmp1,nb*8);
1454 // put v/h + invM*fe into tmp1
1455 for (i=0; i<nb; i++) {
1456 dReal body_invMass = body[i]->invMass;
1457 dReal *body_invI = invI + i*12;
1458 for (j=0; j<3; j++) tmp1[i*8+j] = body[i]->facc[j] * body_invMass +
1459 body[i]->lvel[j] * stepsize1;
1460 dMULTIPLY0_331 (tmp1 + i*8 + 4,body_invI,body[i]->tacc);
1461 for (j=0; j<3; j++) tmp1[i*8+4+j] += body[i]->avel[j] * stepsize1;
1462 }
1463 // put J*tmp1 into rhs
1464 ALLOCA(dReal,rhs,m*sizeof(dReal));
1465#ifdef dUSE_MALLOC_FOR_ALLOCA
1466 if (rhs == NULL) {
1467 UNALLOCA(tmp1);
1468 UNALLOCA(A);
1469 UNALLOCA(JinvM);
1470 UNALLOCA(J);
1471 UNALLOCA(findex);
1472 UNALLOCA(hi);
1473 UNALLOCA(lo);
1474 UNALLOCA(cfm);
1475 UNALLOCA(c);
1476 UNALLOCA(cforce);
1477 UNALLOCA(ofs);
1478 UNALLOCA(info);
1479 UNALLOCA(invI);
1480 UNALLOCA(I);
1481 UNALLOCA(joint);
1482 dMemoryFlag = d_MEMORY_OUT_OF_MEMORY;
1483 return;
1484 }
1485#endif
1486 //dSetZero (rhs,m);
1487 for (i=0; i<nj; i++) {
1488 dReal *JJ = J + 2*8*ofs[i];
1489 Multiply0_p81 (rhs+ofs[i],JJ,
1490 tmp1 + 8*joint[i]->node[0].body->tag, info[i].m);
1491 if (joint[i]->node[1].body) {
1492 MultiplyAdd0_p81 (rhs+ofs[i],JJ + 8*info[i].m,
1493 tmp1 + 8*joint[i]->node[1].body->tag, info[i].m);
1494 }
1495 }
1496 // complete rhs
1497 for (i=0; i<m; i++) rhs[i] = c[i]*stepsize1 - rhs[i];
1498
1499# ifdef COMPARE_METHODS
1500 comparator.nextMatrix (c,m,1,0,"c");
1501 comparator.nextMatrix (rhs,m,1,0,"rhs");
1502# endif
1503
1504 // solve the LCP problem and get lambda.
1505 // this will destroy A but that's okay
1506# ifdef TIMING
1507 dTimerNow ("solving LCP problem");
1508# endif
1509 ALLOCA(dReal,lambda,m*sizeof(dReal));
1510#ifdef dUSE_MALLOC_FOR_ALLOCA
1511 if (lambda == NULL) {
1512 UNALLOCA(rhs);
1513 UNALLOCA(tmp1);
1514 UNALLOCA(A);
1515 UNALLOCA(JinvM);
1516 UNALLOCA(J);
1517 UNALLOCA(findex);
1518 UNALLOCA(hi);
1519 UNALLOCA(lo);
1520 UNALLOCA(cfm);
1521 UNALLOCA(c);
1522 UNALLOCA(cforce);
1523 UNALLOCA(ofs);
1524 UNALLOCA(info);
1525 UNALLOCA(invI);
1526 UNALLOCA(I);
1527 UNALLOCA(joint);
1528 dMemoryFlag = d_MEMORY_OUT_OF_MEMORY;
1529 return;
1530 }
1531#endif
1532 ALLOCA(dReal,residual,m*sizeof(dReal));
1533#ifdef dUSE_MALLOC_FOR_ALLOCA
1534 if (residual == NULL) {
1535 UNALLOCA(lambda);
1536 UNALLOCA(rhs);
1537 UNALLOCA(tmp1);
1538 UNALLOCA(A);
1539 UNALLOCA(JinvM);
1540 UNALLOCA(J);
1541 UNALLOCA(findex);
1542 UNALLOCA(hi);
1543 UNALLOCA(lo);
1544 UNALLOCA(cfm);
1545 UNALLOCA(c);
1546 UNALLOCA(cforce);
1547 UNALLOCA(ofs);
1548 UNALLOCA(info);
1549 UNALLOCA(invI);
1550 UNALLOCA(I);
1551 UNALLOCA(joint);
1552 dMemoryFlag = d_MEMORY_OUT_OF_MEMORY;
1553 return;
1554 }
1555#endif
1556 dSolveLCP (m,A,lambda,rhs,residual,nub,lo,hi,findex);
1557
1558#ifdef dUSE_MALLOC_FOR_ALLOCA
1559 if (dMemoryFlag == d_MEMORY_OUT_OF_MEMORY)
1560 return;
1561#endif
1562
1563
1564// OLD WAY - direct factor and solve
1565//
1566// // factorize A (L*L'=A)
1567//# ifdef TIMING
1568// dTimerNow ("factorize A");
1569//# endif
1570// dReal *L = (dReal*) ALLOCA (m*mskip*sizeof(dReal));
1571// memcpy (L,A,m*mskip*sizeof(dReal));
1572//# ifdef FAST_FACTOR
1573// dFastFactorCholesky (L,m); // does not report non positive definiteness
1574//# else
1575// if (dFactorCholesky (L,m)==0) dDebug (0,"A is not positive definite");
1576//# endif
1577//
1578// // compute lambda
1579//# ifdef TIMING
1580// dTimerNow ("compute lambda");
1581//# endif
1582// dReal *lambda = (dReal*) ALLOCA (m * sizeof(dReal));
1583// memcpy (lambda,rhs,m * sizeof(dReal));
1584// dSolveCholesky (L,lambda,m);
1585
1586# ifdef COMPARE_METHODS
1587 comparator.nextMatrix (lambda,m,1,0,"lambda");
1588# endif
1589
1590 // compute the constraint force `cforce'
1591# ifdef TIMING
1592 dTimerNow ("compute constraint force");
1593# endif
1594 // compute cforce = J'*lambda
1595 for (i=0; i<nj; i++) {
1596 dReal *JJ = J + 2*8*ofs[i];
1597 dxBody* b1 = joint[i]->node[0].body;
1598 dxBody* b2 = joint[i]->node[1].body;
1599 dJointFeedback *fb = joint[i]->feedback;
1600
1601 if (fb) {
1602 // the user has requested feedback on the amount of force that this
1603 // joint is applying to the bodies. we use a slightly slower
1604 // computation that splits out the force components and puts them
1605 // in the feedback structure.
1606 dReal data1[8],data2[8];
1607 Multiply1_8q1 (data1, JJ, lambda+ofs[i], info[i].m);
1608 dReal *cf1 = cforce + 8*b1->tag;
1609 cf1[0] += (fb->f1[0] = data1[0]);
1610 cf1[1] += (fb->f1[1] = data1[1]);
1611 cf1[2] += (fb->f1[2] = data1[2]);
1612 cf1[4] += (fb->t1[0] = data1[4]);
1613 cf1[5] += (fb->t1[1] = data1[5]);
1614 cf1[6] += (fb->t1[2] = data1[6]);
1615 if (b2){
1616 Multiply1_8q1 (data2, JJ + 8*info[i].m, lambda+ofs[i], info[i].m);
1617 dReal *cf2 = cforce + 8*b2->tag;
1618 cf2[0] += (fb->f2[0] = data2[0]);
1619 cf2[1] += (fb->f2[1] = data2[1]);
1620 cf2[2] += (fb->f2[2] = data2[2]);
1621 cf2[4] += (fb->t2[0] = data2[4]);
1622 cf2[5] += (fb->t2[1] = data2[5]);
1623 cf2[6] += (fb->t2[2] = data2[6]);
1624 }
1625 }
1626 else {
1627 // no feedback is required, let's compute cforce the faster way
1628 MultiplyAdd1_8q1 (cforce + 8*b1->tag,JJ, lambda+ofs[i], info[i].m);
1629 if (b2) {
1630 MultiplyAdd1_8q1 (cforce + 8*b2->tag,
1631 JJ + 8*info[i].m, lambda+ofs[i], info[i].m);
1632 }
1633 }
1634 }
1635 UNALLOCA(c);
1636 UNALLOCA(cfm);
1637 UNALLOCA(lo);
1638 UNALLOCA(hi);
1639 UNALLOCA(findex);
1640 UNALLOCA(J);
1641 UNALLOCA(JinvM);
1642 UNALLOCA(A);
1643 UNALLOCA(tmp1);
1644 UNALLOCA(rhs);
1645 UNALLOCA(lambda);
1646 UNALLOCA(residual);
1647 }
1648
1649 // compute the velocity update
1650#ifdef TIMING
1651 dTimerNow ("compute velocity update");
1652#endif
1653
1654 // add fe to cforce
1655 for (i=0; i<nb; i++) {
1656 for (j=0; j<3; j++) cforce[i*8+j] += body[i]->facc[j];
1657 for (j=0; j<3; j++) cforce[i*8+4+j] += body[i]->tacc[j];
1658 }
1659 // multiply cforce by stepsize
1660 for (i=0; i < nb*8; i++) cforce[i] *= stepsize;
1661 // add invM * cforce to the body velocity
1662 for (i=0; i<nb; i++) {
1663 dReal body_invMass = body[i]->invMass;
1664 dReal *body_invI = invI + i*12;
1665 for (j=0; j<3; j++) body[i]->lvel[j] += body_invMass * cforce[i*8+j];
1666 dMULTIPLYADD0_331 (body[i]->avel,body_invI,cforce+i*8+4);
1667 }
1668
1669 // update the position and orientation from the new linear/angular velocity
1670 // (over the given timestep)
1671# ifdef TIMING
1672 dTimerNow ("update position");
1673# endif
1674 for (i=0; i<nb; i++) dxStepBody (body[i],stepsize);
1675
1676#ifdef COMPARE_METHODS
1677 ALLOCA(dReal,tmp, ALLOCA (nb*6*sizeof(dReal));
1678#ifdef dUSE_MALLOC_FOR_ALLOCA
1679 if (tmp == NULL) {
1680 UNALLOCA(cforce);
1681 UNALLOCA(ofs);
1682 UNALLOCA(info);
1683 UNALLOCA(invI);
1684 UNALLOCA(I);
1685 UNALLOCA(joint);
1686 dMemoryFlag = d_MEMORY_OUT_OF_MEMORY;
1687 return;
1688 }
1689#endif
1690 for (i=0; i<nb; i++) {
1691 for (j=0; j<3; j++) tmp_vnew[i*6+j] = body[i]->lvel[j];
1692 for (j=0; j<3; j++) tmp_vnew[i*6+3+j] = body[i]->avel[j];
1693 }
1694 comparator.nextMatrix (tmp_vnew,nb*6,1,0,"vnew");
1695 UNALLOCA(tmp);
1696#endif
1697
1698#ifdef TIMING
1699 dTimerNow ("tidy up");
1700#endif
1701
1702 // zero all force accumulators
1703 for (i=0; i<nb; i++) {
1704 body[i]->facc[0] = 0;
1705 body[i]->facc[1] = 0;
1706 body[i]->facc[2] = 0;
1707 body[i]->facc[3] = 0;
1708 body[i]->tacc[0] = 0;
1709 body[i]->tacc[1] = 0;
1710 body[i]->tacc[2] = 0;
1711 body[i]->tacc[3] = 0;
1712 }
1713
1714#ifdef TIMING
1715 dTimerEnd();
1716 if (m > 0) dTimerReport (stdout,1);
1717#endif
1718
1719 UNALLOCA(joint);
1720 UNALLOCA(I);
1721 UNALLOCA(invI);
1722 UNALLOCA(info);
1723 UNALLOCA(ofs);
1724 UNALLOCA(cforce);
1725}
1726
1727//****************************************************************************
1728
1729void dInternalStepIsland (dxWorld *world, dxBody * const *body, int nb,
1730 dxJoint * const *joint, int nj, dReal stepsize)
1731{
1732
1733#ifdef dUSE_MALLOC_FOR_ALLOCA
1734 dMemoryFlag = d_MEMORY_OK;
1735#endif
1736
1737#ifndef COMPARE_METHODS
1738 dInternalStepIsland_x2 (world,body,nb,joint,nj,stepsize);
1739
1740#ifdef dUSE_MALLOC_FOR_ALLOCA
1741 if (dMemoryFlag == d_MEMORY_OUT_OF_MEMORY) {
1742 REPORT_OUT_OF_MEMORY;
1743 return;
1744 }
1745#endif
1746
1747#endif
1748
1749#ifdef COMPARE_METHODS
1750 int i;
1751
1752 // save body state
1753 ALLOCA(dxBody,state,nb*sizeof(dxBody));
1754#ifdef dUSE_MALLOC_FOR_ALLOCA
1755 if (state == NULL) {
1756 dMemoryFlag = d_MEMORY_OUT_OF_MEMORY;
1757 REPORT_OUT_OF_MEMORY;
1758 return;
1759 }
1760#endif
1761 for (i=0; i<nb; i++) memcpy (state+i,body[i],sizeof(dxBody));
1762
1763 // take slow step
1764 comparator.reset();
1765 dInternalStepIsland_x1 (world,body,nb,joint,nj,stepsize);
1766 comparator.end();
1767#ifdef dUSE_MALLOC_FOR_ALLOCA
1768 if (dMemoryFlag == d_MEMORY_OUT_OF_MEMORY) {
1769 UNALLOCA(state);
1770 REPORT_OUT_OF_MEMORY;
1771 return;
1772 }
1773#endif
1774
1775 // restore state
1776 for (i=0; i<nb; i++) memcpy (body[i],state+i,sizeof(dxBody));
1777
1778 // take fast step
1779 dInternalStepIsland_x2 (world,body,nb,joint,nj,stepsize);
1780 comparator.end();
1781#ifdef dUSE_MALLOC_FOR_ALLOCA
1782 if (dMemoryFlag == d_MEMORY_OUT_OF_MEMORY) {
1783 UNALLOCA(state);
1784 REPORT_OUT_OF_MEMORY;
1785 return;
1786 }
1787#endif
1788
1789 //comparator.dump();
1790 //_exit (1);
1791 UNALLOCA(state);
1792#endif
1793}
1794
1795
generated by cgit v1.1 at 2024-11-11 09:55:12 +0000