← Back to branch summary

~diresu/blender/blender-command-port

« back to all changes in this revision

Viewing changes to extern/ode/dist/ode/src/step.cpp

  • Committer: theeth
  • Date: 2008-10-14 16:52:04 UTC
  • Revision ID: vcs-imports@canonical.com-20081014165204-r32w2gm6s0osvdhn
copy back trunk

Show diffs side-by-side

added added

removed removed

Lines of Context:
extern/ode/dist/ode/src/step.cpp
 
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
 
 
33
//****************************************************************************
 
34
// misc defines
 
35
 
 
36
#define FAST_FACTOR
 
37
//#define TIMING
 
38
 
 
39
#define ALLOCA dALLOCA16
 
40
 
 
41
//****************************************************************************
 
42
// debugging - comparison of various vectors and matrices produced by the
 
43
// slow and fast versions of the stepper.
 
44
 
 
45
//#define COMPARE_METHODS
 
46
 
 
47
#ifdef COMPARE_METHODS
 
48
#include "testing.h"
 
49
dMatrixComparison comparator;
 
50
#endif
 
51
 
 
52
//****************************************************************************
 
53
// special matrix multipliers
 
54
 
 
55
// this assumes the 4th and 8th rows of B and C are zero.
 
56
 
 
57
static void Multiply2_p8r (dReal *A, dReal *B, dReal *C,
 
58
                           int p, int r, int Askip)
 
59
{
 
60
  int i,j;
 
61
  dReal sum,*bb,*cc;
 
62
  dIASSERT (p>0 && r>0 && A && B && C);
 
63
  bb = B;
 
64
  for (i=p; i; i--) {
 
65
    cc = C;
 
66
    for (j=r; j; j--) {
 
67
      sum = bb[0]*cc[0];
 
68
      sum += bb[1]*cc[1];
 
69
      sum += bb[2]*cc[2];
 
70
      sum += bb[4]*cc[4];
 
71
      sum += bb[5]*cc[5];
 
72
      sum += bb[6]*cc[6];
 
73
      *(A++) = sum; 
 
74
      cc += 8;
 
75
    }
 
76
    A += Askip - r;
 
77
    bb += 8;
 
78
  }
 
79
}
 
80
 
 
81
 
 
82
// this assumes the 4th and 8th rows of B and C are zero.
 
83
 
 
84
static void MultiplyAdd2_p8r (dReal *A, dReal *B, dReal *C,
 
85
                              int p, int r, int Askip)
 
86
{
 
87
  int i,j;
 
88
  dReal sum,*bb,*cc;
 
89
  dIASSERT (p>0 && r>0 && A && B && C);
 
90
  bb = B;
 
91
  for (i=p; i; i--) {
 
92
    cc = C;
 
93
    for (j=r; j; j--) {
 
94
      sum = bb[0]*cc[0];
 
95
      sum += bb[1]*cc[1];
 
96
      sum += bb[2]*cc[2];
 
97
      sum += bb[4]*cc[4];
 
98
      sum += bb[5]*cc[5];
 
99
      sum += bb[6]*cc[6];
 
100
      *(A++) += sum; 
 
101
      cc += 8;
 
102
    }
 
103
    A += Askip - r;
 
104
    bb += 8;
 
105
  }
 
106
}
 
107
 
 
108
 
 
109
// this assumes the 4th and 8th rows of B are zero.
 
110
 
 
111
static void Multiply0_p81 (dReal *A, dReal *B, dReal *C, int p)
 
112
{
 
113
  int i;
 
114
  dIASSERT (p>0 && A && B && C);
 
115
  dReal sum;
 
116
  for (i=p; i; i--) {
 
117
    sum =  B[0]*C[0];
 
118
    sum += B[1]*C[1];
 
119
    sum += B[2]*C[2];
 
120
    sum += B[4]*C[4];
 
121
    sum += B[5]*C[5];
 
122
    sum += B[6]*C[6];
 
123
    *(A++) = sum;
 
124
    B += 8;
 
125
  }
 
126
}
 
127
 
 
128
 
 
129
// this assumes the 4th and 8th rows of B are zero.
 
130
 
 
131
static void MultiplyAdd0_p81 (dReal *A, dReal *B, dReal *C, int p)
 
132
{
 
133
  int i;
 
134
  dIASSERT (p>0 && A && B && C);
 
135
  dReal sum;
 
136
  for (i=p; i; i--) {
 
137
    sum =  B[0]*C[0];
 
138
    sum += B[1]*C[1];
 
139
    sum += B[2]*C[2];
 
140
    sum += B[4]*C[4];
 
141
    sum += B[5]*C[5];
 
142
    sum += B[6]*C[6];
 
143
    *(A++) += sum;
 
144
    B += 8;
 
145
  }
 
146
}
 
147
 
 
148
 
 
149
// this assumes the 4th and 8th rows of B are zero.
 
150
 
 
151
static void MultiplyAdd1_8q1 (dReal *A, dReal *B, dReal *C, int q)
 
152
{
 
153
  int k;
 
154
  dReal sum;
 
155
  dIASSERT (q>0 && A && B && C);
 
156
  sum = 0;
 
157
  for (k=0; k<q; k++) sum += B[k*8] * C[k];
 
158
  A[0] += sum;
 
159
  sum = 0;
 
160
  for (k=0; k<q; k++) sum += B[1+k*8] * C[k];
 
161
  A[1] += sum;
 
162
  sum = 0;
 
163
  for (k=0; k<q; k++) sum += B[2+k*8] * C[k];
 
164
  A[2] += sum;
 
165
  sum = 0;
 
166
  for (k=0; k<q; k++) sum += B[4+k*8] * C[k];
 
167
  A[4] += sum;
 
168
  sum = 0;
 
169
  for (k=0; k<q; k++) sum += B[5+k*8] * C[k];
 
170
  A[5] += sum;
 
171
  sum = 0;
 
172
  for (k=0; k<q; k++) sum += B[6+k*8] * C[k];
 
173
  A[6] += sum;
 
174
}
 
175
 
 
176
 
 
177
// this assumes the 4th and 8th rows of B are zero.
 
178
 
 
179
static void Multiply1_8q1 (dReal *A, dReal *B, dReal *C, int q)
 
180
{
 
181
  int k;
 
182
  dReal sum;
 
183
  dIASSERT (q>0 && A && B && C);
 
184
  sum = 0;
 
185
  for (k=0; k<q; k++) sum += B[k*8] * C[k];
 
186
  A[0] = sum;
 
187
  sum = 0;
 
188
  for (k=0; k<q; k++) sum += B[1+k*8] * C[k];
 
189
  A[1] = sum;
 
190
  sum = 0;
 
191
  for (k=0; k<q; k++) sum += B[2+k*8] * C[k];
 
192
  A[2] = sum;
 
193
  sum = 0;
 
194
  for (k=0; k<q; k++) sum += B[4+k*8] * C[k];
 
195
  A[4] = sum;
 
196
  sum = 0;
 
197
  for (k=0; k<q; k++) sum += B[5+k*8] * C[k];
 
198
  A[5] = sum;
 
199
  sum = 0;
 
200
  for (k=0; k<q; k++) sum += B[6+k*8] * C[k];
 
201
  A[6] = sum;
 
202
}
 
203
 
 
204
//****************************************************************************
 
205
// body rotation
 
206
 
 
207
// return sin(x)/x. this has a singularity at 0 so special handling is needed
 
208
// for small arguments.
 
209
 
 
210
static inline dReal sinc (dReal x)
 
211
{
 
212
  // if |x| < 1e-4 then use a taylor series expansion. this two term expansion
 
213
  // is actually accurate to one LS bit within this range if double precision
 
214
  // is being used - so don't worry!
 
215
  if (dFabs(x) < 1.0e-4) return REAL(1.0) - x*x*REAL(0.166666666666666666667);
 
216
  else return dSin(x)/x;
 
217
}
 
218
 
 
219
 
 
220
// given a body b, apply its linear and angular rotation over the time
 
221
// interval h, thereby adjusting its position and orientation.
 
222
 
 
223
static inline void moveAndRotateBody (dxBody *b, dReal h)
 
224
{
 
225
  int j;
 
226
 
 
227
  // handle linear velocity
 
228
  for (j=0; j<3; j++) b->pos[j] += h * b->lvel[j];
 
229
 
 
230
  if (b->flags & dxBodyFlagFiniteRotation) {
 
231
    dVector3 irv;       // infitesimal rotation vector
 
232
    dQuaternion q;      // quaternion for finite rotation
 
233
 
 
234
    if (b->flags & dxBodyFlagFiniteRotationAxis) {
 
235
      // split the angular velocity vector into a component along the finite
 
236
      // rotation axis, and a component orthogonal to it.
 
237
      dVector3 frv,irv;         // finite rotation vector
 
238
      dReal k = dDOT (b->finite_rot_axis,b->avel);
 
239
      frv[0] = b->finite_rot_axis[0] * k;
 
240
      frv[1] = b->finite_rot_axis[1] * k;
 
241
      frv[2] = b->finite_rot_axis[2] * k;
 
242
      irv[0] = b->avel[0] - frv[0];
 
243
      irv[1] = b->avel[1] - frv[1];
 
244
      irv[2] = b->avel[2] - frv[2];
 
245
 
 
246
      // make a rotation quaternion q that corresponds to frv * h.
 
247
      // compare this with the full-finite-rotation case below.
 
248
      h *= REAL(0.5);
 
249
      dReal theta = k * h;
 
250
      q[0] = dCos(theta);
 
251
      dReal s = sinc(theta) * h;
 
252
      q[1] = frv[0] * s;
 
253
      q[2] = frv[1] * s;
 
254
      q[3] = frv[2] * s;
 
255
    }
 
256
    else {
 
257
      // make a rotation quaternion q that corresponds to w * h
 
258
      dReal wlen = dSqrt (b->avel[0]*b->avel[0] + b->avel[1]*b->avel[1] +
 
259
                          b->avel[2]*b->avel[2]);
 
260
      h *= REAL(0.5);
 
261
      dReal theta = wlen * h;
 
262
      q[0] = dCos(theta);
 
263
      dReal s = sinc(theta) * h;
 
264
      q[1] = b->avel[0] * s;
 
265
      q[2] = b->avel[1] * s;
 
266
      q[3] = b->avel[2] * s;
 
267
    }
 
268
 
 
269
    // do the finite rotation
 
270
    dQuaternion q2;
 
271
    dQMultiply0 (q2,q,b->q);
 
272
    for (j=0; j<4; j++) b->q[j] = q2[j];
 
273
 
 
274
    // do the infitesimal rotation if required
 
275
    if (b->flags & dxBodyFlagFiniteRotationAxis) {
 
276
      dReal dq[4];
 
277
      dWtoDQ (irv,b->q,dq);
 
278
      for (j=0; j<4; j++) b->q[j] += h * dq[j];
 
279
    }
 
280
  }
 
281
  else {
 
282
    // the normal way - do an infitesimal rotation
 
283
    dReal dq[4];
 
284
    dWtoDQ (b->avel,b->q,dq);
 
285
    for (j=0; j<4; j++) b->q[j] += h * dq[j];
 
286
  }
 
287
 
 
288
  // normalize the quaternion and convert it to a rotation matrix
 
289
  dNormalize4 (b->q);
 
290
  dQtoR (b->q,b->R);
 
291
}
 
292
 
 
293
//****************************************************************************
 
294
// the slow, but sure way
 
295
// note that this does not do any joint feedback!
 
296
 
 
297
// given lists of bodies and joints that form an island, perform a first
 
298
// order timestep.
 
299
//
 
300
// `body' is the body array, `nb' is the size of the array.
 
301
// `_joint' is the body array, `nj' is the size of the array.
 
302
 
 
303
void dInternalStepIsland_x1 (dxWorld *world, dxBody * const *body, int nb,
 
304
                             dxJoint * const *_joint, int nj, dReal stepsize)
 
305
{
 
306
  int i,j,k;
 
307
  int n6 = 6*nb;
 
308
 
 
309
# ifdef TIMING
 
310
  dTimerStart("preprocessing");
 
311
# endif
 
312
 
 
313
  // number all bodies in the body list - set their tag values
 
314
  for (i=0; i<nb; i++) body[i]->tag = i;
 
315
 
 
316
  // make a local copy of the joint array, because we might want to modify it.
 
317
  // (the "dxJoint *const*" declaration says we're allowed to modify the joints
 
318
  // but not the joint array, because the caller might need it unchanged).
 
319
  dxJoint **joint = (dxJoint**) ALLOCA (nj * sizeof(dxJoint*));
 
320
  memcpy (joint,_joint,nj * sizeof(dxJoint*));
 
321
 
 
322
  // for all bodies, compute the inertia tensor and its inverse in the global
 
323
  // frame, and compute the rotational force and add it to the torque
 
324
  // accumulator.
 
325
  // @@@ check computation of rotational force.
 
326
  dReal *I = (dReal*) ALLOCA (3*nb*4 * sizeof(dReal));
 
327
  dReal *invI = (dReal*) ALLOCA (3*nb*4 * sizeof(dReal));
 
328
  dSetZero (I,3*nb*4);
 
329
  dSetZero (invI,3*nb*4);
 
330
  for (i=0; i<nb; i++) {
 
331
    dReal tmp[12];
 
332
    // compute inertia tensor in global frame
 
333
    dMULTIPLY2_333 (tmp,body[i]->mass.I,body[i]->R);
 
334
    dMULTIPLY0_333 (I+i*12,body[i]->R,tmp);
 
335
    // compute inverse inertia tensor in global frame
 
336
    dMULTIPLY2_333 (tmp,body[i]->invI,body[i]->R);
 
337
    dMULTIPLY0_333 (invI+i*12,body[i]->R,tmp);
 
338
    // compute rotational force
 
339
    dMULTIPLY0_331 (tmp,I+i*12,body[i]->avel);
 
340
    dCROSS (body[i]->tacc,-=,body[i]->avel,tmp);
 
341
  }
 
342
 
 
343
  // add the gravity force to all bodies
 
344
  for (i=0; i<nb; i++) {
 
345
    if ((body[i]->flags & dxBodyNoGravity)==0) {
 
346
      body[i]->facc[0] += body[i]->mass.mass * world->gravity[0];
 
347
      body[i]->facc[1] += body[i]->mass.mass * world->gravity[1];
 
348
      body[i]->facc[2] += body[i]->mass.mass * world->gravity[2];
 
349
    }
 
350
  }
 
351
 
 
352
  // get m = total constraint dimension, nub = number of unbounded variables.
 
353
  // create constraint offset array and number-of-rows array for all joints.
 
354
  // the constraints are re-ordered as follows: the purely unbounded
 
355
  // constraints, the mixed unbounded + LCP constraints, and last the purely
 
356
  // LCP constraints.
 
357
  //
 
358
  // joints with m=0 are inactive and are removed from the joints array
 
359
  // entirely, so that the code that follows does not consider them.
 
360
  int m = 0;
 
361
  dxJoint::Info1 *info = (dxJoint::Info1*) ALLOCA (nj*sizeof(dxJoint::Info1));
 
362
  int *ofs = (int*) ALLOCA (nj*sizeof(int));
 
363
  for (i=0, j=0; j<nj; j++) {   // i=dest, j=src
 
364
    joint[j]->vtable->getInfo1 (joint[j],info+i);
 
365
    dIASSERT (info[i].m >= 0 && info[i].m <= 6 &&
 
366
              info[i].nub >= 0 && info[i].nub <= info[i].m);
 
367
    if (info[i].m > 0) {
 
368
      joint[i] = joint[j];
 
369
      i++;
 
370
    }
 
371
  }
 
372
  nj = i;
 
373
 
 
374
  // the purely unbounded constraints
 
375
  for (i=0; i<nj; i++) if (info[i].nub == info[i].m) {
 
376
    ofs[i] = m;
 
377
    m += info[i].m;
 
378
  }
 
379
  int nub = m;
 
380
  // the mixed unbounded + LCP constraints
 
381
  for (i=0; i<nj; i++) if (info[i].nub > 0 && info[i].nub < info[i].m) {
 
382
    ofs[i] = m;
 
383
    m += info[i].m;
 
384
  }
 
385
  // the purely LCP constraints
 
386
  for (i=0; i<nj; i++) if (info[i].nub == 0) {
 
387
    ofs[i] = m;
 
388
    m += info[i].m;
 
389
  }
 
390
 
 
391
  // create (6*nb,6*nb) inverse mass matrix `invM', and fill it with mass
 
392
  // parameters
 
393
# ifdef TIMING
 
394
  dTimerNow ("create mass matrix");
 
395
# endif
 
396
  int nskip = dPAD (n6);
 
397
  dReal *invM = (dReal*) ALLOCA (n6*nskip*sizeof(dReal));
 
398
  dSetZero (invM,n6*nskip);
 
399
  for (i=0; i<nb; i++) {
 
400
    dReal *MM = invM+(i*6)*nskip+(i*6);
 
401
    MM[0] = body[i]->invMass;
 
402
    MM[nskip+1] = body[i]->invMass;
 
403
    MM[2*nskip+2] = body[i]->invMass;
 
404
    MM += 3*nskip+3;
 
405
    for (j=0; j<3; j++) for (k=0; k<3; k++) {
 
406
      MM[j*nskip+k] = invI[i*12+j*4+k];
 
407
    }
 
408
  }
 
409
 
 
410
  // assemble some body vectors: fe = external forces, v = velocities
 
411
  dReal *fe = (dReal*) ALLOCA (n6 * sizeof(dReal));
 
412
  dReal *v = (dReal*) ALLOCA (n6 * sizeof(dReal));
 
413
  dSetZero (fe,n6);
 
414
  dSetZero (v,n6);
 
415
  for (i=0; i<nb; i++) {
 
416
    for (j=0; j<3; j++) fe[i*6+j] = body[i]->facc[j];
 
417
    for (j=0; j<3; j++) fe[i*6+3+j] = body[i]->tacc[j];
 
418
    for (j=0; j<3; j++) v[i*6+j] = body[i]->lvel[j];
 
419
    for (j=0; j<3; j++) v[i*6+3+j] = body[i]->avel[j];
 
420
  }
 
421
 
 
422
  // this will be set to the velocity update
 
423
  dReal *vnew = (dReal*) ALLOCA (n6 * sizeof(dReal));
 
424
  dSetZero (vnew,n6);
 
425
 
 
426
  // if there are constraints, compute cforce
 
427
  if (m > 0) {
 
428
    // create a constraint equation right hand side vector `c', a constraint
 
429
    // force mixing vector `cfm', and LCP low and high bound vectors, and an
 
430
    // 'findex' vector.
 
431
    dReal *c = (dReal*) ALLOCA (m*sizeof(dReal));
 
432
    dReal *cfm = (dReal*) ALLOCA (m*sizeof(dReal));
 
433
    dReal *lo = (dReal*) ALLOCA (m*sizeof(dReal));
 
434
    dReal *hi = (dReal*) ALLOCA (m*sizeof(dReal));
 
435
    int *findex = (int*) alloca (m*sizeof(int));
 
436
    dSetZero (c,m);
 
437
    dSetValue (cfm,m,world->global_cfm);
 
438
    dSetValue (lo,m,-dInfinity);
 
439
    dSetValue (hi,m, dInfinity);
 
440
    for (i=0; i<m; i++) findex[i] = -1;
 
441
 
 
442
    // create (m,6*nb) jacobian mass matrix `J', and fill it with constraint
 
443
    // data. also fill the c vector.
 
444
#   ifdef TIMING
 
445
    dTimerNow ("create J");
 
446
#   endif
 
447
    dReal *J = (dReal*) ALLOCA (m*nskip*sizeof(dReal));
 
448
    dSetZero (J,m*nskip);
 
449
    dxJoint::Info2 Jinfo;
 
450
    Jinfo.rowskip = nskip;
 
451
    Jinfo.fps = dRecip(stepsize);
 
452
    Jinfo.erp = world->global_erp;
 
453
    for (i=0; i<nj; i++) {
 
454
      Jinfo.J1l = J + nskip*ofs[i] + 6*joint[i]->node[0].body->tag;
 
455
      Jinfo.J1a = Jinfo.J1l + 3;
 
456
      if (joint[i]->node[1].body) {
 
457
        Jinfo.J2l = J + nskip*ofs[i] + 6*joint[i]->node[1].body->tag;
 
458
        Jinfo.J2a = Jinfo.J2l + 3;
 
459
      }
 
460
      else {
 
461
        Jinfo.J2l = 0;
 
462
        Jinfo.J2a = 0;
 
463
      }
 
464
      Jinfo.c = c + ofs[i];
 
465
      Jinfo.cfm = cfm + ofs[i];
 
466
      Jinfo.lo = lo + ofs[i];
 
467
      Jinfo.hi = hi + ofs[i];
 
468
      Jinfo.findex = findex + ofs[i];
 
469
      joint[i]->vtable->getInfo2 (joint[i],&Jinfo);
 
470
      // adjust returned findex values for global index numbering
 
471
      for (j=0; j<info[i].m; j++) {
 
472
        if (findex[ofs[i] + j] >= 0) findex[ofs[i] + j] += ofs[i];
 
473
      }
 
474
    }
 
475
 
 
476
    // compute A = J*invM*J'
 
477
#   ifdef TIMING
 
478
    dTimerNow ("compute A");
 
479
#   endif
 
480
    dReal *JinvM = (dReal*) ALLOCA (m*nskip*sizeof(dReal));
 
481
    dSetZero (JinvM,m*nskip);
 
482
    dMultiply0 (JinvM,J,invM,m,n6,n6);
 
483
    int mskip = dPAD(m);
 
484
    dReal *A = (dReal*) ALLOCA (m*mskip*sizeof(dReal));
 
485
    dSetZero (A,m*mskip);
 
486
    dMultiply2 (A,JinvM,J,m,n6,m);
 
487
 
 
488
    // add cfm to the diagonal of A
 
489
    for (i=0; i<m; i++) A[i*mskip+i] += cfm[i] * Jinfo.fps;
 
490
 
 
491
#   ifdef COMPARE_METHODS
 
492
    comparator.nextMatrix (A,m,m,1,"A");
 
493
#   endif
 
494
 
 
495
    // compute `rhs', the right hand side of the equation J*a=c
 
496
#   ifdef TIMING
 
497
    dTimerNow ("compute rhs");
 
498
#   endif
 
499
    dReal *tmp1 = (dReal*) ALLOCA (n6 * sizeof(dReal));
 
500
    dSetZero (tmp1,n6);
 
501
    dMultiply0 (tmp1,invM,fe,n6,n6,1);
 
502
    for (i=0; i<n6; i++) tmp1[i] += v[i]/stepsize;
 
503
    dReal *rhs = (dReal*) ALLOCA (m * sizeof(dReal));
 
504
    dSetZero (rhs,m);
 
505
    dMultiply0 (rhs,J,tmp1,m,n6,1);
 
506
    for (i=0; i<m; i++) rhs[i] = c[i]/stepsize - rhs[i];
 
507
 
 
508
#   ifdef COMPARE_METHODS
 
509
    comparator.nextMatrix (c,m,1,0,"c");
 
510
    comparator.nextMatrix (rhs,m,1,0,"rhs");
 
511
#   endif
 
512
 
 
513
    // solve the LCP problem and get lambda.
 
514
    // this will destroy A but that's okay
 
515
#   ifdef TIMING
 
516
    dTimerNow ("solving LCP problem");
 
517
#   endif
 
518
    dReal *lambda = (dReal*) ALLOCA (m * sizeof(dReal));
 
519
    dReal *residual = (dReal*) ALLOCA (m * sizeof(dReal));
 
520
    dSolveLCP (m,A,lambda,rhs,residual,nub,lo,hi,findex);
 
521
 
 
522
//  OLD WAY - direct factor and solve
 
523
//
 
524
//    // factorize A (L*L'=A)
 
525
//#   ifdef TIMING
 
526
//    dTimerNow ("factorize A");
 
527
//#   endif
 
528
//    dReal *L = (dReal*) ALLOCA (m*mskip*sizeof(dReal));
 
529
//    memcpy (L,A,m*mskip*sizeof(dReal));
 
530
//    if (dFactorCholesky (L,m)==0) dDebug (0,"A is not positive definite");
 
531
//
 
532
//    // compute lambda
 
533
//#   ifdef TIMING
 
534
//    dTimerNow ("compute lambda");
 
535
//#   endif
 
536
//    dReal *lambda = (dReal*) ALLOCA (m * sizeof(dReal));
 
537
//    memcpy (lambda,rhs,m * sizeof(dReal));
 
538
//    dSolveCholesky (L,lambda,m);
 
539
 
 
540
#   ifdef COMPARE_METHODS
 
541
    comparator.nextMatrix (lambda,m,1,0,"lambda");
 
542
#   endif
 
543
 
 
544
    // compute the velocity update `vnew'
 
545
#   ifdef TIMING
 
546
    dTimerNow ("compute velocity update");
 
547
#   endif
 
548
    dMultiply1 (tmp1,J,lambda,n6,m,1);
 
549
    for (i=0; i<n6; i++) tmp1[i] += fe[i];
 
550
    dMultiply0 (vnew,invM,tmp1,n6,n6,1);
 
551
    for (i=0; i<n6; i++) vnew[i] = v[i] + stepsize*vnew[i];
 
552
 
 
553
    // see if the constraint has worked: compute J*vnew and make sure it equals
 
554
    // `c' (to within a certain tolerance).
 
555
#   ifdef TIMING
 
556
    dTimerNow ("verify constraint equation");
 
557
#   endif
 
558
    dMultiply0 (tmp1,J,vnew,m,n6,1);
 
559
    dReal err = 0;
 
560
    for (i=0; i<m; i++) err += dFabs(tmp1[i]-c[i]);
 
561
    printf ("%.6e\n",err);
 
562
  }
 
563
  else {
 
564
    // no constraints
 
565
    dMultiply0 (vnew,invM,fe,n6,n6,1);
 
566
    for (i=0; i<n6; i++) vnew[i] = v[i] + stepsize*vnew[i];
 
567
  }
 
568
 
 
569
# ifdef COMPARE_METHODS
 
570
  comparator.nextMatrix (vnew,n6,1,0,"vnew");
 
571
# endif
 
572
 
 
573
  // apply the velocity update to the bodies
 
574
# ifdef TIMING
 
575
  dTimerNow ("update velocity");
 
576
# endif
 
577
  for (i=0; i<nb; i++) {
 
578
    for (j=0; j<3; j++) body[i]->lvel[j] = vnew[i*6+j];
 
579
    for (j=0; j<3; j++) body[i]->avel[j] = vnew[i*6+3+j];
 
580
  }
 
581
 
 
582
  // update the position and orientation from the new linear/angular velocity
 
583
  // (over the given timestep)
 
584
# ifdef TIMING
 
585
  dTimerNow ("update position");
 
586
# endif
 
587
  for (i=0; i<nb; i++) moveAndRotateBody (body[i],stepsize);
 
588
 
 
589
# ifdef TIMING
 
590
  dTimerNow ("tidy up");
 
591
# endif
 
592
 
 
593
  // zero all force accumulators
 
594
  for (i=0; i<nb; i++) {
 
595
    body[i]->facc[0] = 0;
 
596
    body[i]->facc[1] = 0;
 
597
    body[i]->facc[2] = 0;
 
598
    body[i]->facc[3] = 0;
 
599
    body[i]->tacc[0] = 0;
 
600
    body[i]->tacc[1] = 0;
 
601
    body[i]->tacc[2] = 0;
 
602
    body[i]->tacc[3] = 0;
 
603
  }
 
604
 
 
605
# ifdef TIMING
 
606
  dTimerEnd();
 
607
  if (m > 0) dTimerReport (stdout,1);
 
608
# endif
 
609
}
 
610
 
 
611
//****************************************************************************
 
612
// an optimized version of dInternalStepIsland1()
 
613
 
 
614
void dInternalStepIsland_x2 (dxWorld *world, dxBody * const *body, int nb,
 
615
                             dxJoint * const *_joint, int nj, dReal stepsize)
 
616
{
 
617
  int i,j,k;
 
618
# ifdef TIMING
 
619
  dTimerStart("preprocessing");
 
620
# endif
 
621
 
 
622
  dReal stepsize1 = dRecip(stepsize);
 
623
 
 
624
  // number all bodies in the body list - set their tag values
 
625
  for (i=0; i<nb; i++) body[i]->tag = i;
 
626
 
 
627
  // make a local copy of the joint array, because we might want to modify it.
 
628
  // (the "dxJoint *const*" declaration says we're allowed to modify the joints
 
629
  // but not the joint array, because the caller might need it unchanged).
 
630
  dxJoint **joint = (dxJoint**) ALLOCA (nj * sizeof(dxJoint*));
 
631
  memcpy (joint,_joint,nj * sizeof(dxJoint*));
 
632
 
 
633
  // for all bodies, compute the inertia tensor and its inverse in the global
 
634
  // frame, and compute the rotational force and add it to the torque
 
635
  // accumulator. I and invI are vertically stacked 3x4 matrices, one per body.
 
636
  // @@@ check computation of rotational force.
 
637
  dReal *I = (dReal*) ALLOCA (3*nb*4 * sizeof(dReal));
 
638
  dReal *invI = (dReal*) ALLOCA (3*nb*4 * sizeof(dReal));
 
639
  dSetZero (I,3*nb*4);
 
640
  dSetZero (invI,3*nb*4);
 
641
  for (i=0; i<nb; i++) {
 
642
    dReal tmp[12];
 
643
    // compute inertia tensor in global frame
 
644
    dMULTIPLY2_333 (tmp,body[i]->mass.I,body[i]->R);
 
645
    dMULTIPLY0_333 (I+i*12,body[i]->R,tmp);
 
646
    // compute inverse inertia tensor in global frame
 
647
    dMULTIPLY2_333 (tmp,body[i]->invI,body[i]->R);
 
648
    dMULTIPLY0_333 (invI+i*12,body[i]->R,tmp);
 
649
    // compute rotational force
 
650
    dMULTIPLY0_331 (tmp,I+i*12,body[i]->avel);
 
651
    dCROSS (body[i]->tacc,-=,body[i]->avel,tmp);
 
652
  }
 
653
 
 
654
  // add the gravity force to all bodies
 
655
  for (i=0; i<nb; i++) {
 
656
    if ((body[i]->flags & dxBodyNoGravity)==0) {
 
657
      body[i]->facc[0] += body[i]->mass.mass * world->gravity[0];
 
658
      body[i]->facc[1] += body[i]->mass.mass * world->gravity[1];
 
659
      body[i]->facc[2] += body[i]->mass.mass * world->gravity[2];
 
660
    }
 
661
  }
 
662
 
 
663
  // get m = total constraint dimension, nub = number of unbounded variables.
 
664
  // create constraint offset array and number-of-rows array for all joints.
 
665
  // the constraints are re-ordered as follows: the purely unbounded
 
666
  // constraints, the mixed unbounded + LCP constraints, and last the purely
 
667
  // LCP constraints. this assists the LCP solver to put all unbounded
 
668
  // variables at the start for a quick factorization.
 
669
  //
 
670
  // joints with m=0 are inactive and are removed from the joints array
 
671
  // entirely, so that the code that follows does not consider them.
 
672
  // also number all active joints in the joint list (set their tag values).
 
673
  // inactive joints receive a tag value of -1.
 
674
 
 
675
  int m = 0;
 
676
  dxJoint::Info1 *info = (dxJoint::Info1*) ALLOCA (nj*sizeof(dxJoint::Info1));
 
677
  int *ofs = (int*) ALLOCA (nj*sizeof(int));
 
678
  for (i=0, j=0; j<nj; j++) {   // i=dest, j=src
 
679
    joint[j]->vtable->getInfo1 (joint[j],info+i);
 
680
    dIASSERT (info[i].m >= 0 && info[i].m <= 6 &&
 
681
              info[i].nub >= 0 && info[i].nub <= info[i].m);
 
682
    if (info[i].m > 0) {
 
683
      joint[i] = joint[j];
 
684
      joint[i]->tag = i;
 
685
      i++;
 
686
    }
 
687
    else {
 
688
      joint[j]->tag = -1;
 
689
    }
 
690
  }
 
691
  nj = i;
 
692
 
 
693
  // the purely unbounded constraints
 
694
  for (i=0; i<nj; i++) if (info[i].nub == info[i].m) {
 
695
    ofs[i] = m;
 
696
    m += info[i].m;
 
697
  }
 
698
  int nub = m;
 
699
  // the mixed unbounded + LCP constraints
 
700
  for (i=0; i<nj; i++) if (info[i].nub > 0 && info[i].nub < info[i].m) {
 
701
    ofs[i] = m;
 
702
    m += info[i].m;
 
703
  }
 
704
  // the purely LCP constraints
 
705
  for (i=0; i<nj; i++) if (info[i].nub == 0) {
 
706
    ofs[i] = m;
 
707
    m += info[i].m;
 
708
  }
 
709
 
 
710
  // this will be set to the force due to the constraints
 
711
  dReal *cforce = (dReal*) ALLOCA (nb*8 * sizeof(dReal));
 
712
  dSetZero (cforce,nb*8);
 
713
 
 
714
  // if there are constraints, compute cforce
 
715
  if (m > 0) {
 
716
    // create a constraint equation right hand side vector `c', a constraint
 
717
    // force mixing vector `cfm', and LCP low and high bound vectors, and an
 
718
    // 'findex' vector.
 
719
    dReal *c = (dReal*) ALLOCA (m*sizeof(dReal));
 
720
    dReal *cfm = (dReal*) ALLOCA (m*sizeof(dReal));
 
721
    dReal *lo = (dReal*) ALLOCA (m*sizeof(dReal));
 
722
    dReal *hi = (dReal*) ALLOCA (m*sizeof(dReal));
 
723
    int *findex = (int*) alloca (m*sizeof(int));
 
724
    dSetZero (c,m);
 
725
    dSetValue (cfm,m,world->global_cfm);
 
726
    dSetValue (lo,m,-dInfinity);
 
727
    dSetValue (hi,m, dInfinity);
 
728
    for (i=0; i<m; i++) findex[i] = -1;
 
729
 
 
730
    // get jacobian data from constraints. a (2*m)x8 matrix will be created
 
731
    // to store the two jacobian blocks from each constraint. it has this
 
732
    // format:
 
733
    //
 
734
    //   l l l 0 a a a 0  \ 
 
735
    //   l l l 0 a a a 0   }-- jacobian body 1 block for joint 0 (3 rows)
 
736
    //   l l l 0 a a a 0  /
 
737
    //   l l l 0 a a a 0  \ 
 
738
    //   l l l 0 a a a 0   }-- jacobian body 2 block for joint 0 (3 rows)
 
739
    //   l l l 0 a a a 0  /
 
740
    //   l l l 0 a a a 0  }--- jacobian body 1 block for joint 1 (1 row)
 
741
    //   l l l 0 a a a 0  }--- jacobian body 2 block for joint 1 (1 row)
 
742
    //   etc...
 
743
    //
 
744
    //   (lll) = linear jacobian data
 
745
    //   (aaa) = angular jacobian data
 
746
    //
 
747
#   ifdef TIMING
 
748
    dTimerNow ("create J");
 
749
#   endif
 
750
    dReal *J = (dReal*) ALLOCA (2*m*8*sizeof(dReal));
 
751
    dSetZero (J,2*m*8);
 
752
    dxJoint::Info2 Jinfo;
 
753
    Jinfo.rowskip = 8;
 
754
    Jinfo.fps = stepsize1;
 
755
    Jinfo.erp = world->global_erp;
 
756
    for (i=0; i<nj; i++) {
 
757
      Jinfo.J1l = J + 2*8*ofs[i];
 
758
      Jinfo.J1a = Jinfo.J1l + 4;
 
759
      Jinfo.J2l = Jinfo.J1l + 8*info[i].m;
 
760
      Jinfo.J2a = Jinfo.J2l + 4;
 
761
      Jinfo.c = c + ofs[i];
 
762
      Jinfo.cfm = cfm + ofs[i];
 
763
      Jinfo.lo = lo + ofs[i];
 
764
      Jinfo.hi = hi + ofs[i];
 
765
      Jinfo.findex = findex + ofs[i];
 
766
      joint[i]->vtable->getInfo2 (joint[i],&Jinfo);
 
767
      // adjust returned findex values for global index numbering
 
768
      for (j=0; j<info[i].m; j++) {
 
769
        if (findex[ofs[i] + j] >= 0) findex[ofs[i] + j] += ofs[i];
 
770
      }
 
771
    }
 
772
 
 
773
    // compute A = J*invM*J'. first compute JinvM = J*invM. this has the same
 
774
    // format as J so we just go through the constraints in J multiplying by
 
775
    // the appropriate scalars and matrices.
 
776
#   ifdef TIMING
 
777
    dTimerNow ("compute A");
 
778
#   endif
 
779
    dReal *JinvM = (dReal*) ALLOCA (2*m*8*sizeof(dReal));
 
780
    dSetZero (JinvM,2*m*8);
 
781
    for (i=0; i<nj; i++) {
 
782
      int b = joint[i]->node[0].body->tag;
 
783
      dReal body_invMass = body[b]->invMass;
 
784
      dReal *body_invI = invI + b*12;
 
785
      dReal *Jsrc = J + 2*8*ofs[i];
 
786
      dReal *Jdst = JinvM + 2*8*ofs[i];
 
787
      for (j=info[i].m-1; j>=0; j--) {
 
788
        for (k=0; k<3; k++) Jdst[k] = Jsrc[k] * body_invMass;
 
789
        dMULTIPLY0_133 (Jdst+4,Jsrc+4,body_invI);
 
790
        Jsrc += 8;
 
791
        Jdst += 8;
 
792
      }
 
793
      if (joint[i]->node[1].body) {
 
794
        b = joint[i]->node[1].body->tag;
 
795
        body_invMass = body[b]->invMass;
 
796
        body_invI = invI + b*12;
 
797
        for (j=info[i].m-1; j>=0; j--) {
 
798
          for (k=0; k<3; k++) Jdst[k] = Jsrc[k] * body_invMass;
 
799
          dMULTIPLY0_133 (Jdst+4,Jsrc+4,body_invI);
 
800
          Jsrc += 8;
 
801
          Jdst += 8;
 
802
        }
 
803
      }
 
804
    }
 
805
 
 
806
    // now compute A = JinvM * J'. A's rows and columns are grouped by joint,
 
807
    // i.e. in the same way as the rows of J. block (i,j) of A is only nonzero
 
808
    // if joints i and j have at least one body in common. this fact suggests
 
809
    // the algorithm used to fill A:
 
810
    //
 
811
    //    for b = all bodies
 
812
    //      n = number of joints attached to body b
 
813
    //      for i = 1..n
 
814
    //        for j = i+1..n
 
815
    //          ii = actual joint number for i
 
816
    //          jj = actual joint number for j
 
817
    //          // (ii,jj) will be set to all pairs of joints around body b
 
818
    //          compute blockwise: A(ii,jj) += JinvM(ii) * J(jj)'
 
819
    //
 
820
    // this algorithm catches all pairs of joints that have at least one body
 
821
    // in common. it does not compute the diagonal blocks of A however -
 
822
    // another similar algorithm does that.
 
823
 
 
824
    int mskip = dPAD(m);
 
825
    dReal *A = (dReal*) ALLOCA (m*mskip*sizeof(dReal));
 
826
    dSetZero (A,m*mskip);
 
827
    for (i=0; i<nb; i++) {
 
828
      for (dxJointNode *n1=body[i]->firstjoint; n1; n1=n1->next) {
 
829
        for (dxJointNode *n2=n1->next; n2; n2=n2->next) {
 
830
          // get joint numbers and ensure ofs[j1] >= ofs[j2]
 
831
          int j1 = n1->joint->tag;
 
832
          int j2 = n2->joint->tag;
 
833
          if (ofs[j1] < ofs[j2]) {
 
834
            int tmp = j1;
 
835
            j1 = j2;
 
836
            j2 = tmp;
 
837
          }
 
838
 
 
839
          // if either joint was tagged as -1 then it is an inactive (m=0)
 
840
          // joint that should not be considered
 
841
          if (j1==-1 || j2==-1) continue;
 
842
 
 
843
          // determine if body i is the 1st or 2nd body of joints j1 and j2
 
844
          int jb1 = (joint[j1]->node[1].body == body[i]);
 
845
          int jb2 = (joint[j2]->node[1].body == body[i]);
 
846
          // jb1/jb2 must be 0 for joints with only one body
 
847
          dIASSERT(joint[j1]->node[1].body || jb1==0);
 
848
          dIASSERT(joint[j2]->node[1].body || jb2==0);
 
849
 
 
850
          // set block of A
 
851
          MultiplyAdd2_p8r (A + ofs[j1]*mskip + ofs[j2],
 
852
                            JinvM + 2*8*ofs[j1] + jb1*8*info[j1].m,
 
853
                            J     + 2*8*ofs[j2] + jb2*8*info[j2].m,
 
854
                            info[j1].m,info[j2].m, mskip);
 
855
        }
 
856
      }
 
857
    }
 
858
    // compute diagonal blocks of A
 
859
    for (i=0; i<nj; i++) {
 
860
      Multiply2_p8r (A + ofs[i]*(mskip+1),
 
861
                     JinvM + 2*8*ofs[i],
 
862
                     J + 2*8*ofs[i],
 
863
                     info[i].m,info[i].m, mskip);
 
864
      if (joint[i]->node[1].body) {
 
865
        MultiplyAdd2_p8r (A + ofs[i]*(mskip+1),
 
866
                          JinvM + 2*8*ofs[i] + 8*info[i].m,
 
867
                          J + 2*8*ofs[i] + 8*info[i].m,
 
868
                          info[i].m,info[i].m, mskip);
 
869
      }
 
870
    }
 
871
 
 
872
    // add cfm to the diagonal of A
 
873
    for (i=0; i<m; i++) A[i*mskip+i] += cfm[i] * stepsize1;
 
874
 
 
875
#   ifdef COMPARE_METHODS
 
876
    comparator.nextMatrix (A,m,m,1,"A");
 
877
#   endif
 
878
 
 
879
    // compute the right hand side `rhs'
 
880
#   ifdef TIMING
 
881
    dTimerNow ("compute rhs");
 
882
#   endif
 
883
    dReal *tmp1 = (dReal*) ALLOCA (nb*8 * sizeof(dReal));
 
884
    dSetZero (tmp1,nb*8);
 
885
    // put v/h + invM*fe into tmp1
 
886
    for (i=0; i<nb; i++) {
 
887
      dReal body_invMass = body[i]->invMass;
 
888
      dReal *body_invI = invI + i*12;
 
889
      for (j=0; j<3; j++) tmp1[i*8+j] = body[i]->facc[j] * body_invMass +
 
890
                            body[i]->lvel[j] * stepsize1;
 
891
      dMULTIPLY0_331 (tmp1 + i*8 + 4,body_invI,body[i]->tacc);
 
892
      for (j=0; j<3; j++) tmp1[i*8+4+j] += body[i]->avel[j] * stepsize1;
 
893
    }
 
894
    // put J*tmp1 into rhs
 
895
    dReal *rhs = (dReal*) ALLOCA (m * sizeof(dReal));
 
896
    dSetZero (rhs,m);
 
897
    for (i=0; i<nj; i++) {
 
898
      dReal *JJ = J + 2*8*ofs[i];
 
899
      Multiply0_p81 (rhs+ofs[i],JJ,
 
900
                     tmp1 + 8*joint[i]->node[0].body->tag, info[i].m);
 
901
      if (joint[i]->node[1].body) {
 
902
        MultiplyAdd0_p81 (rhs+ofs[i],JJ + 8*info[i].m,
 
903
                          tmp1 + 8*joint[i]->node[1].body->tag, info[i].m);
 
904
      }
 
905
    }
 
906
    // complete rhs
 
907
    for (i=0; i<m; i++) rhs[i] = c[i]*stepsize1 - rhs[i];
 
908
 
 
909
#   ifdef COMPARE_METHODS
 
910
    comparator.nextMatrix (c,m,1,0,"c");
 
911
    comparator.nextMatrix (rhs,m,1,0,"rhs");
 
912
#   endif
 
913
 
 
914
    // solve the LCP problem and get lambda.
 
915
    // this will destroy A but that's okay
 
916
#   ifdef TIMING
 
917
    dTimerNow ("solving LCP problem");
 
918
#   endif
 
919
    dReal *lambda = (dReal*) ALLOCA (m * sizeof(dReal));
 
920
    dReal *residual = (dReal*) ALLOCA (m * sizeof(dReal));
 
921
    dSolveLCP (m,A,lambda,rhs,residual,nub,lo,hi,findex);
 
922
 
 
923
//  OLD WAY - direct factor and solve
 
924
//
 
925
//    // factorize A (L*L'=A)
 
926
//#   ifdef TIMING
 
927
//    dTimerNow ("factorize A");
 
928
//#   endif
 
929
//    dReal *L = (dReal*) ALLOCA (m*mskip*sizeof(dReal));
 
930
//    memcpy (L,A,m*mskip*sizeof(dReal));
 
931
//#   ifdef FAST_FACTOR
 
932
//    dFastFactorCholesky (L,m);  // does not report non positive definiteness
 
933
//#   else
 
934
//    if (dFactorCholesky (L,m)==0) dDebug (0,"A is not positive definite");
 
935
//#   endif
 
936
//
 
937
//    // compute lambda
 
938
//#   ifdef TIMING
 
939
//    dTimerNow ("compute lambda");
 
940
//#   endif
 
941
//    dReal *lambda = (dReal*) ALLOCA (m * sizeof(dReal));
 
942
//    memcpy (lambda,rhs,m * sizeof(dReal));
 
943
//    dSolveCholesky (L,lambda,m);
 
944
 
 
945
#   ifdef COMPARE_METHODS
 
946
    comparator.nextMatrix (lambda,m,1,0,"lambda");
 
947
#   endif
 
948
 
 
949
    // compute the constraint force `cforce'
 
950
#   ifdef TIMING
 
951
    dTimerNow ("compute constraint force");
 
952
#   endif
 
953
    // compute cforce = J'*lambda
 
954
    for (i=0; i<nj; i++) {
 
955
      dReal *JJ = J + 2*8*ofs[i];
 
956
      dxBody* b1 = joint[i]->node[0].body;
 
957
      dxBody* b2 = joint[i]->node[1].body;
 
958
      dJointFeedback *fb = joint[i]->feedback;
 
959
 
 
960
      if (fb) {
 
961
        // the user has requested feedback on the amount of force that this
 
962
        // joint is applying to the bodies. we use a slightly slower
 
963
        // computation that splits out the force components and puts them
 
964
        // in the feedback structure.
 
965
        dReal data1[8],data2[8];
 
966
        Multiply1_8q1 (data1, JJ, lambda+ofs[i], info[i].m);
 
967
        dReal *cf1 = cforce + 8*b1->tag;
 
968
        cf1[0] += (fb->f1[0] = data1[0]);
 
969
        cf1[1] += (fb->f1[1] = data1[1]);
 
970
        cf1[2] += (fb->f1[2] = data1[2]);
 
971
        cf1[4] += (fb->t1[0] = data1[4]);
 
972
        cf1[5] += (fb->t1[1] = data1[5]);
 
973
        cf1[6] += (fb->t1[2] = data1[6]);
 
974
        if (b2){
 
975
          Multiply1_8q1 (data2, JJ + 8*info[i].m, lambda+ofs[i], info[i].m);
 
976
          dReal *cf2 = cforce + 8*b2->tag;
 
977
          cf2[0] += (fb->f2[0] = data2[0]);
 
978
          cf2[1] += (fb->f2[1] = data2[1]);
 
979
          cf2[2] += (fb->f2[2] = data2[2]);
 
980
          cf2[4] += (fb->t2[0] = data2[4]);
 
981
          cf2[5] += (fb->t2[1] = data2[5]);
 
982
          cf2[6] += (fb->t2[2] = data2[6]);
 
983
        }
 
984
      }
 
985
      else {
 
986
        // no feedback is required, let's compute cforce the faster way
 
987
        MultiplyAdd1_8q1 (cforce + 8*b1->tag,JJ, lambda+ofs[i], info[i].m);
 
988
        if (b2) {
 
989
          MultiplyAdd1_8q1 (cforce + 8*b2->tag,
 
990
                            JJ + 8*info[i].m, lambda+ofs[i], info[i].m);
 
991
        }
 
992
      }
 
993
    }
 
994
  }
 
995
 
 
996
  // compute the velocity update
 
997
# ifdef TIMING
 
998
  dTimerNow ("compute velocity update");
 
999
# endif
 
1000
 
 
1001
  // add fe to cforce
 
1002
  for (i=0; i<nb; i++) {
 
1003
    for (j=0; j<3; j++) cforce[i*8+j] += body[i]->facc[j];
 
1004
    for (j=0; j<3; j++) cforce[i*8+4+j] += body[i]->tacc[j];
 
1005
  }
 
1006
  // multiply cforce by stepsize
 
1007
  for (i=0; i < nb*8; i++) cforce[i] *= stepsize;
 
1008
  // add invM * cforce to the body velocity
 
1009
  for (i=0; i<nb; i++) {
 
1010
    dReal body_invMass = body[i]->invMass;
 
1011
    dReal *body_invI = invI + i*12;
 
1012
    for (j=0; j<3; j++) body[i]->lvel[j] += body_invMass * cforce[i*8+j];
 
1013
    dMULTIPLYADD0_331 (body[i]->avel,body_invI,cforce+i*8+4);
 
1014
  }
 
1015
 
 
1016
  // update the position and orientation from the new linear/angular velocity
 
1017
  // (over the given timestep)
 
1018
# ifdef TIMING
 
1019
  dTimerNow ("update position");
 
1020
# endif
 
1021
  for (i=0; i<nb; i++) moveAndRotateBody (body[i],stepsize);
 
1022
 
 
1023
# ifdef COMPARE_METHODS
 
1024
  dReal *tmp_vnew = (dReal*) ALLOCA (nb*6*sizeof(dReal));
 
1025
  for (i=0; i<nb; i++) {
 
1026
    for (j=0; j<3; j++) tmp_vnew[i*6+j] = body[i]->lvel[j];
 
1027
    for (j=0; j<3; j++) tmp_vnew[i*6+3+j] = body[i]->avel[j];
 
1028
  }
 
1029
  comparator.nextMatrix (tmp_vnew,nb*6,1,0,"vnew");
 
1030
# endif
 
1031
 
 
1032
# ifdef TIMING
 
1033
  dTimerNow ("tidy up");
 
1034
# endif
 
1035
 
 
1036
  // zero all force accumulators
 
1037
  for (i=0; i<nb; i++) {
 
1038
    body[i]->facc[0] = 0;
 
1039
    body[i]->facc[1] = 0;
 
1040
    body[i]->facc[2] = 0;
 
1041
    body[i]->facc[3] = 0;
 
1042
    body[i]->tacc[0] = 0;
 
1043
    body[i]->tacc[1] = 0;
 
1044
    body[i]->tacc[2] = 0;
 
1045
    body[i]->tacc[3] = 0;
 
1046
  }
 
1047
 
 
1048
# ifdef TIMING
 
1049
  dTimerEnd();
 
1050
  if (m > 0) dTimerReport (stdout,1);
 
1051
# endif
 
1052
}
 
1053
 
 
1054
//****************************************************************************
 
1055
 
 
1056
void dInternalStepIsland (dxWorld *world, dxBody * const *body, int nb,
 
1057
                          dxJoint * const *joint, int nj, dReal stepsize)
 
1058
{
 
1059
# ifndef COMPARE_METHODS
 
1060
  dInternalStepIsland_x2 (world,body,nb,joint,nj,stepsize);
 
1061
# endif
 
1062
 
 
1063
# ifdef COMPARE_METHODS
 
1064
  int i;
 
1065
 
 
1066
  // save body state
 
1067
  dxBody *state = (dxBody*) ALLOCA (nb*sizeof(dxBody));
 
1068
  for (i=0; i<nb; i++) memcpy (state+i,body[i],sizeof(dxBody));
 
1069
 
 
1070
  // take slow step
 
1071
  comparator.reset();
 
1072
  dInternalStepIsland_x1 (world,body,nb,joint,nj,stepsize);
 
1073
  comparator.end();
 
1074
 
 
1075
  // restore state
 
1076
  for (i=0; i<nb; i++) memcpy (body[i],state+i,sizeof(dxBody));
 
1077
 
 
1078
  // take fast step
 
1079
  dInternalStepIsland_x2 (world,body,nb,joint,nj,stepsize);
 
1080
  comparator.end();
 
1081
 
 
1082
  //comparator.dump();
 
1083
  //_exit (1);
 
1084
# endif
 
1085
}