aboutsummaryrefslogtreecommitdiffstatshomepage
path: root/libraries/ode-0.9/contrib/BreakableJoints/step.cpp
diff options
context:
space:
mode:
Diffstat (limited to 'libraries/ode-0.9/contrib/BreakableJoints/step.cpp')
-rw-r--r--libraries/ode-0.9/contrib/BreakableJoints/step.cpp1170
1 files changed, 0 insertions, 1170 deletions
diff --git a/libraries/ode-0.9/contrib/BreakableJoints/step.cpp b/libraries/ode-0.9/contrib/BreakableJoints/step.cpp
deleted file mode 100644
index 38aed6c..0000000
--- a/libraries/ode-0.9/contrib/BreakableJoints/step.cpp
+++ /dev/null
@@ -1,1170 +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 * 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"
49dMatrixComparison 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
57static 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
84static 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
111static 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
131static 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
151static 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
179static 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
210static 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
223static 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 // notify all attached geoms that this body has moved
293 for (dxGeom *geom = b->geom; geom; geom = dGeomGetBodyNext (geom))
294 dGeomMoved (geom);
295}
296
297//****************************************************************************
298// the slow, but sure way
299// note that this does not do any joint feedback!
300
301// given lists of bodies and joints that form an island, perform a first
302// order timestep.
303//
304// `body' is the body array, `nb' is the size of the array.
305// `_joint' is the body array, `nj' is the size of the array.
306
307void dInternalStepIsland_x1 (dxWorld *world, dxBody * const *body, int nb,
308 dxJoint * const *_joint, int nj, dReal stepsize)
309{
310 int i,j,k;
311 int n6 = 6*nb;
312
313# ifdef TIMING
314 dTimerStart("preprocessing");
315# endif
316
317 // number all bodies in the body list - set their tag values
318 for (i=0; i<nb; i++) body[i]->tag = i;
319
320 // make a local copy of the joint array, because we might want to modify it.
321 // (the "dxJoint *const*" declaration says we're allowed to modify the joints
322 // but not the joint array, because the caller might need it unchanged).
323 dxJoint **joint = (dxJoint**) ALLOCA (nj * sizeof(dxJoint*));
324 memcpy (joint,_joint,nj * sizeof(dxJoint*));
325
326 // for all bodies, compute the inertia tensor and its inverse in the global
327 // frame, and compute the rotational force and add it to the torque
328 // accumulator.
329 // @@@ check computation of rotational force.
330 dReal *I = (dReal*) ALLOCA (3*nb*4 * sizeof(dReal));
331 dReal *invI = (dReal*) ALLOCA (3*nb*4 * sizeof(dReal));
332
333 //dSetZero (I,3*nb*4);
334 //dSetZero (invI,3*nb*4);
335 for (i=0; i<nb; i++) {
336 dReal tmp[12];
337 // compute inertia tensor in global frame
338 dMULTIPLY2_333 (tmp,body[i]->mass.I,body[i]->R);
339 dMULTIPLY0_333 (I+i*12,body[i]->R,tmp);
340 // compute inverse inertia tensor in global frame
341 dMULTIPLY2_333 (tmp,body[i]->invI,body[i]->R);
342 dMULTIPLY0_333 (invI+i*12,body[i]->R,tmp);
343 // compute rotational force
344 dMULTIPLY0_331 (tmp,I+i*12,body[i]->avel);
345 dCROSS (body[i]->tacc,-=,body[i]->avel,tmp);
346 }
347
348 // add the gravity force to all bodies
349 for (i=0; i<nb; i++) {
350 if ((body[i]->flags & dxBodyNoGravity)==0) {
351 body[i]->facc[0] += body[i]->mass.mass * world->gravity[0];
352 body[i]->facc[1] += body[i]->mass.mass * world->gravity[1];
353 body[i]->facc[2] += body[i]->mass.mass * world->gravity[2];
354 }
355 }
356
357 // get m = total constraint dimension, nub = number of unbounded variables.
358 // create constraint offset array and number-of-rows array for all joints.
359 // the constraints are re-ordered as follows: the purely unbounded
360 // constraints, the mixed unbounded + LCP constraints, and last the purely
361 // LCP constraints.
362 //
363 // joints with m=0 are inactive and are removed from the joints array
364 // entirely, so that the code that follows does not consider them.
365 int m = 0;
366 dxJoint::Info1 *info = (dxJoint::Info1*) ALLOCA (nj*sizeof(dxJoint::Info1));
367 int *ofs = (int*) ALLOCA (nj*sizeof(int));
368 for (i=0, j=0; j<nj; j++) { // i=dest, j=src
369 joint[j]->vtable->getInfo1 (joint[j],info+i);
370 dIASSERT (info[i].m >= 0 && info[i].m <= 6 &&
371 info[i].nub >= 0 && info[i].nub <= info[i].m);
372 if (info[i].m > 0) {
373 joint[i] = joint[j];
374 i++;
375 }
376 }
377 nj = i;
378
379 // the purely unbounded constraints
380 for (i=0; i<nj; i++) if (info[i].nub == info[i].m) {
381 ofs[i] = m;
382 m += info[i].m;
383 }
384 int nub = m;
385 // the mixed unbounded + LCP constraints
386 for (i=0; i<nj; i++) if (info[i].nub > 0 && info[i].nub < info[i].m) {
387 ofs[i] = m;
388 m += info[i].m;
389 }
390 // the purely LCP constraints
391 for (i=0; i<nj; i++) if (info[i].nub == 0) {
392 ofs[i] = m;
393 m += info[i].m;
394 }
395
396 // create (6*nb,6*nb) inverse mass matrix `invM', and fill it with mass
397 // parameters
398# ifdef TIMING
399 dTimerNow ("create mass matrix");
400# endif
401 int nskip = dPAD (n6);
402 dReal *invM = (dReal*) ALLOCA (n6*nskip*sizeof(dReal));
403 dSetZero (invM,n6*nskip);
404 for (i=0; i<nb; i++) {
405 dReal *MM = invM+(i*6)*nskip+(i*6);
406 MM[0] = body[i]->invMass;
407 MM[nskip+1] = body[i]->invMass;
408 MM[2*nskip+2] = body[i]->invMass;
409 MM += 3*nskip+3;
410 for (j=0; j<3; j++) for (k=0; k<3; k++) {
411 MM[j*nskip+k] = invI[i*12+j*4+k];
412 }
413 }
414
415 // assemble some body vectors: fe = external forces, v = velocities
416 dReal *fe = (dReal*) ALLOCA (n6 * sizeof(dReal));
417 dReal *v = (dReal*) ALLOCA (n6 * sizeof(dReal));
418 //dSetZero (fe,n6);
419 //dSetZero (v,n6);
420 for (i=0; i<nb; i++) {
421 for (j=0; j<3; j++) fe[i*6+j] = body[i]->facc[j];
422 for (j=0; j<3; j++) fe[i*6+3+j] = body[i]->tacc[j];
423 for (j=0; j<3; j++) v[i*6+j] = body[i]->lvel[j];
424 for (j=0; j<3; j++) v[i*6+3+j] = body[i]->avel[j];
425 }
426
427 // this will be set to the velocity update
428 dReal *vnew = (dReal*) ALLOCA (n6 * sizeof(dReal));
429 dSetZero (vnew,n6);
430
431 // if there are constraints, compute cforce
432 if (m > 0) {
433 // create a constraint equation right hand side vector `c', a constraint
434 // force mixing vector `cfm', and LCP low and high bound vectors, and an
435 // 'findex' vector.
436 dReal *c = (dReal*) ALLOCA (m*sizeof(dReal));
437 dReal *cfm = (dReal*) ALLOCA (m*sizeof(dReal));
438 dReal *lo = (dReal*) ALLOCA (m*sizeof(dReal));
439 dReal *hi = (dReal*) ALLOCA (m*sizeof(dReal));
440 int *findex = (int*) alloca (m*sizeof(int));
441 dSetZero (c,m);
442 dSetValue (cfm,m,world->global_cfm);
443 dSetValue (lo,m,-dInfinity);
444 dSetValue (hi,m, dInfinity);
445 for (i=0; i<m; i++) findex[i] = -1;
446
447 // create (m,6*nb) jacobian mass matrix `J', and fill it with constraint
448 // data. also fill the c vector.
449# ifdef TIMING
450 dTimerNow ("create J");
451# endif
452 dReal *J = (dReal*) ALLOCA (m*nskip*sizeof(dReal));
453 dSetZero (J,m*nskip);
454 dxJoint::Info2 Jinfo;
455 Jinfo.rowskip = nskip;
456 Jinfo.fps = dRecip(stepsize);
457 Jinfo.erp = world->global_erp;
458 for (i=0; i<nj; i++) {
459 Jinfo.J1l = J + nskip*ofs[i] + 6*joint[i]->node[0].body->tag;
460 Jinfo.J1a = Jinfo.J1l + 3;
461 if (joint[i]->node[1].body) {
462 Jinfo.J2l = J + nskip*ofs[i] + 6*joint[i]->node[1].body->tag;
463 Jinfo.J2a = Jinfo.J2l + 3;
464 }
465 else {
466 Jinfo.J2l = 0;
467 Jinfo.J2a = 0;
468 }
469 Jinfo.c = c + ofs[i];
470 Jinfo.cfm = cfm + ofs[i];
471 Jinfo.lo = lo + ofs[i];
472 Jinfo.hi = hi + ofs[i];
473 Jinfo.findex = findex + ofs[i];
474 joint[i]->vtable->getInfo2 (joint[i],&Jinfo);
475 // adjust returned findex values for global index numbering
476 for (j=0; j<info[i].m; j++) {
477 if (findex[ofs[i] + j] >= 0) findex[ofs[i] + j] += ofs[i];
478 }
479 }
480
481 // compute A = J*invM*J'
482# ifdef TIMING
483 dTimerNow ("compute A");
484# endif
485 dReal *JinvM = (dReal*) ALLOCA (m*nskip*sizeof(dReal));
486 //dSetZero (JinvM,m*nskip);
487 dMultiply0 (JinvM,J,invM,m,n6,n6);
488 int mskip = dPAD(m);
489 dReal *A = (dReal*) ALLOCA (m*mskip*sizeof(dReal));
490 //dSetZero (A,m*mskip);
491 dMultiply2 (A,JinvM,J,m,n6,m);
492
493 // add cfm to the diagonal of A
494 for (i=0; i<m; i++) A[i*mskip+i] += cfm[i] * Jinfo.fps;
495
496# ifdef COMPARE_METHODS
497 comparator.nextMatrix (A,m,m,1,"A");
498# endif
499
500 // compute `rhs', the right hand side of the equation J*a=c
501# ifdef TIMING
502 dTimerNow ("compute rhs");
503# endif
504 dReal *tmp1 = (dReal*) ALLOCA (n6 * sizeof(dReal));
505 //dSetZero (tmp1,n6);
506 dMultiply0 (tmp1,invM,fe,n6,n6,1);
507 for (i=0; i<n6; i++) tmp1[i] += v[i]/stepsize;
508 dReal *rhs = (dReal*) ALLOCA (m * sizeof(dReal));
509 //dSetZero (rhs,m);
510 dMultiply0 (rhs,J,tmp1,m,n6,1);
511 for (i=0; i<m; i++) rhs[i] = c[i]/stepsize - rhs[i];
512
513# ifdef COMPARE_METHODS
514 comparator.nextMatrix (c,m,1,0,"c");
515 comparator.nextMatrix (rhs,m,1,0,"rhs");
516# endif
517
518 // solve the LCP problem and get lambda.
519 // this will destroy A but that's okay
520# ifdef TIMING
521 dTimerNow ("solving LCP problem");
522# endif
523 dReal *lambda = (dReal*) ALLOCA (m * sizeof(dReal));
524 dReal *residual = (dReal*) ALLOCA (m * sizeof(dReal));
525 dSolveLCP (m,A,lambda,rhs,residual,nub,lo,hi,findex);
526
527// OLD WAY - direct factor and solve
528//
529// // factorize A (L*L'=A)
530//# ifdef TIMING
531// dTimerNow ("factorize A");
532//# endif
533// dReal *L = (dReal*) ALLOCA (m*mskip*sizeof(dReal));
534// memcpy (L,A,m*mskip*sizeof(dReal));
535// if (dFactorCholesky (L,m)==0) dDebug (0,"A is not positive definite");
536//
537// // compute lambda
538//# ifdef TIMING
539// dTimerNow ("compute lambda");
540//# endif
541// dReal *lambda = (dReal*) ALLOCA (m * sizeof(dReal));
542// memcpy (lambda,rhs,m * sizeof(dReal));
543// dSolveCholesky (L,lambda,m);
544
545# ifdef COMPARE_METHODS
546 comparator.nextMatrix (lambda,m,1,0,"lambda");
547# endif
548
549 // compute the velocity update `vnew'
550# ifdef TIMING
551 dTimerNow ("compute velocity update");
552# endif
553 dMultiply1 (tmp1,J,lambda,n6,m,1);
554 for (i=0; i<n6; i++) tmp1[i] += fe[i];
555 dMultiply0 (vnew,invM,tmp1,n6,n6,1);
556 for (i=0; i<n6; i++) vnew[i] = v[i] + stepsize*vnew[i];
557
558 // see if the constraint has worked: compute J*vnew and make sure it equals
559 // `c' (to within a certain tolerance).
560# ifdef TIMING
561 dTimerNow ("verify constraint equation");
562# endif
563 dMultiply0 (tmp1,J,vnew,m,n6,1);
564 dReal err = 0;
565 for (i=0; i<m; i++) err += dFabs(tmp1[i]-c[i]);
566 printf ("%.6e\n",err);
567 }
568 else {
569 // no constraints
570 dMultiply0 (vnew,invM,fe,n6,n6,1);
571 for (i=0; i<n6; i++) vnew[i] = v[i] + stepsize*vnew[i];
572 }
573
574# ifdef COMPARE_METHODS
575 comparator.nextMatrix (vnew,n6,1,0,"vnew");
576# endif
577
578 // apply the velocity update to the bodies
579# ifdef TIMING
580 dTimerNow ("update velocity");
581# endif
582 for (i=0; i<nb; i++) {
583 for (j=0; j<3; j++) body[i]->lvel[j] = vnew[i*6+j];
584 for (j=0; j<3; j++) body[i]->avel[j] = vnew[i*6+3+j];
585 }
586
587 // update the position and orientation from the new linear/angular velocity
588 // (over the given timestep)
589# ifdef TIMING
590 dTimerNow ("update position");
591# endif
592 for (i=0; i<nb; i++) moveAndRotateBody (body[i],stepsize);
593
594# ifdef TIMING
595 dTimerNow ("tidy up");
596# endif
597
598 // zero all force accumulators
599 for (i=0; i<nb; i++) {
600 body[i]->facc[0] = 0;
601 body[i]->facc[1] = 0;
602 body[i]->facc[2] = 0;
603 body[i]->facc[3] = 0;
604 body[i]->tacc[0] = 0;
605 body[i]->tacc[1] = 0;
606 body[i]->tacc[2] = 0;
607 body[i]->tacc[3] = 0;
608 }
609
610# ifdef TIMING
611 dTimerEnd();
612 if (m > 0) dTimerReport (stdout,1);
613# endif
614}
615
616//****************************************************************************
617// an optimized version of dInternalStepIsland1()
618
619void dInternalStepIsland_x2 (dxWorld *world, dxBody * const *body, int nb,
620 dxJoint * const *_joint, int nj, dReal stepsize)
621{
622 int i,j,k;
623# ifdef TIMING
624 dTimerStart("preprocessing");
625# endif
626
627 dReal stepsize1 = dRecip(stepsize);
628
629 // number all bodies in the body list - set their tag values
630 for (i=0; i<nb; i++) body[i]->tag = i;
631
632 // make a local copy of the joint array, because we might want to modify it.
633 // (the "dxJoint *const*" declaration says we're allowed to modify the joints
634 // but not the joint array, because the caller might need it unchanged).
635 dxJoint **joint = (dxJoint**) ALLOCA (nj * sizeof(dxJoint*));
636 memcpy (joint,_joint,nj * sizeof(dxJoint*));
637
638 // for all bodies, compute the inertia tensor and its inverse in the global
639 // frame, and compute the rotational force and add it to the torque
640 // accumulator. I and invI are vertically stacked 3x4 matrices, one per body.
641 // @@@ check computation of rotational force.
642 dReal *I = (dReal*) ALLOCA (3*nb*4 * sizeof(dReal));
643 dReal *invI = (dReal*) ALLOCA (3*nb*4 * sizeof(dReal));
644
645 //dSetZero (I,3*nb*4);
646 //dSetZero (invI,3*nb*4);
647 for (i=0; i<nb; i++) {
648 dReal tmp[12];
649 // compute inertia tensor in global frame
650 dMULTIPLY2_333 (tmp,body[i]->mass.I,body[i]->R);
651 dMULTIPLY0_333 (I+i*12,body[i]->R,tmp);
652 // compute inverse inertia tensor in global frame
653 dMULTIPLY2_333 (tmp,body[i]->invI,body[i]->R);
654 dMULTIPLY0_333 (invI+i*12,body[i]->R,tmp);
655 // compute rotational force
656 dMULTIPLY0_331 (tmp,I+i*12,body[i]->avel);
657 dCROSS (body[i]->tacc,-=,body[i]->avel,tmp);
658 }
659
660 // add the gravity force to all bodies
661 for (i=0; i<nb; i++) {
662 if ((body[i]->flags & dxBodyNoGravity)==0) {
663 body[i]->facc[0] += body[i]->mass.mass * world->gravity[0];
664 body[i]->facc[1] += body[i]->mass.mass * world->gravity[1];
665 body[i]->facc[2] += body[i]->mass.mass * world->gravity[2];
666 }
667 }
668
669 // get m = total constraint dimension, nub = number of unbounded variables.
670 // create constraint offset array and number-of-rows array for all joints.
671 // the constraints are re-ordered as follows: the purely unbounded
672 // constraints, the mixed unbounded + LCP constraints, and last the purely
673 // LCP constraints. this assists the LCP solver to put all unbounded
674 // variables at the start for a quick factorization.
675 //
676 // joints with m=0 are inactive and are removed from the joints array
677 // entirely, so that the code that follows does not consider them.
678 // also number all active joints in the joint list (set their tag values).
679 // inactive joints receive a tag value of -1.
680
681 int m = 0;
682 dxJoint::Info1 *info = (dxJoint::Info1*) ALLOCA (nj*sizeof(dxJoint::Info1));
683 int *ofs = (int*) ALLOCA (nj*sizeof(int));
684 for (i=0, j=0; j<nj; j++) { // i=dest, j=src
685 joint[j]->vtable->getInfo1 (joint[j],info+i);
686 dIASSERT (info[i].m >= 0 && info[i].m <= 6 &&
687 info[i].nub >= 0 && info[i].nub <= info[i].m);
688 if (info[i].m > 0) {
689 joint[i] = joint[j];
690 joint[i]->tag = i;
691 i++;
692 }
693 else {
694 joint[j]->tag = -1;
695 }
696 }
697 nj = i;
698
699 // the purely unbounded constraints
700 for (i=0; i<nj; i++) if (info[i].nub == info[i].m) {
701 ofs[i] = m;
702 m += info[i].m;
703 }
704 int nub = m;
705 // the mixed unbounded + LCP constraints
706 for (i=0; i<nj; i++) if (info[i].nub > 0 && info[i].nub < info[i].m) {
707 ofs[i] = m;
708 m += info[i].m;
709 }
710 // the purely LCP constraints
711 for (i=0; i<nj; i++) if (info[i].nub == 0) {
712 ofs[i] = m;
713 m += info[i].m;
714 }
715
716 // this will be set to the force due to the constraints
717 dReal *cforce = (dReal*) ALLOCA (nb*8 * sizeof(dReal));
718 dSetZero (cforce,nb*8);
719
720 // if there are constraints, compute cforce
721 if (m > 0) {
722 // create a constraint equation right hand side vector `c', a constraint
723 // force mixing vector `cfm', and LCP low and high bound vectors, and an
724 // 'findex' vector.
725 dReal *c = (dReal*) ALLOCA (m*sizeof(dReal));
726 dReal *cfm = (dReal*) ALLOCA (m*sizeof(dReal));
727 dReal *lo = (dReal*) ALLOCA (m*sizeof(dReal));
728 dReal *hi = (dReal*) ALLOCA (m*sizeof(dReal));
729 int *findex = (int*) alloca (m*sizeof(int));
730 dSetZero (c,m);
731 dSetValue (cfm,m,world->global_cfm);
732 dSetValue (lo,m,-dInfinity);
733 dSetValue (hi,m, dInfinity);
734 for (i=0; i<m; i++) findex[i] = -1;
735
736 // get jacobian data from constraints. a (2*m)x8 matrix will be created
737 // to store the two jacobian blocks from each constraint. it has this
738 // format:
739 //
740 // l l l 0 a a a 0 \ .
741 // l l l 0 a a a 0 }-- jacobian body 1 block for joint 0 (3 rows)
742 // l l l 0 a a a 0 /
743 // l l l 0 a a a 0 \ .
744 // l l l 0 a a a 0 }-- jacobian body 2 block for joint 0 (3 rows)
745 // l l l 0 a a a 0 /
746 // l l l 0 a a a 0 }--- jacobian body 1 block for joint 1 (1 row)
747 // l l l 0 a a a 0 }--- jacobian body 2 block for joint 1 (1 row)
748 // etc...
749 //
750 // (lll) = linear jacobian data
751 // (aaa) = angular jacobian data
752 //
753# ifdef TIMING
754 dTimerNow ("create J");
755# endif
756 dReal *J = (dReal*) ALLOCA (2*m*8*sizeof(dReal));
757 dSetZero (J,2*m*8);
758 dxJoint::Info2 Jinfo;
759 Jinfo.rowskip = 8;
760 Jinfo.fps = stepsize1;
761 Jinfo.erp = world->global_erp;
762 for (i=0; i<nj; i++) {
763 Jinfo.J1l = J + 2*8*ofs[i];
764 Jinfo.J1a = Jinfo.J1l + 4;
765 Jinfo.J2l = Jinfo.J1l + 8*info[i].m;
766 Jinfo.J2a = Jinfo.J2l + 4;
767 Jinfo.c = c + ofs[i];
768 Jinfo.cfm = cfm + ofs[i];
769 Jinfo.lo = lo + ofs[i];
770 Jinfo.hi = hi + ofs[i];
771 Jinfo.findex = findex + ofs[i];
772 joint[i]->vtable->getInfo2 (joint[i],&Jinfo);
773 // adjust returned findex values for global index numbering
774 for (j=0; j<info[i].m; j++) {
775 if (findex[ofs[i] + j] >= 0) findex[ofs[i] + j] += ofs[i];
776 }
777 }
778
779 // compute A = J*invM*J'. first compute JinvM = J*invM. this has the same
780 // format as J so we just go through the constraints in J multiplying by
781 // the appropriate scalars and matrices.
782# ifdef TIMING
783 dTimerNow ("compute A");
784# endif
785 dReal *JinvM = (dReal*) ALLOCA (2*m*8*sizeof(dReal));
786 dSetZero (JinvM,2*m*8);
787 for (i=0; i<nj; i++) {
788 int b = joint[i]->node[0].body->tag;
789 dReal body_invMass = body[b]->invMass;
790 dReal *body_invI = invI + b*12;
791 dReal *Jsrc = J + 2*8*ofs[i];
792 dReal *Jdst = JinvM + 2*8*ofs[i];
793 for (j=info[i].m-1; j>=0; j--) {
794 for (k=0; k<3; k++) Jdst[k] = Jsrc[k] * body_invMass;
795 dMULTIPLY0_133 (Jdst+4,Jsrc+4,body_invI);
796 Jsrc += 8;
797 Jdst += 8;
798 }
799 if (joint[i]->node[1].body) {
800 b = joint[i]->node[1].body->tag;
801 body_invMass = body[b]->invMass;
802 body_invI = invI + b*12;
803 for (j=info[i].m-1; j>=0; j--) {
804 for (k=0; k<3; k++) Jdst[k] = Jsrc[k] * body_invMass;
805 dMULTIPLY0_133 (Jdst+4,Jsrc+4,body_invI);
806 Jsrc += 8;
807 Jdst += 8;
808 }
809 }
810 }
811
812 // now compute A = JinvM * J'. A's rows and columns are grouped by joint,
813 // i.e. in the same way as the rows of J. block (i,j) of A is only nonzero
814 // if joints i and j have at least one body in common. this fact suggests
815 // the algorithm used to fill A:
816 //
817 // for b = all bodies
818 // n = number of joints attached to body b
819 // for i = 1..n
820 // for j = i+1..n
821 // ii = actual joint number for i
822 // jj = actual joint number for j
823 // // (ii,jj) will be set to all pairs of joints around body b
824 // compute blockwise: A(ii,jj) += JinvM(ii) * J(jj)'
825 //
826 // this algorithm catches all pairs of joints that have at least one body
827 // in common. it does not compute the diagonal blocks of A however -
828 // another similar algorithm does that.
829
830 int mskip = dPAD(m);
831 dReal *A = (dReal*) ALLOCA (m*mskip*sizeof(dReal));
832 dSetZero (A,m*mskip);
833 for (i=0; i<nb; i++) {
834 for (dxJointNode *n1=body[i]->firstjoint; n1; n1=n1->next) {
835 for (dxJointNode *n2=n1->next; n2; n2=n2->next) {
836 // get joint numbers and ensure ofs[j1] >= ofs[j2]
837 int j1 = n1->joint->tag;
838 int j2 = n2->joint->tag;
839 if (ofs[j1] < ofs[j2]) {
840 int tmp = j1;
841 j1 = j2;
842 j2 = tmp;
843 }
844
845 // if either joint was tagged as -1 then it is an inactive (m=0)
846 // joint that should not be considered
847 if (j1==-1 || j2==-1) continue;
848
849 // determine if body i is the 1st or 2nd body of joints j1 and j2
850 int jb1 = (joint[j1]->node[1].body == body[i]);
851 int jb2 = (joint[j2]->node[1].body == body[i]);
852 // jb1/jb2 must be 0 for joints with only one body
853 dIASSERT(joint[j1]->node[1].body || jb1==0);
854 dIASSERT(joint[j2]->node[1].body || jb2==0);
855
856 // set block of A
857 MultiplyAdd2_p8r (A + ofs[j1]*mskip + ofs[j2],
858 JinvM + 2*8*ofs[j1] + jb1*8*info[j1].m,
859 J + 2*8*ofs[j2] + jb2*8*info[j2].m,
860 info[j1].m,info[j2].m, mskip);
861 }
862 }
863 }
864 // compute diagonal blocks of A
865 for (i=0; i<nj; i++) {
866 Multiply2_p8r (A + ofs[i]*(mskip+1),
867 JinvM + 2*8*ofs[i],
868 J + 2*8*ofs[i],
869 info[i].m,info[i].m, mskip);
870 if (joint[i]->node[1].body) {
871 MultiplyAdd2_p8r (A + ofs[i]*(mskip+1),
872 JinvM + 2*8*ofs[i] + 8*info[i].m,
873 J + 2*8*ofs[i] + 8*info[i].m,
874 info[i].m,info[i].m, mskip);
875 }
876 }
877
878 // add cfm to the diagonal of A
879 for (i=0; i<m; i++) A[i*mskip+i] += cfm[i] * stepsize1;
880
881# ifdef COMPARE_METHODS
882 comparator.nextMatrix (A,m,m,1,"A");
883# endif
884
885 // compute the right hand side `rhs'
886# ifdef TIMING
887 dTimerNow ("compute rhs");
888# endif
889 dReal *tmp1 = (dReal*) ALLOCA (nb*8 * sizeof(dReal));
890 //dSetZero (tmp1,nb*8);
891 // put v/h + invM*fe into tmp1
892 for (i=0; i<nb; i++) {
893 dReal body_invMass = body[i]->invMass;
894 dReal *body_invI = invI + i*12;
895 for (j=0; j<3; j++) tmp1[i*8+j] = body[i]->facc[j] * body_invMass +
896 body[i]->lvel[j] * stepsize1;
897 dMULTIPLY0_331 (tmp1 + i*8 + 4,body_invI,body[i]->tacc);
898 for (j=0; j<3; j++) tmp1[i*8+4+j] += body[i]->avel[j] * stepsize1;
899 }
900 // put J*tmp1 into rhs
901 dReal *rhs = (dReal*) ALLOCA (m * sizeof(dReal));
902 //dSetZero (rhs,m);
903 for (i=0; i<nj; i++) {
904 dReal *JJ = J + 2*8*ofs[i];
905 Multiply0_p81 (rhs+ofs[i],JJ,
906 tmp1 + 8*joint[i]->node[0].body->tag, info[i].m);
907 if (joint[i]->node[1].body) {
908 MultiplyAdd0_p81 (rhs+ofs[i],JJ + 8*info[i].m,
909 tmp1 + 8*joint[i]->node[1].body->tag, info[i].m);
910 }
911 }
912 // complete rhs
913 for (i=0; i<m; i++) rhs[i] = c[i]*stepsize1 - rhs[i];
914
915# ifdef COMPARE_METHODS
916 comparator.nextMatrix (c,m,1,0,"c");
917 comparator.nextMatrix (rhs,m,1,0,"rhs");
918# endif
919
920 // solve the LCP problem and get lambda.
921 // this will destroy A but that's okay
922# ifdef TIMING
923 dTimerNow ("solving LCP problem");
924# endif
925 dReal *lambda = (dReal*) ALLOCA (m * sizeof(dReal));
926 dReal *residual = (dReal*) ALLOCA (m * sizeof(dReal));
927 dSolveLCP (m,A,lambda,rhs,residual,nub,lo,hi,findex);
928
929// OLD WAY - direct factor and solve
930//
931// // factorize A (L*L'=A)
932//# ifdef TIMING
933// dTimerNow ("factorize A");
934//# endif
935// dReal *L = (dReal*) ALLOCA (m*mskip*sizeof(dReal));
936// memcpy (L,A,m*mskip*sizeof(dReal));
937//# ifdef FAST_FACTOR
938// dFastFactorCholesky (L,m); // does not report non positive definiteness
939//# else
940// if (dFactorCholesky (L,m)==0) dDebug (0,"A is not positive definite");
941//# endif
942//
943// // compute lambda
944//# ifdef TIMING
945// dTimerNow ("compute lambda");
946//# endif
947// dReal *lambda = (dReal*) ALLOCA (m * sizeof(dReal));
948// memcpy (lambda,rhs,m * sizeof(dReal));
949// dSolveCholesky (L,lambda,m);
950
951# ifdef COMPARE_METHODS
952 comparator.nextMatrix (lambda,m,1,0,"lambda");
953# endif
954
955 // compute the constraint force `cforce'
956# ifdef TIMING
957 dTimerNow ("compute constraint force");
958# endif
959 // compute cforce = J'*lambda
960 for (i=0; i<nj; i++) {
961 dReal *JJ = J + 2*8*ofs[i];
962 dxBody* b1 = joint[i]->node[0].body;
963 dxBody* b2 = joint[i]->node[1].body;
964 dJointFeedback *fb = joint[i]->feedback;
965
966/******************** breakable joint contribution ***********************/
967 // this saves us a few dereferences
968 dxJointBreakInfo *jBI = joint[i]->breakInfo;
969 // we need joint feedback if the joint is breakable or if the user
970 // requested feedback.
971 if (jBI||fb) {
972 // we need feedback on the amount of force that this joint is
973 // applying to the bodies. we use a slightly slower computation
974 // that splits out the force components and puts them in the
975 // feedback structure.
976 dJointFeedback temp_fb; // temporary storage for joint feedback
977 dReal data1[8],data2[8];
978 Multiply1_8q1 (data1, JJ, lambda+ofs[i], info[i].m);
979 dReal *cf1 = cforce + 8*b1->tag;
980 cf1[0] += (temp_fb.f1[0] = data1[0]);
981 cf1[1] += (temp_fb.f1[1] = data1[1]);
982 cf1[2] += (temp_fb.f1[2] = data1[2]);
983 cf1[4] += (temp_fb.t1[0] = data1[4]);
984 cf1[5] += (temp_fb.t1[1] = data1[5]);
985 cf1[6] += (temp_fb.t1[2] = data1[6]);
986 if (b2) {
987 Multiply1_8q1 (data2, JJ + 8*info[i].m, lambda+ofs[i], info[i].m);
988 dReal *cf2 = cforce + 8*b2->tag;
989 cf2[0] += (temp_fb.f2[0] = data2[0]);
990 cf2[1] += (temp_fb.f2[1] = data2[1]);
991 cf2[2] += (temp_fb.f2[2] = data2[2]);
992 cf2[4] += (temp_fb.t2[0] = data2[4]);
993 cf2[5] += (temp_fb.t2[1] = data2[5]);
994 cf2[6] += (temp_fb.t2[2] = data2[6]);
995 }
996 // if the user requested so we must copy the feedback information to
997 // the feedback struct that the user suplied.
998 if (fb) {
999 // copy temp_fb to fb
1000 fb->f1[0] = temp_fb.f1[0];
1001 fb->f1[1] = temp_fb.f1[1];
1002 fb->f1[2] = temp_fb.f1[2];
1003 fb->t1[0] = temp_fb.t1[0];
1004 fb->t1[1] = temp_fb.t1[1];
1005 fb->t1[2] = temp_fb.t1[2];
1006 if (b2) {
1007 fb->f2[0] = temp_fb.f2[0];
1008 fb->f2[1] = temp_fb.f2[1];
1009 fb->f2[2] = temp_fb.f2[2];
1010 fb->t2[0] = temp_fb.t2[0];
1011 fb->t2[1] = temp_fb.t2[1];
1012 fb->t2[2] = temp_fb.t2[2];
1013 }
1014 }
1015 // if the joint is breakable we need to check the breaking conditions
1016 if (jBI) {
1017 dReal relCF1[3];
1018 dReal relCT1[3];
1019 // multiply the force and torque vectors by the rotation matrix of body 1
1020 dMULTIPLY1_331 (&relCF1[0],b1->R,&temp_fb.f1[0]);
1021 dMULTIPLY1_331 (&relCT1[0],b1->R,&temp_fb.t1[0]);
1022 if (jBI->flags & dJOINT_BREAK_AT_B1_FORCE) {
1023 // check if the force is to high
1024 for (int i = 0; i < 3; i++) {
1025 if (relCF1[i] > jBI->b1MaxF[i]) {
1026 jBI->flags |= dJOINT_BROKEN;
1027 goto doneCheckingBreaks;
1028 }
1029 }
1030 }
1031 if (jBI->flags & dJOINT_BREAK_AT_B1_TORQUE) {
1032 // check if the torque is to high
1033 for (int i = 0; i < 3; i++) {
1034 if (relCT1[i] > jBI->b1MaxT[i]) {
1035 jBI->flags |= dJOINT_BROKEN;
1036 goto doneCheckingBreaks;
1037 }
1038 }
1039 }
1040 if (b2) {
1041 dReal relCF2[3];
1042 dReal relCT2[3];
1043 // multiply the force and torque vectors by the rotation matrix of body 2
1044 dMULTIPLY1_331 (&relCF2[0],b2->R,&temp_fb.f2[0]);
1045 dMULTIPLY1_331 (&relCT2[0],b2->R,&temp_fb.t2[0]);
1046 if (jBI->flags & dJOINT_BREAK_AT_B2_FORCE) {
1047 // check if the force is to high
1048 for (int i = 0; i < 3; i++) {
1049 if (relCF2[i] > jBI->b2MaxF[i]) {
1050 jBI->flags |= dJOINT_BROKEN;
1051 goto doneCheckingBreaks;
1052 }
1053 }
1054 }
1055 if (jBI->flags & dJOINT_BREAK_AT_B2_TORQUE) {
1056 // check if the torque is to high
1057 for (int i = 0; i < 3; i++) {
1058 if (relCT2[i] > jBI->b2MaxT[i]) {
1059 jBI->flags |= dJOINT_BROKEN;
1060 goto doneCheckingBreaks;
1061 }
1062 }
1063 }
1064 }
1065 doneCheckingBreaks:
1066 ;
1067 }
1068 }
1069/*************************************************************************/
1070 else {
1071 // no feedback is required, let's compute cforce the faster way
1072 MultiplyAdd1_8q1 (cforce + 8*b1->tag,JJ, lambda+ofs[i], info[i].m);
1073 if (b2) {
1074 MultiplyAdd1_8q1 (cforce + 8*b2->tag,
1075 JJ + 8*info[i].m, lambda+ofs[i], info[i].m);
1076 }
1077 }
1078 }
1079 }
1080
1081 // compute the velocity update
1082# ifdef TIMING
1083 dTimerNow ("compute velocity update");
1084# endif
1085
1086 // add fe to cforce
1087 for (i=0; i<nb; i++) {
1088 for (j=0; j<3; j++) cforce[i*8+j] += body[i]->facc[j];
1089 for (j=0; j<3; j++) cforce[i*8+4+j] += body[i]->tacc[j];
1090 }
1091 // multiply cforce by stepsize
1092 for (i=0; i < nb*8; i++) cforce[i] *= stepsize;
1093 // add invM * cforce to the body velocity
1094 for (i=0; i<nb; i++) {
1095 dReal body_invMass = body[i]->invMass;
1096 dReal *body_invI = invI + i*12;
1097 for (j=0; j<3; j++) body[i]->lvel[j] += body_invMass * cforce[i*8+j];
1098 dMULTIPLYADD0_331 (body[i]->avel,body_invI,cforce+i*8+4);
1099 }
1100
1101 // update the position and orientation from the new linear/angular velocity
1102 // (over the given timestep)
1103# ifdef TIMING
1104 dTimerNow ("update position");
1105# endif
1106 for (i=0; i<nb; i++) moveAndRotateBody (body[i],stepsize);
1107
1108# ifdef COMPARE_METHODS
1109 dReal *tmp_vnew = (dReal*) ALLOCA (nb*6*sizeof(dReal));
1110 for (i=0; i<nb; i++) {
1111 for (j=0; j<3; j++) tmp_vnew[i*6+j] = body[i]->lvel[j];
1112 for (j=0; j<3; j++) tmp_vnew[i*6+3+j] = body[i]->avel[j];
1113 }
1114 comparator.nextMatrix (tmp_vnew,nb*6,1,0,"vnew");
1115# endif
1116
1117# ifdef TIMING
1118 dTimerNow ("tidy up");
1119# endif
1120
1121 // zero all force accumulators
1122 for (i=0; i<nb; i++) {
1123 body[i]->facc[0] = 0;
1124 body[i]->facc[1] = 0;
1125 body[i]->facc[2] = 0;
1126 body[i]->facc[3] = 0;
1127 body[i]->tacc[0] = 0;
1128 body[i]->tacc[1] = 0;
1129 body[i]->tacc[2] = 0;
1130 body[i]->tacc[3] = 0;
1131 }
1132
1133# ifdef TIMING
1134 dTimerEnd();
1135 if (m > 0) dTimerReport (stdout,1);
1136# endif
1137}
1138
1139//****************************************************************************
1140
1141void dInternalStepIsland (dxWorld *world, dxBody * const *body, int nb,
1142 dxJoint * const *joint, int nj, dReal stepsize)
1143{
1144# ifndef COMPARE_METHODS
1145 dInternalStepIsland_x2 (world,body,nb,joint,nj,stepsize);
1146# endif
1147
1148# ifdef COMPARE_METHODS
1149 int i;
1150
1151 // save body state
1152 dxBody *state = (dxBody*) ALLOCA (nb*sizeof(dxBody));
1153 for (i=0; i<nb; i++) memcpy (state+i,body[i],sizeof(dxBody));
1154
1155 // take slow step
1156 comparator.reset();
1157 dInternalStepIsland_x1 (world,body,nb,joint,nj,stepsize);
1158 comparator.end();
1159
1160 // restore state
1161 for (i=0; i<nb; i++) memcpy (body[i],state+i,sizeof(dxBody));
1162
1163 // take fast step
1164 dInternalStepIsland_x2 (world,body,nb,joint,nj,stepsize);
1165 comparator.end();
1166
1167 //comparator.dump();
1168 //_exit (1);
1169# endif
1170}
generated by cgit v1.1 at 2024-11-10 03:34:20 +0000