Fixed a bug affecting dWorldStep and disabled joints; now the step code knows about...
[ode:mainlinemirror.git] / 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 <ode/odeconfig.h>
24 #include <ode/odemath.h>
25 #include <ode/rotation.h>
26 #include <ode/timer.h>
27 #include <ode/error.h>
28 #include <ode/matrix.h>
29 #include "config.h"
30 #include "objects.h"
31 #include "joints/joint.h"
32 #include "lcp.h"
33 #include "util.h"
34
35 //****************************************************************************
36 // misc defines
37
38 //#define TIMING
39
40
41 #ifdef TIMING
42 #define IFTIMING(x) x
43 #else
44 #define IFTIMING(x) ((void)0)
45 #endif
46
47 //****************************************************************************
48 // special matrix multipliers
49
50 // this assumes the 4th and 8th rows of B and C are zero.
51
52 static void Multiply2_p8r (dReal *A, const dReal *B, const dReal *C,
53                            int p, int r, int Askip)
54 {
55   dIASSERT (p>0 && r>0 && A && B && C);
56   const int Askip_munus_r = Askip - r;
57   dReal *aa = A;
58   const dReal *bb = B;
59   int i;
60   for (i=p; i; --i) {
61     const dReal *cc = C;
62     int j;
63     for (j=r; j; --j) {
64       dReal sum;
65       sum  = bb[0]*cc[0];
66       sum += bb[1]*cc[1];
67       sum += bb[2]*cc[2];
68       sum += bb[4]*cc[4];
69       sum += bb[5]*cc[5];
70       sum += bb[6]*cc[6];
71       *(aa++) = sum; 
72       cc += 8;
73     }
74     bb += 8;
75     aa += Askip_munus_r;
76   }
77 }
78
79
80 // this assumes the 4th and 8th rows of B and C are zero.
81
82 static void MultiplyAdd2_p8r (dReal *A, const dReal *B, const dReal *C,
83                               int p, int r, int Askip)
84 {
85   dIASSERT (p>0 && r>0 && A && B && C);
86   const int Askip_munus_r = Askip - r;
87   dReal *aa = A;
88   const dReal *bb = B;
89   int i;
90   for (i=p; i; --i) {
91     const dReal *cc = C;
92     int j;
93     for (j=r; j; --j) {
94       dReal sum;
95       sum  = bb[0]*cc[0];
96       sum += bb[1]*cc[1];
97       sum += bb[2]*cc[2];
98       sum += bb[4]*cc[4];
99       sum += bb[5]*cc[5];
100       sum += bb[6]*cc[6];
101       *(aa++) += sum; 
102       cc += 8;
103     }
104     bb += 8;
105     aa += Askip_munus_r;
106   }
107 }
108
109
110 // this assumes the 4th and 8th rows of B are zero.
111
112 static void MultiplySub0_p81 (dReal *A, const dReal *B, const dReal *C, int p)
113 {
114   dIASSERT (p>0 && A && B && C);
115   dReal *aa = A;
116   const dReal *bb = B;
117   int i;
118   for (i=p; i; --i) {
119     dReal sum;
120     sum  = bb[0]*C[0];
121     sum += bb[1]*C[1];
122     sum += bb[2]*C[2];
123     sum += bb[4]*C[4];
124     sum += bb[5]*C[5];
125     sum += bb[6]*C[6];
126     *(aa++) -= sum;
127     bb += 8;
128   }
129 }
130
131
132 // this assumes the 4th and 8th rows of B are zero.
133
134 static void MultiplyAdd1_8q1 (dReal *A, const dReal *B, const dReal *C, int q)
135 {
136   dIASSERT (q>0 && A && B && C);
137   const dReal *bb = B;
138   dReal sum0 = 0, sum1 = 0, sum2 = 0, sum4=0, sum5 = 0, sum6 = 0;
139   int k;
140   for (k=0; k<q; ++k) {
141     const dReal C_k = C[k];
142     sum0 += bb[0] * C_k;
143     sum1 += bb[1] * C_k;
144     sum2 += bb[2] * C_k;
145     sum4 += bb[4] * C_k;
146     sum5 += bb[5] * C_k;
147     sum6 += bb[6] * C_k;
148     bb += 8;
149   }
150   A[0] += sum0;
151   A[1] += sum1;
152   A[2] += sum2;
153   A[4] += sum4;
154   A[5] += sum5;
155   A[6] += sum6;
156 }
157
158
159 // this assumes the 4th and 8th rows of B are zero.
160
161 static void Multiply1_8q1 (dReal *A, const dReal *B, const dReal *C, int q)
162 {
163   const dReal *bb = B;
164   dReal sum0 = 0, sum1 = 0, sum2 = 0, sum4=0, sum5 = 0, sum6 = 0;
165   int k;
166   for (k=0; k<q; ++k) {
167     const dReal C_k = C[k];
168     sum0 += bb[0] * C_k;
169     sum1 += bb[1] * C_k;
170     sum2 += bb[2] * C_k;
171     sum4 += bb[4] * C_k;
172     sum5 += bb[5] * C_k;
173     sum6 += bb[6] * C_k;
174     bb += 8;
175   }
176   A[0] = sum0;
177   A[1] = sum1;
178   A[2] = sum2;
179   A[4] = sum4;
180   A[5] = sum5;
181   A[6] = sum6;
182 }
183
184 //****************************************************************************
185 // an optimized version of dInternalStepIsland1()
186
187 struct dJointWithInfo1
188 {
189   dxJoint *joint;
190   dxJoint::Info1 info;
191 };
192
193 void dInternalStepIsland_x2 (dxWorldProcessContext *context, 
194                              dxWorld *world, dxBody * const *body, int nb,
195                              dxJoint * const *_joint, int _nj, dReal stepsize)
196 {
197   IFTIMING(dTimerStart("preprocessing"));
198
199   const dReal stepsizeRecip = dRecip(stepsize);
200
201   {
202     // number all bodies in the body list - set their tag values
203     int i;
204     for (i=0; i<nb; ++i) body[i]->tag = i;
205   }
206
207   // for all bodies, compute the inertia tensor and its inverse in the global
208   // frame, and compute the rotational force and add it to the torque
209   // accumulator. invI are vertically stacked 3x4 matrices, one per body.
210   // @@@ check computation of rotational force.
211
212   dReal *invI = context->AllocateArray<dReal> (3*4*nb);
213
214   { // Identical to QuickStep
215     dReal *invIrow = invI;
216     dxBody *const *const bodyend = body + nb;
217     for (dxBody *const *bodycurr = body; bodycurr != bodyend; invIrow += 12, ++bodycurr) {
218       dMatrix3 tmp;
219       dxBody *b = *bodycurr;
220
221       // compute inverse inertia tensor in global frame
222       dMultiply2_333 (tmp,b->invI,b->posr.R);
223       dMultiply0_333 (invIrow,b->posr.R,tmp);
224
225       if (b->flags & dxBodyGyroscopic) {
226         dMatrix3 I;
227         // compute inertia tensor in global frame
228         dMultiply2_333 (tmp,b->mass.I,b->posr.R);
229         dMultiply0_333 (I,b->posr.R,tmp);
230         // compute rotational force
231         dMultiply0_331 (tmp,I,b->avel);
232         dSubtractVectorCross3 (b->tacc,b->avel,tmp);
233       }
234     }
235   }
236
237   { // Identical to QuickStep
238     // add the gravity force to all bodies
239     // since gravity does normally have only one component it's more efficient
240     // to run three loops for each individual component
241     dxBody *const *const bodyend = body + nb;
242     dReal gravity_x = world->gravity[0];
243     if (gravity_x) {
244       for (dxBody *const *bodycurr = body; bodycurr != bodyend; ++bodycurr) {
245         dxBody *b = *bodycurr;
246         if ((b->flags & dxBodyNoGravity)==0) {
247           b->facc[0] += b->mass.mass * gravity_x;
248         }
249       }
250     }
251     dReal gravity_y = world->gravity[1];
252     if (gravity_y) {
253       for (dxBody *const *bodycurr = body; bodycurr != bodyend; ++bodycurr) {
254         dxBody *b = *bodycurr;
255         if ((b->flags & dxBodyNoGravity)==0) {
256           b->facc[1] += b->mass.mass * gravity_y;
257         }
258       }
259     }
260     dReal gravity_z = world->gravity[2];
261     if (gravity_z) {
262       for (dxBody *const *bodycurr = body; bodycurr != bodyend; ++bodycurr) {
263         dxBody *b = *bodycurr;
264         if ((b->flags & dxBodyNoGravity)==0) {
265           b->facc[2] += b->mass.mass * gravity_z;
266         }
267       }
268     }
269   }
270
271   // get m = total constraint dimension, nub = number of unbounded variables.
272   // create constraint offset array and number-of-rows array for all joints.
273   // the constraints are re-ordered as follows: the purely unbounded
274   // constraints, the mixed unbounded + LCP constraints, and last the purely
275   // LCP constraints. this assists the LCP solver to put all unbounded
276   // variables at the start for a quick factorization.
277   //
278   // joints with m=0 are inactive and are removed from the joints array
279   // entirely, so that the code that follows does not consider them.
280   // also number all active joints in the joint list (set their tag values).
281   // inactive joints receive a tag value of -1.
282
283   // Reserve twice as much memory and start from the middle so that regardless of 
284   // what direction the array grows to there would be sufficient room available.
285   const size_t ji_reserve_count = 2 * _nj;
286   dJointWithInfo1 *jointiinfos = context->AllocateArray<dJointWithInfo1> (ji_reserve_count);
287   int nub, ji_start, ji_end;
288
289   {
290     int unb_start, mix_start, mix_end, lcp_end;
291     unb_start = mix_start = mix_end = lcp_end = _nj;
292
293     dJointWithInfo1 *jicurr = jointiinfos + lcp_end;
294     dxJoint *const *const _jend = _joint + _nj;
295     dxJoint *const *_jcurr = _joint;
296     while (true) {
297       // -------------------------------------------------------------------------
298       // Switch to growing array forward
299       {
300         bool fwd_end_reached = false;
301         dJointWithInfo1 *jimixend = jointiinfos + mix_end;
302         while (true) {  // jicurr=dest, _jcurr=src
303           if (_jcurr == _jend) {
304             lcp_end = jicurr - jointiinfos;
305             fwd_end_reached = true;
306             break;
307           }
308           dxJoint *j = *_jcurr++;
309           j->getInfo1 (&jicurr->info);
310           dIASSERT (jicurr->info.m >= 0 && jicurr->info.m <= 6 && jicurr->info.nub >= 0 && jicurr->info.nub <= jicurr->info.m);
311           if (jicurr->info.m > 0) {
312             if (jicurr->info.nub == 0) { // A lcp info - a correct guess!!!
313               jicurr->joint = j;
314               ++jicurr;
315             } else if (jicurr->info.nub < jicurr->info.m) { // A mixed case
316               if (unb_start == mix_start) { // no unbounded infos yet - just move to opposite side of mixed-s
317                 unb_start = mix_start = mix_start - 1;
318                 dJointWithInfo1 *jimixstart = jointiinfos + mix_start;
319                 jimixstart->info = jicurr->info;
320                 jimixstart->joint = j;
321               } else if (jimixend != jicurr) { // have to swap to the tail of mixed-s
322                 dxJoint::Info1 tmp_info = jicurr->info;
323                 *jicurr = *jimixend;
324                 jimixend->info = tmp_info;
325                 jimixend->joint = j;
326                 ++jimixend; ++jicurr;
327               } else { // no need to swap as there are no LCP info-s yet
328                 jicurr->joint = j;
329                 jimixend = jicurr = jicurr + 1;
330               }
331             } else { // A purely unbounded case -- break out and proceed growing in opposite direction
332               unb_start = unb_start - 1;
333               dJointWithInfo1 *jiunbstart = jointiinfos + unb_start;
334               jiunbstart->info = jicurr->info;
335               jiunbstart->joint = j;
336               lcp_end = jicurr - jointiinfos;
337               mix_end = jimixend - jointiinfos;
338               jicurr = jiunbstart - 1;
339               break;
340             }
341           } else {
342             j->tag = -1;
343           }
344         }
345         if (fwd_end_reached) {
346           break;
347         }
348       }
349       // -------------------------------------------------------------------------
350       // Switch to growing array backward
351       {
352         bool bkw_end_reached = false;
353         dJointWithInfo1 *jimixstart = jointiinfos + mix_start - 1;
354         while (true) {  // jicurr=dest, _jcurr=src
355           if (_jcurr == _jend) {
356             unb_start = (jicurr + 1) - jointiinfos;
357             mix_start = (jimixstart + 1) - jointiinfos;
358             bkw_end_reached = true;
359             break;
360           }
361           dxJoint *j = *_jcurr++;
362           j->getInfo1 (&jicurr->info);
363           dIASSERT (jicurr->info.m >= 0 && jicurr->info.m <= 6 && jicurr->info.nub >= 0 && jicurr->info.nub <= jicurr->info.m);
364           if (jicurr->info.m > 0) {
365             if (jicurr->info.nub == jicurr->info.m) { // An unbounded info - a correct guess!!!
366               jicurr->joint = j;
367               --jicurr;
368             } else if (jicurr->info.nub > 0) { // A mixed case
369               if (mix_end = lcp_end) { // no lcp infos yet - just move to opposite side of mixed-s
370                 dJointWithInfo1 *jimixend = jointiinfos + mix_end;
371                 lcp_end = mix_end = mix_end + 1;
372                 jimixend->info = jicurr->info;
373                 jimixend->joint = j;
374               } else if (jimixstart != jicurr) { // have to swap to the head of mixed-s
375                 dxJoint::Info1 tmp_info = jicurr->info;
376                 *jicurr = *jimixstart;
377                 jimixstart->info = tmp_info;
378                 jimixstart->joint = j;
379                 --jimixstart; --jicurr;
380               } else { // no need to swap as there are no unbounded info-s yet
381                 jicurr->joint = j;
382                 jimixstart = jicurr = jicurr - 1;
383               }
384             } else { // A purely lcp case -- break out and proceed growing in opposite direction
385               dJointWithInfo1 *jilcpend = jointiinfos + lcp_end;
386               lcp_end = lcp_end + 1;
387               jilcpend->info = jicurr->info;
388               jilcpend->joint = j;
389               unb_start = (jicurr + 1) - jointiinfos;
390               mix_start = (jimixstart + 1) - jointiinfos;
391               jicurr = jilcpend + 1;
392               break;
393             }
394           } else {
395             j->tag = -1;
396           }
397         }
398         if (bkw_end_reached) {
399           break;
400         }
401       }
402     }
403     
404     nub = mix_start - unb_start;
405     ji_start = unb_start;
406     ji_end = lcp_end;
407   }
408
409   context->ShrinkArray<dJointWithInfo1>(jointiinfos, ji_reserve_count, ji_end);
410   jointiinfos += ji_start;
411   int nj = ji_end - ji_start;
412
413   int m = 0;
414
415   {
416     int mcurr = 0;
417     const dJointWithInfo1 *jicurr = jointiinfos;
418     const dJointWithInfo1 *const jiend = jicurr + nj;
419     for (int i = 0; jicurr != jiend; i++, ++jicurr) {
420       jicurr->joint->tag = i;
421       int jm = jicurr->info.m;
422       mcurr += jm;
423     }
424
425     m = mcurr;
426   }
427
428   // this will be set to the force due to the constraints
429   dReal *cforce = context->AllocateArray<dReal> (nb*8);
430   dSetZero (cforce,nb*8);
431
432   // if there are constraints, compute cforce
433   if (m > 0) {
434     // create a constraint equation right hand side vector `c', a constraint
435     // force mixing vector `cfm', and LCP low and high bound vectors, and an
436     // 'findex' vector.
437     dReal *lo, *hi, *J, *A, *rhs;
438     int *findex;
439
440     {
441       int mlocal = m;
442
443       lo = context->AllocateArray<dReal> (mlocal);
444       dSetValue (lo,mlocal,-dInfinity);
445
446       hi = context->AllocateArray<dReal> (mlocal);
447       dSetValue (hi,mlocal, dInfinity);
448
449       J = context->AllocateArray<dReal> (2*8*mlocal);
450       dSetZero (J,2*8*mlocal);
451
452       findex = context->AllocateArray<int> (mlocal);
453       for (int i=0; i<mlocal; ++i) findex[i] = -1;
454
455       int mskip = dPAD(mlocal);
456       A = context->AllocateArray<dReal> (mlocal*mskip);
457       dSetZero (A,mlocal*mskip);
458
459       rhs = context->AllocateArray<dReal> (mlocal);
460       dSetZero (rhs,mlocal);
461     }
462
463     // Put 'c' in the same memory as 'rhs' as they transit into each other
464     dReal *c = rhs; rhs = NULL; // erase rhs pointer for now as it is not to be used yet
465
466     BEGIN_STATE_SAVE(context, cfmstate) {
467       dReal *cfm = context->AllocateArray<dReal> (m);
468       dSetValue (cfm,m,world->global_cfm);
469
470       dReal *JinvM = context->AllocateArray<dReal> (2*8*m);
471       dSetZero (JinvM,2*8*m);
472
473       {
474         IFTIMING(dTimerNow ("create J"));
475         // get jacobian data from constraints. a (2*m)x8 matrix will be created
476         // to store the two jacobian blocks from each constraint. it has this
477         // format:
478         //
479         //   l l l 0 a a a 0  \    .
480         //   l l l 0 a a a 0   }-- jacobian body 1 block for joint 0 (3 rows)
481         //   l l l 0 a a a 0  /
482         //   l l l 0 a a a 0  \    .
483         //   l l l 0 a a a 0   }-- jacobian body 2 block for joint 0 (3 rows)
484         //   l l l 0 a a a 0  /
485         //   l l l 0 a a a 0  }--- jacobian body 1 block for joint 1 (1 row)
486         //   l l l 0 a a a 0  }--- jacobian body 2 block for joint 1 (1 row)
487         //   etc...
488         //
489         //   (lll) = linear jacobian data
490         //   (aaa) = angular jacobian data
491         //
492
493         dxJoint::Info2 Jinfo;
494         Jinfo.rowskip = 8;
495         Jinfo.fps = stepsizeRecip;
496         Jinfo.erp = world->global_erp;
497
498         unsigned ofsi = 0;
499         const dJointWithInfo1 *jicurr = jointiinfos;
500         const dJointWithInfo1 *const jiend = jicurr + nj;
501         for (; jicurr != jiend; ++jicurr) {
502           const int infom = jicurr->info.m;
503           dReal *const J1row = J + 2*8*ofsi;
504           Jinfo.J1l = J1row;
505           Jinfo.J1a = J1row + 4;
506           dReal *const J2row = J1row + 8*infom;
507           Jinfo.J2l = J2row;
508           Jinfo.J2a = J2row + 4;
509           Jinfo.c = c + ofsi;
510           Jinfo.cfm = cfm + ofsi;
511           Jinfo.lo = lo + ofsi;
512           Jinfo.hi = hi + ofsi;
513           Jinfo.findex = findex + ofsi;
514           
515           dxJoint *joint = jicurr->joint;
516           joint->getInfo2 (&Jinfo);
517           
518           // adjust returned findex values for global index numbering
519           int *findex_ofsi = findex + ofsi;
520           for (int j=0; j<infom; ++j) {
521             int fival = findex_ofsi[j];
522             if (fival >= 0) 
523               findex_ofsi[j] = fival + ofsi;
524           }
525
526           ofsi += infom;
527         }
528       }
529
530       {
531         IFTIMING(dTimerNow ("compute A"));
532         {
533           // compute A = J*invM*J'. first compute JinvM = J*invM. this has the same
534           // format as J so we just go through the constraints in J multiplying by
535           // the appropriate scalars and matrices.
536           unsigned ofsi = 0;
537           const dJointWithInfo1 *jicurr = jointiinfos;
538           const dJointWithInfo1 *const jiend = jicurr + nj;
539           for (; jicurr != jiend; ++jicurr) {
540             const int infom = jicurr->info.m;
541             dxJoint *joint = jicurr->joint;
542             int b0 = joint->node[0].body->tag;
543             dReal body_invMass0 = body[b0]->invMass;
544             dReal *body_invI0 = invI + b0*12;
545             dReal *Jsrc = J + 2*8*ofsi;
546             dReal *Jdst = JinvM + 2*8*ofsi;
547             for (int j=infom-1; j>=0; --j) {
548               for (int k=0; k<3; ++k) Jdst[k] = Jsrc[k] * body_invMass0;
549               dMultiply0_133 (Jdst+4,Jsrc+4,body_invI0);
550               Jsrc += 8;
551               Jdst += 8;
552             }
553
554             if (joint->node[1].body) {
555               int b1 = joint->node[1].body->tag;
556               dReal body_invMass1 = body[b1]->invMass;
557               dReal *body_invI1 = invI + b1*12;
558               for (int j=infom-1; j>=0; --j) {
559                 for (int k=0; k<3; ++k) Jdst[k] = Jsrc[k] * body_invMass1;
560                 dMultiply0_133 (Jdst+4,Jsrc+4,body_invI1);
561                 Jsrc += 8;
562                 Jdst += 8;
563               }
564             }
565
566             ofsi += infom;
567           }
568         }
569
570         {
571           // now compute A = JinvM * J'. A's rows and columns are grouped by joint,
572           // i.e. in the same way as the rows of J. block (i,j) of A is only nonzero
573           // if joints i and j have at least one body in common. 
574
575           BEGIN_STATE_SAVE(context, ofsstate) {
576             int *ofs = context->AllocateArray<int> (m);
577             const int mskip = dPAD(m);
578
579             unsigned ofsi = 0;
580             const dJointWithInfo1 *jicurr = jointiinfos;
581             const dJointWithInfo1 *const jiend = jicurr + nj;
582             for (int i = 0; jicurr != jiend; i++, ++jicurr) {
583               const int infom = jicurr->info.m;
584               dxJoint *joint = jicurr->joint;
585
586               dReal *Arow = A + mskip*ofsi;
587               dReal *JinvMrow = JinvM + 2*8*ofsi;
588
589               dxBody *jb0 = joint->node[0].body;
590               for (dxJointNode *n0=jb0->firstjoint; n0; n0=n0->next) {
591                 if (!n0->joint->isEnabled()) continue;
592                 // if joint was tagged as -1 then it is an inactive (m=0)
593                 // joint that should not be considered
594                 int j0 = n0->joint->tag;
595                 if (j0==-1 || j0 >= i) continue;
596
597                 const dJointWithInfo1 *jiother = jointiinfos + j0;
598                 int ofsother = (jiother->joint->node[1].body == jb0) ? 8*jiother->info.m : 0;
599                 // set block of A
600                 MultiplyAdd2_p8r (Arow + ofs[j0], JinvMrow, 
601                   J + 2*8*ofs[j0] + ofsother, infom, jiother->info.m, mskip);
602               }
603
604               dxBody *jb1 = joint->node[1].body;
605               dIASSERT(jb1 != jb0);
606               if (jb1)
607               {
608                 for (dxJointNode *n1=jb1->firstjoint; n1; n1=n1->next) {
609                   if (!n1->joint->isEnabled()) continue;
610                   // if joint was tagged as -1 then it is an inactive (m=0)
611                   // joint that should not be considered
612                   int j1 = n1->joint->tag;
613                   if (j1==-1 || j1 >= i) continue;
614
615                   const dJointWithInfo1 *jiother = jointiinfos + j1;
616                   int ofsother = (jiother->joint->node[1].body == jb1) ? 8*jiother->info.m : 0;
617                   // set block of A
618                   MultiplyAdd2_p8r (Arow + ofs[j1], JinvMrow + 8*infom, 
619                     J + 2*8*ofs[j1] + ofsother, infom, jiother->info.m, mskip);
620                 }
621               }
622
623               ofs[i] = ofsi;
624               ofsi += infom;
625             }
626
627           } END_STATE_SAVE(context, ofsstate);
628         }
629
630         {
631           // compute diagonal blocks of A
632           const int mskip = dPAD(m);
633
634           unsigned ofsi = 0;
635           const dJointWithInfo1 *jicurr = jointiinfos;
636           const dJointWithInfo1 *const jiend = jicurr + nj;
637           for (; jicurr != jiend; ++jicurr) {
638             const int infom = jicurr->info.m;
639             dReal *Arow = A + (mskip+1)*ofsi;
640             dReal *JinvMrow = JinvM + 2*8*ofsi;
641             dReal *Jrow = J + 2*8*ofsi;
642             Multiply2_p8r (Arow, JinvMrow, Jrow, infom, infom, mskip);
643             if (jicurr->joint->node[1].body) {
644               MultiplyAdd2_p8r (Arow, JinvMrow + 8*infom, Jrow + 8*infom, infom, infom, mskip);
645             }
646
647             ofsi += infom;
648           }
649         }
650
651         {
652           // add cfm to the diagonal of A
653           const int mskip = dPAD(m);
654
655           dReal *Arow = A;
656           for (int i=0; i<m; Arow += mskip, ++i) {
657             Arow[i] += cfm[i] * stepsizeRecip;
658           }
659         }
660        }
661
662     } END_STATE_SAVE(context, cfmstate);
663
664     BEGIN_STATE_SAVE(context, tmp1state) {
665       // compute the right hand side `rhs'
666       IFTIMING(dTimerNow ("compute rhs"));
667
668       dReal *tmp1 = context->AllocateArray<dReal> (nb*8);
669       //dSetZero (tmp1,nb*8);
670
671       {
672         // put v/h + invM*fe into tmp1
673         dReal *tmp1curr = tmp1;
674         const dReal *invIrow = invI;
675         dxBody *const *const bodyend = body + nb;
676         for (dxBody *const *bodycurr = body; bodycurr != bodyend; tmp1curr+=8, invIrow+=12, ++bodycurr) {
677           dxBody *b = *bodycurr;
678           for (int j=0; j<3; ++j) tmp1curr[j] = b->facc[j]*b->invMass + b->lvel[j]*stepsizeRecip;
679           dMultiply0_331 (tmp1curr+4, invIrow, b->tacc);
680           for (int k=0; k<3; ++k) tmp1curr[4+k] += b->avel[k]*stepsizeRecip;
681         }
682       }
683
684       {
685         // init rhs -- this erases 'c' as they reside in the same memory!!!
686         rhs = c;
687         for (int i=0; i<m; ++i) rhs[i] = c[i]*stepsizeRecip;
688         c = NULL; // set 'c' to NULL to prevent unexpected access
689       }
690
691       {
692         // put J*tmp1 into rhs
693         unsigned ofsi = 0;
694         const dJointWithInfo1 *jicurr = jointiinfos;
695         const dJointWithInfo1 *const jiend = jicurr + nj;
696         for (; jicurr != jiend; ++jicurr) {
697           const int infom = jicurr->info.m;
698           dxJoint *joint = jicurr->joint;
699
700           dReal *rhscurr = rhs+ofsi;
701           const dReal *Jrow = J + 2*8*ofsi;
702           MultiplySub0_p81 (rhscurr, Jrow, tmp1 + 8*joint->node[0].body->tag, infom);
703           if (joint->node[1].body) {
704             MultiplySub0_p81 (rhscurr, Jrow + 8*infom, tmp1 + 8*joint->node[1].body->tag, infom);
705           }
706
707           ofsi += infom;
708         }
709       }
710     } END_STATE_SAVE(context, tmp1state);
711
712     dReal *lambda = context->AllocateArray<dReal> (m);
713
714     BEGIN_STATE_SAVE(context, lcpstate) {
715       IFTIMING(dTimerNow ("solving LCP problem"));
716
717       // solve the LCP problem and get lambda.
718       // this will destroy A but that's OK
719       dSolveLCP (context, m, A, lambda, rhs, NULL, nub, lo, hi, findex);
720
721     } END_STATE_SAVE(context, lcpstate);
722
723     {
724       IFTIMING(dTimerNow ("compute constraint force"));
725
726       // compute the constraint force `cforce'
727       // compute cforce = J'*lambda
728       unsigned ofsi = 0;
729       const dJointWithInfo1 *jicurr = jointiinfos;
730       const dJointWithInfo1 *const jiend = jicurr + nj;
731       for (; jicurr != jiend; ++jicurr) {
732         const int infom = jicurr->info.m;
733         dxJoint *joint = jicurr->joint;
734         
735         const dReal *JJ = J + 2*8*ofsi;
736         const dReal *lambdarow = lambda + ofsi;
737
738         dJointFeedback *fb = joint->feedback;
739
740         if (fb) {
741           // the user has requested feedback on the amount of force that this
742           // joint is applying to the bodies. we use a slightly slower
743           // computation that splits out the force components and puts them
744           // in the feedback structure.
745           dReal data[8];
746           Multiply1_8q1 (data, JJ, lambdarow, infom);
747
748           dxBody* b1 = joint->node[0].body;
749           dReal *cf1 = cforce + 8*b1->tag;
750           cf1[0] += (fb->f1[0] = data[0]);
751           cf1[1] += (fb->f1[1] = data[1]);
752           cf1[2] += (fb->f1[2] = data[2]);
753           cf1[4] += (fb->t1[0] = data[4]);
754           cf1[5] += (fb->t1[1] = data[5]);
755           cf1[6] += (fb->t1[2] = data[6]);
756
757           dxBody* b2 = joint->node[1].body;
758           if (b2){
759             Multiply1_8q1 (data, JJ + 8*infom, lambdarow, infom);
760
761             dReal *cf2 = cforce + 8*b2->tag;
762             cf2[0] += (fb->f2[0] = data[0]);
763             cf2[1] += (fb->f2[1] = data[1]);
764             cf2[2] += (fb->f2[2] = data[2]);
765             cf2[4] += (fb->t2[0] = data[4]);
766             cf2[5] += (fb->t2[1] = data[5]);
767             cf2[6] += (fb->t2[2] = data[6]);
768           }
769         }
770         else {
771           // no feedback is required, let's compute cforce the faster way
772           dxBody* b1 = joint->node[0].body;
773           dReal *cf1 = cforce + 8*b1->tag;
774           MultiplyAdd1_8q1 (cf1, JJ, lambdarow, infom);
775           
776           dxBody* b2 = joint->node[1].body;
777           if (b2) {
778             dReal *cf2 = cforce + 8*b2->tag;
779             MultiplyAdd1_8q1 (cf2, JJ + 8*infom, lambdarow, infom);
780           }
781         }
782
783         ofsi += infom;
784       }
785     }
786   } // if (m > 0)
787
788   {
789     // compute the velocity update
790     IFTIMING(dTimerNow ("compute velocity update"));
791
792     // add fe to cforce and multiply cforce by stepsize
793     dReal data[4];
794     const dReal *invIrow = invI;
795     dReal *cforcecurr = cforce;
796     dxBody *const *const bodyend = body + nb;
797     for (dxBody *const *bodycurr = body; bodycurr != bodyend; invIrow+=12, cforcecurr+=8, ++bodycurr) {
798       dxBody *b = *bodycurr;
799
800       dReal body_invMass_mul_stepsize = stepsize * b->invMass;
801       for (int j=0; j<3; ++j) b->lvel[j] += (cforcecurr[j] + b->facc[j]) * body_invMass_mul_stepsize;
802
803       for (int k=0; k<3; ++k) data[k] = (cforcecurr[4+k] + b->tacc[k]) * stepsize;
804       dMultiplyAdd0_331 (b->avel, invIrow, data);
805     }
806   }
807
808   {
809     // update the position and orientation from the new linear/angular velocity
810     // (over the given timestep)
811     IFTIMING(dTimerNow ("update position"));
812     dxBody *const *const bodyend = body + nb;
813     for (dxBody *const *bodycurr = body; bodycurr != bodyend; ++bodycurr) {
814       dxBody *b = *bodycurr;
815       dxStepBody (b,stepsize);
816     }
817   }
818
819   {
820     IFTIMING(dTimerNow ("tidy up"));
821
822     // zero all force accumulators
823     dxBody *const *const bodyend = body + nb;
824     for (dxBody *const *bodycurr = body; bodycurr != bodyend; ++bodycurr) {
825       dxBody *b = *bodycurr;
826       b->facc[0] = 0;
827       b->facc[1] = 0;
828       b->facc[2] = 0;
829       b->facc[3] = 0;
830       b->tacc[0] = 0;
831       b->tacc[1] = 0;
832       b->tacc[2] = 0;
833       b->tacc[3] = 0;
834     }
835   }
836
837   IFTIMING(dTimerEnd());
838   if (m > 0) IFTIMING(dTimerReport (stdout,1));
839
840 }
841
842 //****************************************************************************
843
844 void dInternalStepIsland (dxWorldProcessContext *context, 
845                           dxWorld *world, dxBody * const *body, int nb,
846                           dxJoint * const *joint, int nj, dReal stepsize)
847 {
848   dInternalStepIsland_x2 (context,world,body,nb,joint,nj,stepsize);
849 }
850
851 size_t dxEstimateStepMemoryRequirements (dxBody * const *body, int nb, dxJoint * const *_joint, int _nj)
852 {
853   int nj, m;
854
855   {
856     int njcurr = 0, mcurr = 0;
857     dxJoint::SureMaxInfo info;
858     dxJoint *const *const _jend = _joint + _nj;
859     for (dxJoint *const *_jcurr = _joint; _jcurr != _jend; ++_jcurr) {  
860       dxJoint *j = *_jcurr;
861       j->getSureMaxInfo (&info);
862
863       int jm = info.max_m;
864       if (jm > 0) {
865         njcurr++;
866
867         mcurr += jm;
868       }
869     }
870     nj = njcurr; m = mcurr;
871   }
872
873   size_t res = 0;
874
875   res += dEFFICIENT_SIZE(sizeof(dReal) * 3 * 4 * nb); // for invI
876
877   {
878     size_t sub1_res1 = dEFFICIENT_SIZE(sizeof(dJointWithInfo1) * 2 * _nj); // for initial jointiinfos
879
880     // The array can't grow right more than by nj
881     size_t sub1_res2 = dEFFICIENT_SIZE(sizeof(dJointWithInfo1) * (_nj + nj)); // for shrunk jointiinfos
882     sub1_res2 += dEFFICIENT_SIZE(sizeof(dReal) * 8 * nb); // for cforce
883     if (m > 0) {
884       sub1_res2 += dEFFICIENT_SIZE(sizeof(dReal) * 2 * 8 * m); // for J
885       int mskip = dPAD(m);
886       sub1_res2 += dEFFICIENT_SIZE(sizeof(dReal) * mskip * m); // for A
887       sub1_res2 += 3 * dEFFICIENT_SIZE(sizeof(dReal) * m); // for lo, hi, rhs
888       sub1_res2 += dEFFICIENT_SIZE(sizeof(int) * m); // for findex
889       {
890         size_t sub2_res1 = dEFFICIENT_SIZE(sizeof(dReal) * m); // for cfm
891         sub2_res1 += dEFFICIENT_SIZE(sizeof(dReal) * 2 * 8 * m); // for JinvM
892         {
893           size_t sub3_res1 = dEFFICIENT_SIZE(sizeof(int) * m); // for ofs
894
895           size_t sub3_res2 = 0;
896
897           sub2_res1 += (sub3_res1 >= sub3_res2) ? sub3_res1 : sub3_res2;
898         }
899
900         size_t sub2_res2 = 0;
901         {
902           size_t sub3_res1 = 0;
903           {
904             size_t sub4_res1 = dEFFICIENT_SIZE(sizeof(dReal) * 8 * nb); // for tmp1
905
906             size_t sub4_res2 = 0;
907
908             sub3_res1 += (sub4_res1 >= sub4_res2) ? sub4_res1 : sub4_res2;
909           }
910
911           size_t sub3_res2 = dEFFICIENT_SIZE(sizeof(dReal) * m); // for lambda
912           {
913             size_t sub4_res1 = dEstimateSolveLCPMemoryReq(m, false);
914
915             size_t sub4_res2 = 0;
916
917             sub3_res2 += (sub4_res1 >= sub4_res2) ? sub4_res1 : sub4_res2;
918           }
919
920           sub2_res2 += (sub3_res1 >= sub3_res2) ? sub3_res1 : sub3_res2;
921         }
922
923         sub1_res2 += (sub2_res1 >= sub2_res2) ? sub2_res1 : sub2_res2;
924       }
925     }
926
927     res += (sub1_res1 >= sub1_res2) ? sub1_res1 : sub1_res2;
928   }
929
930   return res;
931 }
932
933