Back to index

supertuxkart  0.5+dfsg1
btSorLcp.cpp
Go to the documentation of this file.
00001 /*
00002  * Quickstep constraint solver re-distributed under the ZLib license with permission from Russell L. Smith
00003  * Original version is from Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith.
00004  * All rights reserved.  Email: russ@q12.org   Web: www.q12.org
00005  Bullet Continuous Collision Detection and Physics Library
00006  Bullet is Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
00007 
00008 This software is provided 'as-is', without any express or implied warranty.
00009 In no event will the authors be held liable for any damages arising from the use of this software.
00010 Permission is granted to anyone to use this software for any purpose, 
00011 including commercial applications, and to alter it and redistribute it freely, 
00012 subject to the following restrictions:
00013 
00014 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
00015 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
00016 3. This notice may not be removed or altered from any source distribution.
00017 */
00018 
00019 #include "btSorLcp.h"
00020 #include "btOdeSolverBody.h"
00021 
00022 #ifdef USE_SOR_SOLVER
00023 
00024 // SOR LCP taken from ode quickstep, for comparisons to Bullet sequential impulse solver.
00025 #include "LinearMath/btScalar.h"
00026 
00027 #include "BulletDynamics/Dynamics/btRigidBody.h"
00028 #include <math.h>
00029 #include <float.h>//FLT_MAX
00030 #ifdef WIN32
00031 #include <memory.h>
00032 #endif
00033 #include <string.h>
00034 #include <stdio.h>
00035 
00036 #if defined (WIN32)
00037 #include <malloc.h>
00038 #else
00039 #if defined (__FreeBSD__)
00040 #include <stdlib.h>
00041 #else
00042 #include <alloca.h>
00043 #endif
00044 #endif
00045 
00046 #include "btOdeJoint.h"
00047 #include "BulletDynamics/ConstraintSolver/btContactSolverInfo.h"
00049 //math stuff
00050 #include "btOdeMacros.h"
00051 
00052 //***************************************************************************
00053 // configuration
00054 
00055 // for the SOR and CG methods:
00056 // uncomment the following line to use warm starting. this definitely
00057 // help for motor-driven joints. unfortunately it appears to hurt
00058 // with high-friction contacts using the SOR method. use with care
00059 
00060 //#define WARM_STARTING 1
00061 
00062 // for the SOR method:
00063 // uncomment the following line to randomly reorder constraint rows
00064 // during the solution. depending on the situation, this can help a lot
00065 // or hardly at all, but it doesn't seem to hurt.
00066 
00067 #define RANDOMLY_REORDER_CONSTRAINTS 1
00068 
00069 //***************************************************************************
00070 // various common computations involving the matrix J
00071 // compute iMJ = inv(M)*J'
00072 inline void compute_invM_JT (int m, dRealMutablePtr J, dRealMutablePtr iMJ, int *jb,
00073        //OdeSolverBody* const *body,
00074         const btAlignedObjectArray<btOdeSolverBody*> &body,
00075        dRealPtr invI)
00076 {
00077        int i,j;
00078        dRealMutablePtr iMJ_ptr = iMJ;
00079        dRealMutablePtr J_ptr = J;
00080        for (i=0; i<m; i++) {
00081               int b1 = jb[i*2];
00082               int b2 = jb[i*2+1];
00083               btScalar k = body[b1]->m_invMass;
00084               for (j=0; j<3; j++) iMJ_ptr[j] = k*J_ptr[j];
00085               dMULTIPLY0_331 (iMJ_ptr + 3, invI + 12*b1, J_ptr + 3);
00086               if (b2 >= 0) {
00087                      k = body[b2]->m_invMass;
00088                      for (j=0; j<3; j++) iMJ_ptr[j+6] = k*J_ptr[j+6];
00089                      dMULTIPLY0_331 (iMJ_ptr + 9, invI + 12*b2, J_ptr + 9);
00090               }
00091               J_ptr += 12;
00092               iMJ_ptr += 12;
00093        }
00094 }
00095 
00096 #if 0
00097 static void multiply_invM_JTSpecial (int m, int nb, dRealMutablePtr iMJ, int *jb,
00098        dRealMutablePtr in, dRealMutablePtr out,int onlyBody1,int onlyBody2)
00099 {
00100        int i,j;
00101 
00102 
00103 
00104        dRealMutablePtr out_ptr1 = out + onlyBody1*6;
00105 
00106        for (j=0; j<6; j++)
00107               out_ptr1[j] = 0;
00108 
00109        if (onlyBody2 >= 0)
00110        {
00111               out_ptr1 = out + onlyBody2*6;
00112 
00113               for (j=0; j<6; j++)
00114                      out_ptr1[j] = 0;
00115        }
00116 
00117        dRealPtr iMJ_ptr = iMJ;
00118        for (i=0; i<m; i++) {
00119 
00120               int b1 = jb[i*2];
00121 
00122               dRealMutablePtr out_ptr = out + b1*6;
00123               if ((b1 == onlyBody1) || (b1 == onlyBody2))
00124               {
00125                      for (j=0; j<6; j++)
00126                             out_ptr[j] += iMJ_ptr[j] * in[i] ;
00127               }
00128 
00129               iMJ_ptr += 6;
00130 
00131               int b2 = jb[i*2+1];
00132               if ((b2 == onlyBody1) || (b2 == onlyBody2))
00133               {
00134                      if (b2 >= 0)
00135                      {
00136                                    out_ptr = out + b2*6;
00137                                    for (j=0; j<6; j++)
00138                                           out_ptr[j] += iMJ_ptr[j] * in[i];
00139                      }
00140               }
00141 
00142               iMJ_ptr += 6;
00143 
00144        }
00145 }
00146 #endif
00147 
00148 
00149 // compute out = inv(M)*J'*in.
00150 
00151 #if 0
00152 static void multiply_invM_JT (int m, int nb, dRealMutablePtr iMJ, int *jb,
00153        dRealMutablePtr in, dRealMutablePtr out)
00154 {
00155        int i,j;
00156        dSetZero1 (out,6*nb);
00157        dRealPtr iMJ_ptr = iMJ;
00158        for (i=0; i<m; i++) {
00159               int b1 = jb[i*2];
00160               int b2 = jb[i*2+1];
00161               dRealMutablePtr out_ptr = out + b1*6;
00162               for (j=0; j<6; j++)
00163                      out_ptr[j] += iMJ_ptr[j] * in[i];
00164               iMJ_ptr += 6;
00165               if (b2 >= 0) {
00166                      out_ptr = out + b2*6;
00167                      for (j=0; j<6; j++) out_ptr[j] += iMJ_ptr[j] * in[i];
00168               }
00169               iMJ_ptr += 6;
00170        }
00171 }
00172 #endif
00173 
00174 
00175 // compute out = J*in.
00176 inline void multiply_J (int m, dRealMutablePtr J, int *jb,
00177        dRealMutablePtr in, dRealMutablePtr out)
00178 {
00179        int i,j;
00180        dRealPtr J_ptr = J;
00181        for (i=0; i<m; i++) {
00182               int b1 = jb[i*2];
00183               int b2 = jb[i*2+1];
00184               btScalar sum = 0;
00185               dRealMutablePtr in_ptr = in + b1*6;
00186               for (j=0; j<6; j++) sum += J_ptr[j] * in_ptr[j];
00187               J_ptr += 6;
00188               if (b2 >= 0) {
00189                      in_ptr = in + b2*6;
00190                      for (j=0; j<6; j++) sum += J_ptr[j] * in_ptr[j];
00191               }
00192               J_ptr += 6;
00193               out[i] = sum;
00194        }
00195 }
00196 
00197 //***************************************************************************
00198 // SOR-LCP method
00199 
00200 // nb is the number of bodies in the body array.
00201 // J is an m*12 matrix of constraint rows
00202 // jb is an array of first and second body numbers for each constraint row
00203 // invI is the global frame inverse inertia for each body (stacked 3x3 matrices)
00204 //
00205 // this returns lambda and fc (the constraint force).
00206 // note: fc is returned as inv(M)*J'*lambda, the constraint force is actually J'*lambda
00207 //
00208 // b, lo and hi are modified on exit
00209 
00210 //------------------------------------------------------------------------------
00211 ATTRIBUTE_ALIGNED16(struct) IndexError {
00212        btScalar error;             // error to sort on
00213        int findex;
00214        int index;           // row index
00215 };
00216 
00217 //------------------------------------------------------------------------------
00218 void btSorLcpSolver::SOR_LCP(int m, int nb, dRealMutablePtr J, int *jb, 
00219        const btAlignedObjectArray<btOdeSolverBody*> &body,
00220        dRealPtr invI, dRealMutablePtr lambda, dRealMutablePtr invMforce, dRealMutablePtr rhs,
00221        dRealMutablePtr lo, dRealMutablePtr hi, dRealPtr cfm, int *findex,
00222        int numiter,float overRelax,
00223        btStackAlloc* stackAlloc
00224        )
00225 {
00226        //btBlock* saBlock = stackAlloc->beginBlock();//Remo: 10.10.2007
00227        AutoBlockSa asaBlock(stackAlloc);
00228 
00229        const int num_iterations = numiter;
00230        const float sor_w = overRelax;            // SOR over-relaxation parameter
00231 
00232        int i,j;
00233 
00234 #ifdef WARM_STARTING
00235        // for warm starting, this seems to be necessary to prevent
00236        // jerkiness in motor-driven joints. i have no idea why this works.
00237        for (i=0; i<m; i++) lambda[i] *= 0.9;
00238 #else
00239        dSetZero1 (lambda,m);
00240 #endif
00241 
00242        // the lambda computed at the previous iteration.
00243        // this is used to measure error for when we are reordering the indexes.
00244        dRealAllocaArray (last_lambda,m);
00245 
00246        // a copy of the 'hi' vector in case findex[] is being used
00247        dRealAllocaArray (hicopy,m);
00248        memcpy (hicopy,hi,m*sizeof(float));
00249 
00250        // precompute iMJ = inv(M)*J'
00251        dRealAllocaArray (iMJ,m*12);
00252        compute_invM_JT (m,J,iMJ,jb,body,invI);
00253 
00254        // compute fc=(inv(M)*J')*lambda. we will incrementally maintain fc
00255        // as we change lambda.
00256 #ifdef WARM_STARTING
00257        multiply_invM_JT (m,nb,iMJ,jb,lambda,fc);
00258 #else
00259        dSetZero1 (invMforce,nb*6);
00260 #endif
00261 
00262        // precompute 1 / diagonals of A
00263        dRealAllocaArray (Ad,m);
00264        dRealPtr iMJ_ptr = iMJ;
00265        dRealMutablePtr J_ptr = J;
00266        for (i=0; i<m; i++) {
00267               float sum = 0;
00268               for (j=0; j<6; j++) sum += iMJ_ptr[j] * J_ptr[j];
00269               if (jb[i*2+1] >= 0) {
00270                      for (j=6; j<12; j++) sum += iMJ_ptr[j] * J_ptr[j];
00271               }
00272               iMJ_ptr += 12;
00273               J_ptr += 12;
00274               Ad[i] = sor_w / sum;//(sum + cfm[i]);
00275        }
00276 
00277        // scale J and b by Ad
00278        J_ptr = J;
00279        for (i=0; i<m; i++) {
00280               for (j=0; j<12; j++) {
00281                      J_ptr[0] *= Ad[i];
00282                      J_ptr++;
00283               }
00284               rhs[i] *= Ad[i];
00285        }
00286 
00287        // scale Ad by CFM
00288        for (i=0; i<m; i++)
00289               Ad[i] *= cfm[i];
00290 
00291        // order to solve constraint rows in
00292        //IndexError *order = (IndexError*) alloca (m*sizeof(IndexError));
00293        IndexError *order = (IndexError*) ALLOCA (m*sizeof(IndexError));
00294        
00295 
00296 #ifndef REORDER_CONSTRAINTS
00297        // make sure constraints with findex < 0 come first.
00298        j=0;
00299        for (i=0; i<m; i++)
00300               if (findex[i] < 0)
00301                      order[j++].index = i;
00302        for (i=0; i<m; i++)
00303               if (findex[i] >= 0)
00304                      order[j++].index = i;
00305        dIASSERT (j==m);
00306 #endif
00307 
00308        for (int iteration=0; iteration < num_iterations; iteration++) {
00309 
00310 #ifdef REORDER_CONSTRAINTS
00311               // constraints with findex < 0 always come first.
00312               if (iteration < 2) {
00313                      // for the first two iterations, solve the constraints in
00314                      // the given order
00315                      for (i=0; i<m; i++) {
00316                             order[i].error = i;
00317                             order[i].findex = findex[i];
00318                             order[i].index = i;
00319                      }
00320               }
00321               else {
00322                      // sort the constraints so that the ones converging slowest
00323                      // get solved last. use the absolute (not relative) error.
00324                      for (i=0; i<m; i++) {
00325                             float v1 = dFabs (lambda[i]);
00326                             float v2 = dFabs (last_lambda[i]);
00327                             float max = (v1 > v2) ? v1 : v2;
00328                             if (max > 0) {
00329                                    //@@@ relative error: order[i].error = dFabs(lambda[i]-last_lambda[i])/max;
00330                                    order[i].error = dFabs(lambda[i]-last_lambda[i]);
00331                             }
00332                             else {
00333                                    order[i].error = dInfinity;
00334                             }
00335                             order[i].findex = findex[i];
00336                             order[i].index = i;
00337                      }
00338               }
00339               qsort (order,m,sizeof(IndexError),&compare_index_error);
00340 #endif
00341 #ifdef RANDOMLY_REORDER_CONSTRAINTS
00342                 if ((iteration & 7) == 0) {
00343                      for (i=1; i<m; ++i) {
00344                             IndexError tmp = order[i];
00345                             int swapi = dRandInt2(i+1);
00346                             order[i] = order[swapi];
00347                             order[swapi] = tmp;
00348                      }
00349                 }
00350 #endif
00351 
00352               //@@@ potential optimization: swap lambda and last_lambda pointers rather
00353               //    than copying the data. we must make sure lambda is properly
00354               //    returned to the caller
00355               memcpy (last_lambda,lambda,m*sizeof(float));
00356 
00357               for (int i=0; i<m; i++) {
00358                      // @@@ potential optimization: we could pre-sort J and iMJ, thereby
00359                      //     linearizing access to those arrays. hmmm, this does not seem
00360                      //     like a win, but we should think carefully about our memory
00361                      //     access pattern.
00362 
00363                      int index = order[i].index;
00364                      J_ptr = J + index*12;
00365                      iMJ_ptr = iMJ + index*12;
00366 
00367                      // set the limits for this constraint. note that 'hicopy' is used.
00368                      // this is the place where the QuickStep method differs from the
00369                      // direct LCP solving method, since that method only performs this
00370                      // limit adjustment once per time step, whereas this method performs
00371                      // once per iteration per constraint row.
00372                      // the constraints are ordered so that all lambda[] values needed have
00373                      // already been computed.
00374                      if (findex[index] >= 0) {
00375                             hi[index] = btFabs (hicopy[index] * lambda[findex[index]]);
00376                             lo[index] = -hi[index];
00377                      }
00378 
00379                      int b1 = jb[index*2];
00380                      int b2 = jb[index*2+1];
00381                      float delta = rhs[index] - lambda[index]*Ad[index];
00382                      dRealMutablePtr fc_ptr = invMforce + 6*b1;
00383 
00384                      // @@@ potential optimization: SIMD-ize this and the b2 >= 0 case
00385                      delta -=fc_ptr[0] * J_ptr[0] + fc_ptr[1] * J_ptr[1] +
00386                             fc_ptr[2] * J_ptr[2] + fc_ptr[3] * J_ptr[3] +
00387                             fc_ptr[4] * J_ptr[4] + fc_ptr[5] * J_ptr[5];
00388                      // @@@ potential optimization: handle 1-body constraints in a separate
00389                      //     loop to avoid the cost of test & jump?
00390                      if (b2 >= 0) {
00391                             fc_ptr = invMforce + 6*b2;
00392                             delta -=fc_ptr[0] * J_ptr[6] + fc_ptr[1] * J_ptr[7] +
00393                                    fc_ptr[2] * J_ptr[8] + fc_ptr[3] * J_ptr[9] +
00394                                    fc_ptr[4] * J_ptr[10] + fc_ptr[5] * J_ptr[11];
00395                      }
00396 
00397                      // compute lambda and clamp it to [lo,hi].
00398                      // @@@ potential optimization: does SSE have clamping instructions
00399                      //     to save test+jump penalties here?
00400                      float new_lambda = lambda[index] + delta;
00401                      if (new_lambda < lo[index]) {
00402                             delta = lo[index]-lambda[index];
00403                             lambda[index] = lo[index];
00404                      }
00405                      else if (new_lambda > hi[index]) {
00406                             delta = hi[index]-lambda[index];
00407                             lambda[index] = hi[index];
00408                      }
00409                      else {
00410                             lambda[index] = new_lambda;
00411                      }
00412 
00413                      //@@@ a trick that may or may not help
00414                      //float ramp = (1-((float)(iteration+1)/(float)num_iterations));
00415                      //delta *= ramp;
00416 
00417                      // update invMforce.
00418                      // @@@ potential optimization: SIMD for this and the b2 >= 0 case
00419                      fc_ptr = invMforce + 6*b1;
00420                      fc_ptr[0] += delta * iMJ_ptr[0];
00421                      fc_ptr[1] += delta * iMJ_ptr[1];
00422                      fc_ptr[2] += delta * iMJ_ptr[2];
00423                      fc_ptr[3] += delta * iMJ_ptr[3];
00424                      fc_ptr[4] += delta * iMJ_ptr[4];
00425                      fc_ptr[5] += delta * iMJ_ptr[5];
00426                      // @@@ potential optimization: handle 1-body constraints in a separate
00427                      //     loop to avoid the cost of test & jump?
00428                      if (b2 >= 0) {
00429                             fc_ptr = invMforce + 6*b2;
00430                             fc_ptr[0] += delta * iMJ_ptr[6];
00431                             fc_ptr[1] += delta * iMJ_ptr[7];
00432                             fc_ptr[2] += delta * iMJ_ptr[8];
00433                             fc_ptr[3] += delta * iMJ_ptr[9];
00434                             fc_ptr[4] += delta * iMJ_ptr[10];
00435                             fc_ptr[5] += delta * iMJ_ptr[11];
00436                      }
00437               }
00438        }
00439        //stackAlloc->endBlock(saBlock);//Remo: 10.10.2007
00440 }
00441 
00442 //------------------------------------------------------------------------------
00443 void btSorLcpSolver::SolveInternal1 (
00444                      float global_cfm,
00445                      float global_erp,
00446                      const btAlignedObjectArray<btOdeSolverBody*> &body, int nb,
00447                      btAlignedObjectArray<btOdeJoint*> &joint, 
00448                      int nj, const btContactSolverInfo& solverInfo,
00449                      btStackAlloc* stackAlloc)
00450 {
00451        //btBlock* saBlock = stackAlloc->beginBlock();//Remo: 10.10.2007
00452        AutoBlockSa asaBlock(stackAlloc);
00453 
00454        int numIter = solverInfo.m_numIterations;
00455        float sOr = solverInfo.m_sor;
00456 
00457        int i,j;
00458 
00459        btScalar stepsize1 = dRecip(solverInfo.m_timeStep);
00460 
00461        // number all bodies in the body list - set their tag values
00462        for (i=0; i<nb; i++)
00463               body[i]->m_odeTag = i;
00464 
00465        // make a local copy of the joint array, because we might want to modify it.
00466        // (the "btOdeJoint *const*" declaration says we're allowed to modify the joints
00467        // but not the joint array, because the caller might need it unchanged).
00468        //@@@ do we really need to do this? we'll be sorting constraint rows individually, not joints
00469        //btOdeJoint **joint = (btOdeJoint**) alloca (nj * sizeof(btOdeJoint*));
00470        //memcpy (joint,_joint,nj * sizeof(btOdeJoint*));
00471 
00472        // for all bodies, compute the inertia tensor and its inverse in the global
00473        // frame, and compute the rotational force and add it to the torque
00474        // accumulator. I and invI are a vertical stack of 3x4 matrices, one per body.
00475        dRealAllocaArray (I,3*4*nb);
00476        dRealAllocaArray (invI,3*4*nb);
00477 /*     for (i=0; i<nb; i++) {
00478               dMatrix3 tmp;
00479               // compute inertia tensor in global frame
00480               dMULTIPLY2_333 (tmp,body[i]->m_I,body[i]->m_R);
00481               // compute inverse inertia tensor in global frame
00482               dMULTIPLY2_333 (tmp,body[i]->m_invI,body[i]->m_R);
00483               dMULTIPLY0_333 (invI+i*12,body[i]->m_R,tmp);
00484               // compute rotational force
00485               dCROSS (body[i]->m_tacc,-=,body[i]->getAngularVelocity(),tmp);
00486        }
00487 */
00488        for (i=0; i<nb; i++) {
00489               dMatrix3 tmp;
00490               // compute inertia tensor in global frame
00491               dMULTIPLY2_333 (tmp,body[i]->m_I,body[i]->m_R);
00492               dMULTIPLY0_333 (I+i*12,body[i]->m_R,tmp);
00493 
00494               // compute inverse inertia tensor in global frame
00495               dMULTIPLY2_333 (tmp,body[i]->m_invI,body[i]->m_R);
00496               dMULTIPLY0_333 (invI+i*12,body[i]->m_R,tmp);
00497               // compute rotational force
00498 //            dMULTIPLY0_331 (tmp,I+i*12,body[i]->m_angularVelocity);
00499 //            dCROSS (body[i]->m_tacc,-=,body[i]->m_angularVelocity,tmp);
00500        }
00501 
00502 
00503 
00504 
00505        // get joint information (m = total constraint dimension, nub = number of unbounded variables).
00506        // joints with m=0 are inactive and are removed from the joints array
00507        // entirely, so that the code that follows does not consider them.
00508        //@@@ do we really need to save all the info1's
00509        btOdeJoint::Info1 *info = (btOdeJoint::Info1*) ALLOCA (nj*sizeof(btOdeJoint::Info1));
00510        
00511        for (i=0, j=0; j<nj; j++) { // i=dest, j=src
00512               joint[j]->GetInfo1 (info+i);
00513               dIASSERT (info[i].m >= 0 && info[i].m <= 6 && info[i].nub >= 0 && info[i].nub <= info[i].m);
00514               if (info[i].m > 0) {
00515                      joint[i] = joint[j];
00516                      i++;
00517               }
00518        }
00519        nj = i;
00520 
00521        // create the row offset array
00522        int m = 0;
00523        int *ofs = (int*) ALLOCA (nj*sizeof(int));
00524        for (i=0; i<nj; i++) {
00525               ofs[i] = m;
00526               m += info[i].m;
00527        }
00528 
00529        // if there are constraints, compute the constraint force
00530        dRealAllocaArray (J,m*12);
00531        int *jb = (int*) ALLOCA (m*2*sizeof(int));
00532        if (m > 0) {
00533               // create a constraint equation right hand side vector `c', a constraint
00534               // force mixing vector `cfm', and LCP low and high bound vectors, and an
00535               // 'findex' vector.
00536               dRealAllocaArray (c,m);
00537               dRealAllocaArray (cfm,m);
00538               dRealAllocaArray (lo,m);
00539               dRealAllocaArray (hi,m);
00540 
00541               int *findex = (int*) ALLOCA (m*sizeof(int));
00542 
00543               dSetZero1 (c,m);
00544               dSetValue1 (cfm,m,global_cfm);
00545               dSetValue1 (lo,m,-dInfinity);
00546               dSetValue1 (hi,m, dInfinity);
00547               for (i=0; i<m; i++) findex[i] = -1;
00548 
00549               // get jacobian data from constraints. an m*12 matrix will be created
00550               // to store the two jacobian blocks from each constraint. it has this
00551               // format:
00552               //
00553               //   l1 l1 l1 a1 a1 a1 l2 l2 l2 a2 a2 a2 \    .
00554               //   l1 l1 l1 a1 a1 a1 l2 l2 l2 a2 a2 a2  }-- jacobian for joint 0, body 1 and body 2 (3 rows)
00555               //   l1 l1 l1 a1 a1 a1 l2 l2 l2 a2 a2 a2 /
00556               //   l1 l1 l1 a1 a1 a1 l2 l2 l2 a2 a2 a2 }--- jacobian for joint 1, body 1 and body 2 (3 rows)
00557               //   etc...
00558               //
00559               //   (lll) = linear jacobian data
00560               //   (aaa) = angular jacobian data
00561               //
00562               dSetZero1 (J,m*12);
00563               btOdeJoint::Info2 Jinfo;
00564               Jinfo.rowskip = 12;
00565               Jinfo.fps = stepsize1;
00566               Jinfo.erp = global_erp;
00567               for (i=0; i<nj; i++) {
00568                      Jinfo.J1l = J + ofs[i]*12;
00569                      Jinfo.J1a = Jinfo.J1l + 3;
00570                      Jinfo.J2l = Jinfo.J1l + 6;
00571                      Jinfo.J2a = Jinfo.J1l + 9;
00572                      Jinfo.c = c + ofs[i];
00573                      Jinfo.cfm = cfm + ofs[i];
00574                      Jinfo.lo = lo + ofs[i];
00575                      Jinfo.hi = hi + ofs[i];
00576                      Jinfo.findex = findex + ofs[i];
00577                      joint[i]->GetInfo2 (&Jinfo);
00578 
00579                      if (Jinfo.c[0] > solverInfo.m_maxErrorReduction)
00580                             Jinfo.c[0] = solverInfo.m_maxErrorReduction;
00581 
00582                      // adjust returned findex values for global index numbering
00583                      for (j=0; j<info[i].m; j++) {
00584                             if (findex[ofs[i] + j] >= 0)
00585                                    findex[ofs[i] + j] += ofs[i];
00586                      }
00587               }
00588 
00589               // create an array of body numbers for each joint row
00590               int *jb_ptr = jb;
00591               for (i=0; i<nj; i++) {
00592                      int b1 = (joint[i]->node[0].body) ? (joint[i]->node[0].body->m_odeTag) : -1;
00593                      int b2 = (joint[i]->node[1].body) ? (joint[i]->node[1].body->m_odeTag) : -1;
00594                      for (j=0; j<info[i].m; j++) {
00595                             jb_ptr[0] = b1;
00596                             jb_ptr[1] = b2;
00597                             jb_ptr += 2;
00598                      }
00599               }
00600               dIASSERT (jb_ptr == jb+2*m);
00601 
00602               // compute the right hand side `rhs'
00603               dRealAllocaArray (tmp1,nb*6);
00604               // put v/h + invM*fe into tmp1
00605               for (i=0; i<nb; i++) {
00606                      btScalar body_invMass = body[i]->m_invMass;
00607                      for (j=0; j<3; j++)
00608                             tmp1[i*6+j] = body[i]->m_facc[j] * body_invMass + body[i]->m_linearVelocity[j] * stepsize1;
00609                      dMULTIPLY0_331NEW (tmp1 + i*6 + 3,=,invI + i*12,body[i]->m_tacc);
00610                      for (j=0; j<3; j++)
00611                             tmp1[i*6+3+j] += body[i]->m_angularVelocity[j] * stepsize1;
00612               }
00613 
00614               // put J*tmp1 into rhs
00615               dRealAllocaArray (rhs,m);
00616               multiply_J (m,J,jb,tmp1,rhs);
00617 
00618               // complete rhs
00619               for (i=0; i<m; i++) rhs[i] = c[i]*stepsize1 - rhs[i];
00620 
00621               // scale CFM
00622               for (i=0; i<m; i++)
00623                      cfm[i] *= stepsize1;
00624 
00625               // load lambda from the value saved on the previous iteration
00626               dRealAllocaArray (lambda,m);
00627 #ifdef WARM_STARTING
00628               dSetZero1 (lambda,m);       //@@@ shouldn't be necessary
00629               for (i=0; i<nj; i++) {
00630                      memcpy (lambda+ofs[i],joint[i]->lambda,info[i].m * sizeof(btScalar));
00631               }
00632 #endif
00633 
00634               // solve the LCP problem and get lambda and invM*constraint_force
00635               dRealAllocaArray (cforce,nb*6);
00636 
00638               SOR_LCP (m,nb,J,jb,body,invI,lambda,cforce,rhs,lo,hi,cfm,findex,numIter,sOr,stackAlloc);
00639 
00640 #ifdef WARM_STARTING
00641               // save lambda for the next iteration
00642               //@@@ note that this doesn't work for contact joints yet, as they are
00643               // recreated every iteration
00644               for (i=0; i<nj; i++) {
00645                      memcpy (joint[i]->lambda,lambda+ofs[i],info[i].m * sizeof(btScalar));
00646               }
00647 #endif
00648 
00649               // note that the SOR method overwrites rhs and J at this point, so
00650               // they should not be used again.
00651               // add stepsize * cforce to the body velocity
00652               for (i=0; i<nb; i++) {
00653                      for (j=0; j<3; j++)
00654                             body[i]->m_linearVelocity[j] += solverInfo.m_timeStep* cforce[i*6+j];
00655                      for (j=0; j<3; j++)
00656                             body[i]->m_angularVelocity[j] += solverInfo.m_timeStep* cforce[i*6+3+j];
00657 
00658               }
00659        }
00660 
00661        // compute the velocity update:
00662        // add stepsize * invM * fe to the body velocity
00663        for (i=0; i<nb; i++) {
00664               btScalar body_invMass = body[i]->m_invMass;
00665               btVector3 linvel = body[i]->m_linearVelocity;
00666               btVector3 angvel = body[i]->m_angularVelocity;
00667 
00668               for (j=0; j<3; j++)
00669               {
00670                      linvel[j] += solverInfo.m_timeStep * body_invMass * body[i]->m_facc[j];
00671               }
00672               for (j=0; j<3; j++)
00673               {
00674                      body[i]->m_tacc[j] *= solverInfo.m_timeStep;
00675               }
00676               dMULTIPLY0_331NEW(angvel,+=,invI + i*12,body[i]->m_tacc);
00677               body[i]->m_angularVelocity = angvel;
00678        }
00679        //stackAlloc->endBlock(saBlock);//Remo: 10.10.2007
00680 }
00681 
00682 
00683 #endif //USE_SOR_SOLVER