home *** CD-ROM | disk | FTP | other *** search
/ GameStar 2006 March / Gamestar_82_2006-03_dvd.iso / DVDStar / Editace / quake4_sdkv10.exe / source / idlib / math / Lcp.cpp < prev    next >
C/C++ Source or Header  |  2005-11-14  |  40KB  |  1,618 lines
  1.  
  2. #include "../precompiled.h"
  3. #pragma hdrstop
  4.  
  5. static idCVar lcp_showFailures( "lcp_showFailures", "0", CVAR_SYSTEM | CVAR_BOOL, "show LCP solver failures" );
  6.  
  7. const float LCP_BOUND_EPSILON            = 1e-5f;
  8. const float LCP_ACCEL_EPSILON            = 1e-5f;
  9. const float LCP_DELTA_ACCEL_EPSILON        = 1e-9f;
  10. const float LCP_DELTA_FORCE_EPSILON        = 1e-9f;
  11.  
  12. #define IGNORE_UNSATISFIABLE_VARIABLES
  13.  
  14. //===============================================================
  15. //                                                        M
  16. //  idLCP_Square                                         MrE
  17. //                                                        E
  18. //===============================================================
  19.  
  20. class idLCP_Square : public idLCP {
  21. public:
  22.     virtual bool    Solve( const idMatX &o_m, idVecX &o_x, const idVecX &o_b, const idVecX &o_lo, const idVecX &o_hi, const int *o_boxIndex );
  23.  
  24. private:
  25.     idMatX            m;                    // original matrix
  26.     idVecX            b;                    // right hand side
  27.     idVecX            lo, hi;                // low and high bounds
  28.     idVecX            f, a;                // force and acceleration
  29.     idVecX            delta_f, delta_a;    // delta force and delta acceleration
  30.     idMatX            clamped;            // LU factored sub matrix for clamped variables
  31.     idVecX            diagonal;            // reciprocal of diagonal of U of the LU factored sub matrix for clamped variables
  32.     int                numUnbounded;        // number of unbounded variables
  33.     int                numClamped;            // number of clamped variables
  34.     float **        rowPtrs;            // pointers to the rows of m
  35.     int *            boxIndex;            // box index
  36.     int *            side;                // tells if a variable is at the low boundary = -1, high boundary = 1 or inbetween = 0
  37.     int *            permuted;            // index to keep track of the permutation
  38.     bool            padded;                // set to true if the rows of the initial matrix are 16 byte padded
  39.  
  40. private:
  41.     bool            FactorClamped( void );
  42.     void            SolveClamped( idVecX &x, const float *b );
  43.     void            Swap( int i, int j );
  44.     void            AddClamped( int r );
  45.     void            RemoveClamped( int r );
  46.     void            CalcForceDelta( int d, float dir );
  47.     void            CalcAccelDelta( int d );
  48.     void            ChangeForce( int d, float step );
  49.     void            ChangeAccel( int d, float step );
  50.     void            GetMaxStep( int d, float dir, float &maxStep, int &limit, int &limitSide ) const;
  51. };
  52.  
  53. /*
  54. ============
  55. idLCP_Square::FactorClamped
  56. ============
  57. */
  58. bool idLCP_Square::FactorClamped( void ) {
  59.     int i, j, k;
  60.     float s, d;
  61.  
  62.     for ( i = 0; i < numClamped; i++ ) {
  63.         memcpy( clamped[i], rowPtrs[i], numClamped * sizeof( float ) );
  64.     }
  65.  
  66.     for ( i = 0; i < numClamped; i++ ) {
  67.  
  68.         s = idMath::Fabs( clamped[i][i] );
  69.  
  70.         if ( s == 0.0f ) {
  71.             return false;
  72.         }
  73.  
  74.         diagonal[i] = d = 1.0f / clamped[i][i];
  75.         for ( j = i + 1; j < numClamped; j++ ) {
  76.             clamped[j][i] *= d;
  77.         }
  78.  
  79.         for ( j = i + 1; j < numClamped; j++ ) {
  80.             d = clamped[j][i];
  81.             for ( k = i + 1; k < numClamped; k++ ) {
  82.                 clamped[j][k] -= d * clamped[i][k];
  83.             }
  84.         }
  85.     }
  86.  
  87.     return true;
  88. }
  89.  
  90. /*
  91. ============
  92. idLCP_Square::SolveClamped
  93. ============
  94. */
  95. void idLCP_Square::SolveClamped( idVecX &x, const float *b ) {
  96.     int i, j;
  97.     float sum;
  98.  
  99.     // solve L
  100.     for ( i = 0; i < numClamped; i++ ) {
  101.         sum = b[i];
  102.         for ( j = 0; j < i; j++ ) {
  103.             sum -= clamped[i][j] * x[j];
  104.         }
  105.         x[i] = sum;
  106.     }
  107.  
  108.     // solve U
  109.     for ( i = numClamped - 1; i >= 0; i-- ) {
  110.         sum = x[i];
  111.         for ( j = i + 1; j < numClamped; j++ ) {
  112.             sum -= clamped[i][j] * x[j];
  113.         }
  114.         x[i] = sum * diagonal[i];
  115.     }
  116. }
  117.  
  118. /*
  119. ============
  120. idLCP_Square::Swap
  121. ============
  122. */
  123. void idLCP_Square::Swap( int i, int j ) {
  124.  
  125.     if ( i == j ) {
  126.         return;
  127.     }
  128.  
  129.     idSwap( rowPtrs[i], rowPtrs[j] );
  130.     m.SwapColumns( i, j );
  131.     b.SwapElements( i, j );
  132.     lo.SwapElements( i, j );
  133.     hi.SwapElements( i, j );
  134.     a.SwapElements( i, j );
  135.     f.SwapElements( i, j );
  136.     if ( boxIndex ) {
  137.         idSwap( boxIndex[i], boxIndex[j] );
  138.     }
  139.     idSwap( side[i], side[j] );
  140.     idSwap( permuted[i], permuted[j] );
  141. }
  142.  
  143. /*
  144. ============
  145. idLCP_Square::AddClamped
  146. ============
  147. */
  148. void idLCP_Square::AddClamped( int r ) {
  149.     int i, j;
  150.     float sum;
  151.  
  152.     assert( r >= numClamped );
  153.  
  154.     // add a row at the bottom and a column at the right of the factored
  155.     // matrix for the clamped variables
  156.  
  157.     Swap( numClamped, r );
  158.  
  159.     // add row to L
  160.     for ( i = 0; i < numClamped; i++ ) {
  161.         sum = rowPtrs[numClamped][i];
  162.         for ( j = 0; j < i; j++ ) {
  163.             sum -= clamped[numClamped][j] * clamped[j][i];
  164.         }
  165.         clamped[numClamped][i] = sum * diagonal[i];
  166.     }
  167.  
  168.     // add column to U
  169.     for ( i = 0; i <= numClamped; i++ ) {
  170.         sum = rowPtrs[i][numClamped];
  171.         for ( j = 0; j < i; j++ ) {
  172.             sum -= clamped[i][j] * clamped[j][numClamped];
  173.         }
  174.         clamped[i][numClamped] = sum;
  175.     }
  176.  
  177.     diagonal[numClamped] = 1.0f / clamped[numClamped][numClamped];
  178.  
  179.     numClamped++;
  180. }
  181.  
  182. /*
  183. ============
  184. idLCP_Square::RemoveClamped
  185. ============
  186. */
  187. void idLCP_Square::RemoveClamped( int r ) {
  188.     int i, j;
  189.     float *y0, *y1, *z0, *z1;
  190.     double diag, beta0, beta1, p0, p1, q0, q1, d;
  191.  
  192.     assert( r < numClamped );
  193.  
  194.     numClamped--;
  195.  
  196.     // no need to swap and update the factored matrix when the last row and column are removed
  197.     if ( r == numClamped ) {
  198.         return;
  199.     }
  200.  
  201.     y0 = (float *) _alloca16( numClamped * sizeof( float ) );
  202.     z0 = (float *) _alloca16( numClamped * sizeof( float ) );
  203.     y1 = (float *) _alloca16( numClamped * sizeof( float ) );
  204.     z1 = (float *) _alloca16( numClamped * sizeof( float ) );
  205.  
  206.     // the row/column need to be subtracted from the factorization
  207.     for ( i = 0; i < numClamped; i++ ) {
  208.         y0[i] = -rowPtrs[i][r];
  209.     }
  210.  
  211.     memset( y1, 0, numClamped * sizeof( float ) );
  212.     y1[r] = 1.0f;
  213.  
  214.     memset( z0, 0, numClamped * sizeof( float ) );
  215.     z0[r] = 1.0f;
  216.  
  217.     for ( i = 0; i < numClamped; i++ ) {
  218.         z1[i] = -rowPtrs[r][i];
  219.     }
  220.  
  221.     // swap the to be removed row/column with the last row/column
  222.     Swap( r, numClamped );
  223.  
  224.     // the swapped last row/column need to be added to the factorization
  225.     for ( i = 0; i < numClamped; i++ ) {
  226.         y0[i] += rowPtrs[i][r];
  227.     }
  228.  
  229.     for ( i = 0; i < numClamped; i++ ) {
  230.         z1[i] += rowPtrs[r][i];
  231.     }
  232.     z1[r] = 0.0f;
  233.  
  234.     // update the beginning of the to be updated row and column
  235.     for ( i = 0; i < r; i++ ) {
  236.         p0 = y0[i];
  237.         beta1 = z1[i] * diagonal[i];
  238.  
  239.         clamped[i][r] += p0;
  240.         for ( j = i+1; j < numClamped; j++ ) {
  241.             z1[j] -= beta1 * clamped[i][j];
  242.         }
  243.         for ( j = i+1; j < numClamped; j++ ) {
  244.             y0[j] -= p0 * clamped[j][i];
  245.         }
  246.         clamped[r][i] += beta1;
  247.     }
  248.  
  249.     // update the lower right corner starting at r,r
  250.     for ( i = r; i < numClamped; i++ ) {
  251.         diag = clamped[i][i];
  252.  
  253.         p0 = y0[i];
  254.         p1 = z0[i];
  255.         diag += p0 * p1;
  256.  
  257.         if ( diag == 0.0f ) {
  258.             idLib::common->Printf( "idLCP_Square::RemoveClamped: updating factorization failed\n" );
  259.             return;
  260.         }
  261.  
  262.         beta0 = p1 / diag;
  263.  
  264.         q0 = y1[i];
  265.         q1 = z1[i];
  266.         diag += q0 * q1;
  267.  
  268.         if ( diag == 0.0f ) {
  269.             idLib::common->Printf( "idLCP_Square::RemoveClamped: updating factorization failed\n" );
  270.             return;
  271.         }
  272.  
  273.         d = 1.0f / diag;
  274.         beta1 = q1 * d;
  275.  
  276.         clamped[i][i] = diag;
  277.         diagonal[i] = d;
  278.  
  279.         for ( j = i+1; j < numClamped; j++ ) {
  280.  
  281.             d = clamped[i][j];
  282.  
  283.             d += p0 * z0[j];
  284.             z0[j] -= beta0 * d;
  285.  
  286.             d += q0 * z1[j];
  287.             z1[j] -= beta1 * d;
  288.  
  289.             clamped[i][j] = d;
  290.         }
  291.  
  292.         for ( j = i+1; j < numClamped; j++ ) {
  293.  
  294.             d = clamped[j][i];
  295.  
  296.             y0[j] -= p0 * d;
  297.             d += beta0 * y0[j];
  298.  
  299.             y1[j] -= q0 * d;
  300.             d += beta1 * y1[j];
  301.  
  302.             clamped[j][i] = d;
  303.         }
  304.     }
  305.     return;
  306. }
  307.  
  308. /*
  309. ============
  310. idLCP_Square::CalcForceDelta
  311.  
  312.   modifies this->delta_f
  313. ============
  314. */
  315. ID_INLINE void idLCP_Square::CalcForceDelta( int d, float dir ) {
  316.     int i;
  317.     float *ptr;
  318.  
  319.     delta_f[d] = dir;
  320.  
  321.     if ( numClamped == 0 ) {
  322.         return;
  323.     }
  324.  
  325.     // get column d of matrix
  326.     ptr = (float *) _alloca16( numClamped * sizeof( float ) );
  327.     for ( i = 0; i < numClamped; i++ ) {
  328.         ptr[i] = rowPtrs[i][d];
  329.     }
  330.  
  331.     // solve force delta
  332.     SolveClamped( delta_f, ptr );
  333.  
  334.     // flip force delta based on direction
  335.     if ( dir > 0.0f ) {
  336.         ptr = delta_f.ToFloatPtr();
  337.         for ( i = 0; i < numClamped; i++ ) {
  338.             ptr[i] = - ptr[i];
  339.         }
  340.     }
  341. }
  342.  
  343. /*
  344. ============
  345. idLCP_Square::CalcAccelDelta
  346.  
  347.   modifies this->delta_a and uses this->delta_f
  348. ============
  349. */
  350. ID_INLINE void idLCP_Square::CalcAccelDelta( int d ) {
  351.     int j;
  352.     float dot;
  353.  
  354.     // only the not clamped variables, including the current variable, can have a change in acceleration
  355.     for ( j = numClamped; j <= d; j++ ) {
  356.         // only the clamped variables and the current variable have a force delta unequal zero
  357.         SIMDProcessor->Dot( dot, rowPtrs[j], delta_f.ToFloatPtr(), numClamped );
  358.         delta_a[j] = dot + rowPtrs[j][d] * delta_f[d];
  359.     }
  360. }
  361.  
  362. /*
  363. ============
  364. idLCP_Square::ChangeForce
  365.  
  366.   modifies this->f and uses this->delta_f
  367. ============
  368. */
  369. ID_INLINE void idLCP_Square::ChangeForce( int d, float step ) {
  370.     // only the clamped variables and current variable have a force delta unequal zero
  371.     SIMDProcessor->MulAdd( f.ToFloatPtr(), step, delta_f.ToFloatPtr(), numClamped );
  372.     f[d] += step * delta_f[d];
  373. }
  374.  
  375. /*
  376. ============
  377. idLCP_Square::ChangeAccel
  378.  
  379.   modifies this->a and uses this->delta_a
  380. ============
  381. */
  382. ID_INLINE void idLCP_Square::ChangeAccel( int d, float step ) {
  383.     // only the not clamped variables, including the current variable, can have an acceleration unequal zero
  384.     SIMDProcessor->MulAdd( a.ToFloatPtr() + numClamped, step, delta_a.ToFloatPtr() + numClamped, d - numClamped + 1 );
  385. }
  386.  
  387. /*
  388. ============
  389. idLCP_Square::GetMaxStep
  390. ============
  391. */
  392. void idLCP_Square::GetMaxStep( int d, float dir, float &maxStep, int &limit, int &limitSide ) const {
  393.     int i;
  394.     float s;
  395.  
  396.     // default to a full step for the current variable
  397.     if ( idMath::Fabs( delta_a[d] ) > LCP_DELTA_ACCEL_EPSILON ) {
  398.         maxStep = -a[d] / delta_a[d];
  399.     } else {
  400.         maxStep = 0.0f;
  401.     }
  402.     limit = d;
  403.     limitSide = 0;
  404.  
  405.     // test the current variable
  406.     if ( dir < 0.0f ) {
  407.         if ( lo[d] != -idMath::INFINITY ) {
  408.             s = ( lo[d] - f[d] ) / dir;
  409.             if ( s < maxStep ) {
  410.                 maxStep = s;
  411.                 limitSide = -1;
  412.             }
  413.         }
  414.     } else {
  415.         if ( hi[d] != idMath::INFINITY ) {
  416.             s = ( hi[d] - f[d] ) / dir;
  417.             if ( s < maxStep ) {
  418.                 maxStep = s;
  419.                 limitSide = 1;
  420.             }
  421.         }
  422.     }
  423.  
  424.     // test the clamped bounded variables
  425.     for ( i = numUnbounded; i < numClamped; i++ ) {
  426.         if ( delta_f[i] < -LCP_DELTA_FORCE_EPSILON ) {
  427.             // if there is a low boundary
  428.             if ( lo[i] != -idMath::INFINITY ) {
  429.                 s = ( lo[i] - f[i] ) / delta_f[i];
  430.                 if ( s < maxStep ) {
  431.                     maxStep = s;
  432.                     limit = i;
  433.                     limitSide = -1;
  434.                 }
  435.             }
  436.         } else if ( delta_f[i] > LCP_DELTA_FORCE_EPSILON ) {
  437.             // if there is a high boundary
  438.             if ( hi[i] != idMath::INFINITY ) {
  439.                 s = ( hi[i] - f[i] ) / delta_f[i];
  440.                 if ( s < maxStep ) {
  441.                     maxStep = s;
  442.                     limit = i;
  443.                     limitSide = 1;
  444.                 }
  445.             }
  446.         }
  447.     }
  448.  
  449.     // test the not clamped bounded variables
  450.     for ( i = numClamped; i < d; i++ ) {
  451.         if ( side[i] == -1 ) {
  452.             if ( delta_a[i] >= -LCP_DELTA_ACCEL_EPSILON ) {
  453.                 continue;
  454.             }
  455.         } else if ( side[i] == 1 ) {
  456.             if ( delta_a[i] <= LCP_DELTA_ACCEL_EPSILON ) {
  457.                 continue;
  458.             }
  459.         } else {
  460.             continue;
  461.         }
  462.         // ignore variables for which the force is not allowed to take any substantial value
  463.         if ( lo[i] >= -LCP_BOUND_EPSILON && hi[i] <= LCP_BOUND_EPSILON ) {
  464.             continue;
  465.         }
  466.         s = -a[i] / delta_a[i];
  467.         if ( s < maxStep ) {
  468.             maxStep = s;
  469.             limit = i;
  470.             limitSide = 0;
  471.         }
  472.     }
  473. }
  474.  
  475. /*
  476. ============
  477. idLCP_Square::Solve
  478. ============
  479. */
  480. bool idLCP_Square::Solve( const idMatX &o_m, idVecX &o_x, const idVecX &o_b, const idVecX &o_lo, const idVecX &o_hi, const int *o_boxIndex ) {
  481.     int i, j, n, limit, limitSide, boxStartIndex;
  482.     float dir, maxStep, dot, s;
  483.     char *failed;
  484.  
  485.     // true when the matrix rows are 16 byte padded
  486.     padded = ((o_m.GetNumRows()+3)&~3) == o_m.GetNumColumns();
  487.  
  488.     assert( padded || o_m.GetNumRows() == o_m.GetNumColumns() );
  489.     assert( o_x.GetSize() == o_m.GetNumRows() );
  490.     assert( o_b.GetSize() == o_m.GetNumRows() );
  491.     assert( o_lo.GetSize() == o_m.GetNumRows() );
  492.     assert( o_hi.GetSize() == o_m.GetNumRows() );
  493.  
  494.     // allocate memory for permuted input
  495.     f.SetData( o_m.GetNumRows(), VECX_ALLOCA( o_m.GetNumRows() ) );
  496.     a.SetData( o_b.GetSize(), VECX_ALLOCA( o_b.GetSize() ) );
  497.     b.SetData( o_b.GetSize(), VECX_ALLOCA( o_b.GetSize() ) );
  498.     lo.SetData( o_lo.GetSize(), VECX_ALLOCA( o_lo.GetSize() ) );
  499.     hi.SetData( o_hi.GetSize(), VECX_ALLOCA( o_hi.GetSize() ) );
  500.     if ( o_boxIndex ) {
  501.         boxIndex = (int *)_alloca16( o_x.GetSize() * sizeof( int ) );
  502.         memcpy( boxIndex, o_boxIndex, o_x.GetSize() * sizeof( int ) );
  503.     } else {
  504.         boxIndex = NULL;
  505.     }
  506.  
  507.     // we override the const on o_m here but on exit the matrix is unchanged
  508.     m.SetData( o_m.GetNumRows(), o_m.GetNumColumns(), const_cast<float *>(o_m[0]) );
  509.     f.Zero();
  510.     a.Zero();
  511.     b = o_b;
  512.     lo = o_lo;
  513.     hi = o_hi;
  514.  
  515.     // pointers to the rows of m
  516.     rowPtrs = (float **) _alloca16( m.GetNumRows() * sizeof( float * ) );
  517.     for ( i = 0; i < m.GetNumRows(); i++ ) {
  518.         rowPtrs[i] = m[i];
  519.     }
  520.  
  521.     // tells if a variable is at the low boundary, high boundary or inbetween
  522.     side = (int *) _alloca16( m.GetNumRows() * sizeof( int ) );
  523.  
  524.     // index to keep track of the permutation
  525.     permuted = (int *) _alloca16( m.GetNumRows() * sizeof( int ) );
  526.     for ( i = 0; i < m.GetNumRows(); i++ ) {
  527.         permuted[i] = i;
  528.     }
  529.  
  530.     // permute input so all unbounded variables come first
  531.     numUnbounded = 0;
  532.     for ( i = 0; i < m.GetNumRows(); i++ ) {
  533.         if ( lo[i] == -idMath::INFINITY && hi[i] == idMath::INFINITY ) {
  534.             if ( numUnbounded != i ) {
  535.                 Swap( numUnbounded, i );
  536.             }
  537.             numUnbounded++;
  538.         }
  539.     }
  540.  
  541.     // permute input so all variables using the boxIndex come last
  542.     boxStartIndex = m.GetNumRows();
  543.     if ( boxIndex ) {
  544.         for ( i = m.GetNumRows() - 1; i >= numUnbounded; i-- ) {
  545.             if ( boxIndex[i] >= 0 && ( lo[i] != -idMath::INFINITY || hi[i] != idMath::INFINITY ) ) {
  546.                 boxStartIndex--;
  547.                 if ( boxStartIndex != i ) {
  548.                     Swap( boxStartIndex, i );
  549.                 }
  550.             }
  551.         }
  552.     }
  553.  
  554.     // sub matrix for factorization 
  555.     clamped.SetData( m.GetNumRows(), m.GetNumColumns(), MATX_ALLOCA( m.GetNumRows() * m.GetNumColumns() ) );
  556.     diagonal.SetData( m.GetNumRows(), VECX_ALLOCA( m.GetNumRows() ) );
  557.  
  558.     // all unbounded variables are clamped
  559.     numClamped = numUnbounded;
  560.  
  561.     // if there are unbounded variables
  562.     if ( numUnbounded ) {
  563.  
  564.         // factor and solve for unbounded variables
  565.         if ( !FactorClamped() ) {
  566.             idLib::common->Printf( "idLCP_Square::Solve: unbounded factorization failed\n" );
  567.             return false;
  568.         }
  569.         SolveClamped( f, b.ToFloatPtr() );
  570.  
  571.         // if there are no bounded variables we are done
  572.         if ( numUnbounded == m.GetNumRows() ) {
  573.             o_x = f;    // the vector is not permuted
  574.             return true;
  575.         }
  576.     }
  577.  
  578. #ifdef IGNORE_UNSATISFIABLE_VARIABLES
  579.     int numIgnored = 0;
  580. #endif
  581.  
  582.     // allocate for delta force and delta acceleration
  583.     delta_f.SetData( m.GetNumRows(), VECX_ALLOCA( m.GetNumRows() ) );
  584.     delta_a.SetData( m.GetNumRows(), VECX_ALLOCA( m.GetNumRows() ) );
  585.  
  586.     // solve for bounded variables
  587.     failed = NULL;
  588.     for ( i = numUnbounded; i < m.GetNumRows(); i++ ) {
  589.  
  590.         // once we hit the box start index we can initialize the low and high boundaries of the variables using the box index
  591.         if ( i == boxStartIndex ) {
  592.             for ( j = 0; j < boxStartIndex; j++ ) {
  593.                 o_x[permuted[j]] = f[j];
  594.             }
  595.             for ( j = boxStartIndex; j < m.GetNumRows(); j++ ) {
  596.                 s = o_x[boxIndex[j]];
  597.                 if ( lo[j] != -idMath::INFINITY ) {
  598.                     lo[j] = - idMath::Fabs( lo[j] * s );
  599.                 }
  600.                 if ( hi[j] != idMath::INFINITY ) {
  601.                     hi[j] = idMath::Fabs( hi[j] * s );
  602.                 }
  603.             }
  604.         }
  605.  
  606.         // calculate acceleration for current variable
  607.         SIMDProcessor->Dot( dot, rowPtrs[i], f.ToFloatPtr(), i );
  608.         a[i] = dot - b[i];
  609.  
  610.         // if already at the low boundary
  611.         if ( lo[i] >= -LCP_BOUND_EPSILON && a[i] >= -LCP_ACCEL_EPSILON ) {
  612.             side[i] = -1;
  613.             continue;
  614.         }
  615.  
  616.         // if already at the high boundary
  617.         if ( hi[i] <= LCP_BOUND_EPSILON && a[i] <= LCP_ACCEL_EPSILON ) {
  618.             side[i] = 1;
  619.             continue;
  620.         }
  621.  
  622.         // if inside the clamped region
  623.         if ( idMath::Fabs( a[i] ) <= LCP_ACCEL_EPSILON ) {
  624.             side[i] = 0;
  625.             AddClamped( i );
  626.             continue;
  627.         }
  628.  
  629.         // drive the current variable into a valid region
  630.         for ( n = 0; n < maxIterations; n++ ) {
  631.  
  632.             // direction to move
  633.             if ( a[i] <= 0.0f ) {
  634.                 dir = 1.0f;
  635.             } else {
  636.                 dir = -1.0f;
  637.             }
  638.  
  639.             // calculate force delta
  640.             CalcForceDelta( i, dir );
  641.  
  642.             // calculate acceleration delta: delta_a = m * delta_f;
  643.             CalcAccelDelta( i );
  644.  
  645.             // maximum step we can take
  646.             GetMaxStep( i, dir, maxStep, limit, limitSide );
  647.  
  648.             if ( maxStep <= 0.0f ) {
  649. #ifdef IGNORE_UNSATISFIABLE_VARIABLES
  650.                 // ignore the current variable completely
  651.                 lo[i] = hi[i] = 0.0f;
  652.                 f[i] = 0.0f;
  653.                 side[i] = -1;
  654.                 numIgnored++;
  655. #else
  656.                 failed = va( "invalid step size %.4f", maxStep );
  657. #endif
  658.                 break;
  659.             }
  660.  
  661.             // change force
  662.             ChangeForce( i, maxStep );
  663.  
  664.             // change acceleration
  665.             ChangeAccel( i, maxStep );
  666.  
  667.             // clamp/unclamp the variable that limited this step
  668.             side[limit] = limitSide;
  669.             switch( limitSide ) {
  670.                 case 0: {
  671.                     a[limit] = 0.0f;
  672.                     AddClamped( limit );
  673.                     break;
  674.                 }
  675.                 case -1: {
  676.                     f[limit] = lo[limit];
  677.                     if ( limit != i ) {
  678.                         RemoveClamped( limit );
  679.                     }
  680.                     break;
  681.                 }
  682.                 case 1: {
  683.                     f[limit] = hi[limit];
  684.                     if ( limit != i ) {
  685.                         RemoveClamped( limit );
  686.                     }
  687.                     break;
  688.                 }
  689.             }
  690.  
  691.             // if the current variable limited the step we can continue with the next variable
  692.             if ( limit == i ) {
  693.                 break;
  694.             }
  695.         }
  696.  
  697.         if ( n >= maxIterations ) {
  698.             failed = va( "max iterations %d", maxIterations );
  699.             break;
  700.         }
  701.  
  702.         if ( failed ) {
  703.             break;
  704.         }
  705.     }
  706.  
  707. #ifdef IGNORE_UNSATISFIABLE_VARIABLES
  708.     if ( numIgnored ) {
  709.         if ( lcp_showFailures.GetBool() ) {
  710.             idLib::common->Printf( "idLCP_Symmetric::Solve: %d of %d bounded variables ignored\n", numIgnored, m.GetNumRows() - numUnbounded );
  711.         }
  712.     }
  713. #endif
  714.  
  715.     // if failed clear remaining forces
  716.     if ( failed ) {
  717.         if ( lcp_showFailures.GetBool() ) {
  718.             idLib::common->Printf( "idLCP_Square::Solve: %s (%d of %d bounded variables ignored)\n", failed, m.GetNumRows() - i, m.GetNumRows() - numUnbounded );
  719.         }
  720.         for ( j = i; j < m.GetNumRows(); j++ ) {
  721.             f[j] = 0.0f;
  722.         }
  723.     }
  724.  
  725. #if defined(_DEBUG) && 0
  726.     if ( !failed ) {
  727.         // test whether or not the solution satisfies the complementarity conditions
  728.         for ( i = 0; i < m.GetNumRows(); i++ ) {
  729.             a[i] = -b[i];
  730.             for ( j = 0; j < m.GetNumRows(); j++ ) {
  731.                 a[i] += rowPtrs[i][j] * f[j];
  732.             }
  733.  
  734.             if ( f[i] == lo[i] ) {
  735.                 if ( lo[i] != hi[i] && a[i] < -LCP_ACCEL_EPSILON ) {
  736.                     int bah1 = 1;
  737.                 }
  738.             } else if ( f[i] == hi[i] ) {
  739.                 if ( lo[i] != hi[i] && a[i] > LCP_ACCEL_EPSILON ) {
  740.                     int bah2 = 1;
  741.                 }
  742.             } else if ( f[i] < lo[i] || f[i] > hi[i] || idMath::Fabs( a[i] ) > 1.0f ) {
  743.                 int bah3 = 1;
  744.             }
  745.         }
  746.     }
  747. #endif
  748.  
  749.     // unpermute result
  750.     for ( i = 0; i < f.GetSize(); i++ ) {
  751.         o_x[permuted[i]] = f[i];
  752.     }
  753.  
  754.     // unpermute original matrix
  755.     for ( i = 0; i < m.GetNumRows(); i++ ) {
  756.         for ( j = 0; j < m.GetNumRows(); j++ ) {
  757.             if ( permuted[j] == i ) {
  758.                 break;
  759.             }
  760.         }
  761.         if ( i != j ) {
  762.             m.SwapColumns( i, j );
  763.             idSwap( permuted[i], permuted[j] );
  764.         }
  765.     }
  766.  
  767.     return true;
  768. }
  769.  
  770.  
  771. //===============================================================
  772. //                                                        M
  773. //  idLCP_Symmetric                                      MrE
  774. //                                                        E
  775. //===============================================================
  776.  
  777. class idLCP_Symmetric : public idLCP {
  778. public:
  779.     virtual bool    Solve( const idMatX &o_m, idVecX &o_x, const idVecX &o_b, const idVecX &o_lo, const idVecX &o_hi, const int *o_boxIndex );
  780.  
  781. private:
  782.     idMatX            m;                    // original matrix
  783.     idVecX            b;                    // right hand side
  784.     idVecX            lo, hi;                // low and high bounds
  785.     idVecX            f, a;                // force and acceleration
  786.     idVecX            delta_f, delta_a;    // delta force and delta acceleration
  787.     idMatX            clamped;            // LDLt factored sub matrix for clamped variables
  788.     idVecX            diagonal;            // reciprocal of diagonal of LDLt factored sub matrix for clamped variables
  789.     idVecX            solveCache1;        // intermediate result cached in SolveClamped
  790.     idVecX            solveCache2;        // "
  791.     int                numUnbounded;        // number of unbounded variables
  792.     int                numClamped;            // number of clamped variables
  793.     int                clampedChangeStart;    // lowest row/column changed in the clamped matrix during an iteration
  794.     float **        rowPtrs;            // pointers to the rows of m
  795.     int *            boxIndex;            // box index
  796.     int *            side;                // tells if a variable is at the low boundary = -1, high boundary = 1 or inbetween = 0
  797.     int *            permuted;            // index to keep track of the permutation
  798.     bool            padded;                // set to true if the rows of the initial matrix are 16 byte padded
  799.  
  800. private:
  801.     bool            FactorClamped( void );
  802.     void            SolveClamped( idVecX &x, const float *b );
  803.     void            Swap( int i, int j );
  804.     void            AddClamped( int r, bool useSolveCache );
  805.     void            RemoveClamped( int r );
  806.     void            CalcForceDelta( int d, float dir );
  807.     void            CalcAccelDelta( int d );
  808.     void            ChangeForce( int d, float step );
  809.     void            ChangeAccel( int d, float step );
  810.     void            GetMaxStep( int d, float dir, float &maxStep, int &limit, int &limitSide ) const;
  811. };
  812.  
  813. /*
  814. ============
  815. idLCP_Symmetric::FactorClamped
  816. ============
  817. */
  818. bool idLCP_Symmetric::FactorClamped( void ) {
  819.  
  820.     clampedChangeStart = 0;
  821.  
  822.     for ( int i = 0; i < numClamped; i++ ) {
  823.         memcpy( clamped[i], rowPtrs[i], numClamped * sizeof( float ) );
  824.     }
  825.     return SIMDProcessor->MatX_LDLTFactor( clamped, diagonal, numClamped );
  826. }
  827.  
  828. /*
  829. ============
  830. idLCP_Symmetric::SolveClamped
  831. ============
  832. */
  833. void idLCP_Symmetric::SolveClamped( idVecX &x, const float *b ) {
  834.  
  835.     // solve L
  836.     SIMDProcessor->MatX_LowerTriangularSolve( clamped, solveCache1.ToFloatPtr(), b, numClamped, clampedChangeStart );
  837.  
  838.     // solve D
  839.     SIMDProcessor->Mul( solveCache2.ToFloatPtr(), solveCache1.ToFloatPtr(), diagonal.ToFloatPtr(), numClamped );
  840.  
  841.     // solve Lt
  842.     SIMDProcessor->MatX_LowerTriangularSolveTranspose( clamped, x.ToFloatPtr(), solveCache2.ToFloatPtr(), numClamped );
  843.  
  844.     clampedChangeStart = numClamped;
  845. }
  846.  
  847. /*
  848. ============
  849. idLCP_Symmetric::Swap
  850. ============
  851. */
  852. void idLCP_Symmetric::Swap( int i, int j ) {
  853.  
  854.     if ( i == j ) {
  855.         return;
  856.     }
  857.  
  858.     idSwap( rowPtrs[i], rowPtrs[j] );
  859.     m.SwapColumns( i, j );
  860.     b.SwapElements( i, j );
  861.     lo.SwapElements( i, j );
  862.     hi.SwapElements( i, j );
  863.     a.SwapElements( i, j );
  864.     f.SwapElements( i, j );
  865.     if ( boxIndex ) {
  866.         idSwap( boxIndex[i], boxIndex[j] );
  867.     }
  868.     idSwap( side[i], side[j] );
  869.     idSwap( permuted[i], permuted[j] );
  870. }
  871.  
  872. /*
  873. ============
  874. idLCP_Symmetric::AddClamped
  875. ============
  876. */
  877. void idLCP_Symmetric::AddClamped( int r, bool useSolveCache ) {
  878.     float d, dot;
  879.  
  880.     assert( r >= numClamped );
  881.  
  882.     if ( numClamped < clampedChangeStart ) {
  883.         clampedChangeStart = numClamped;
  884.     }
  885.  
  886.     // add a row at the bottom and a column at the right of the factored
  887.     // matrix for the clamped variables
  888.  
  889.     Swap( numClamped, r );
  890.  
  891.     // solve for v in L * v = rowPtr[numClamped]
  892.     if ( useSolveCache ) {
  893.  
  894.         // the lower triangular solve was cached in SolveClamped called by CalcForceDelta
  895.         memcpy( clamped[numClamped], solveCache2.ToFloatPtr(), numClamped * sizeof( float ) );
  896.         // calculate row dot product
  897.         SIMDProcessor->Dot( dot, solveCache2.ToFloatPtr(), solveCache1.ToFloatPtr(), numClamped );
  898.  
  899.     } else {
  900.  
  901.         float *v = (float *) _alloca16( numClamped * sizeof( float ) );
  902.  
  903.         SIMDProcessor->MatX_LowerTriangularSolve( clamped, v, rowPtrs[numClamped], numClamped );
  904.         // add bottom row to L
  905.         SIMDProcessor->Mul( clamped[numClamped], v, diagonal.ToFloatPtr(), numClamped );
  906.         // calculate row dot product
  907.         SIMDProcessor->Dot( dot, clamped[numClamped], v, numClamped );
  908.     }
  909.  
  910.     // update diagonal[numClamped]
  911.     d = rowPtrs[numClamped][numClamped] - dot;
  912.  
  913.     if ( d == 0.0f ) {
  914.         idLib::common->Printf( "idLCP_Symmetric::AddClamped: updating factorization failed\n" );
  915.         numClamped++;
  916.         return;
  917.     }
  918.  
  919.     clamped[numClamped][numClamped] = d;
  920.     diagonal[numClamped] = 1.0f / d;
  921.  
  922.     numClamped++;
  923. }
  924.  
  925. /*
  926. ============
  927. idLCP_Symmetric::RemoveClamped
  928. ============
  929. */
  930. void idLCP_Symmetric::RemoveClamped( int r ) {
  931.     int i, j, n;
  932.     float *addSub, *original, *v, *ptr, *v1, *v2, dot;
  933.     double sum, diag, newDiag, invNewDiag, p1, p2, alpha1, alpha2, beta1, beta2;
  934.  
  935.     assert( r < numClamped );
  936.  
  937.     if ( r < clampedChangeStart ) {
  938.         clampedChangeStart = r;
  939.     }
  940.  
  941.     numClamped--;
  942.  
  943.     // no need to swap and update the factored matrix when the last row and column are removed
  944.     if ( r == numClamped ) {
  945.         return;
  946.     }
  947.  
  948.     // swap the to be removed row/column with the last row/column
  949.     Swap( r, numClamped );
  950.  
  951.     // update the factored matrix
  952.     addSub = (float *) _alloca16( numClamped * sizeof( float ) );
  953.  
  954.     if ( r == 0 ) {
  955.  
  956.         if ( numClamped == 1 ) {
  957.             diag = rowPtrs[0][0];
  958.             if ( diag == 0.0f ) {
  959.                 idLib::common->Printf( "idLCP_Symmetric::RemoveClamped: updating factorization failed\n" );
  960.                 return;
  961.             }
  962.             clamped[0][0] = diag;
  963.             diagonal[0] = 1.0f / diag;
  964.             return;
  965.         }
  966.  
  967.         // calculate the row/column to be added to the lower right sub matrix starting at (r, r)
  968.         original = rowPtrs[numClamped];
  969.         ptr = rowPtrs[r];
  970.         addSub[0] = ptr[0] - original[numClamped];
  971.         for ( i = 1; i < numClamped; i++ ) {
  972.             addSub[i] = ptr[i] - original[i];
  973.         }
  974.  
  975.     } else {
  976.  
  977.         v = (float *) _alloca16( numClamped * sizeof( float ) );
  978.  
  979.         // solve for v in L * v = rowPtr[r]
  980.         SIMDProcessor->MatX_LowerTriangularSolve( clamped, v, rowPtrs[r], r );
  981.  
  982.         // update removed row
  983.         SIMDProcessor->Mul( clamped[r], v, diagonal.ToFloatPtr(), r );
  984.  
  985.         // if the last row/column of the matrix is updated
  986.         if ( r == numClamped - 1 ) {
  987.             // only calculate new diagonal
  988.             SIMDProcessor->Dot( dot, clamped[r], v, r );
  989.             diag = rowPtrs[r][r] - dot;
  990.             if ( diag == 0.0f ) {
  991.                 idLib::common->Printf( "idLCP_Symmetric::RemoveClamped: updating factorization failed\n" );
  992.                 return;
  993.             }
  994.             clamped[r][r] = diag;
  995.             diagonal[r] = 1.0f / diag;
  996.             return;
  997.         }
  998.  
  999.         // calculate the row/column to be added to the lower right sub matrix starting at (r, r)
  1000.         for ( i = 0; i < r; i++ ) {
  1001.             v[i] = clamped[r][i] * clamped[i][i];
  1002.         }
  1003.         for ( i = r; i < numClamped; i++ ) {
  1004.             if ( i == r ) {
  1005.                 sum = clamped[r][r];
  1006.             } else {
  1007.                 sum = clamped[r][r] * clamped[i][r];
  1008.             }
  1009.             ptr = clamped[i];
  1010.             for ( j = 0; j < r; j++ ) {
  1011.                 sum += ptr[j] * v[j];
  1012.             }
  1013.             addSub[i] = rowPtrs[r][i] - sum;
  1014.         }
  1015.     }
  1016.  
  1017.     // add row/column to the lower right sub matrix starting at (r, r)
  1018.  
  1019.     v1 = (float *) _alloca16( numClamped * sizeof( float ) );
  1020.     v2 = (float *) _alloca16( numClamped * sizeof( float ) );
  1021.  
  1022.     diag = idMath::SQRT_1OVER2;
  1023.     v1[r] = ( 0.5f * addSub[r] + 1.0f ) * diag;
  1024.     v2[r] = ( 0.5f * addSub[r] - 1.0f ) * diag;
  1025.     for ( i = r+1; i < numClamped; i++ ) {
  1026.         v1[i] = v2[i] = addSub[i] * diag;
  1027.     }
  1028.  
  1029.     alpha1 = 1.0f;
  1030.     alpha2 = -1.0f;
  1031.  
  1032.     // simultaneous update/downdate of the sub matrix starting at (r, r)
  1033.     n = clamped.GetNumColumns();
  1034.     for ( i = r; i < numClamped; i++ ) {
  1035.  
  1036.         diag = clamped[i][i];
  1037.         p1 = v1[i];
  1038.         newDiag = diag + alpha1 * p1 * p1;
  1039.  
  1040.         if ( newDiag == 0.0f ) {
  1041.             idLib::common->Printf( "idLCP_Symmetric::RemoveClamped: updating factorization failed\n" );
  1042.             return;
  1043.         }
  1044.  
  1045.         alpha1 /= newDiag;
  1046.         beta1 = p1 * alpha1;
  1047.         alpha1 *= diag;
  1048.  
  1049.         diag = newDiag;
  1050.         p2 = v2[i];
  1051.         newDiag = diag + alpha2 * p2 * p2;
  1052.  
  1053.         if ( newDiag == 0.0f ) {
  1054.             idLib::common->Printf( "idLCP_Symmetric::RemoveClamped: updating factorization failed\n" );
  1055.             return;
  1056.         }
  1057.  
  1058.         clamped[i][i] = newDiag;
  1059.         diagonal[i] = invNewDiag = 1.0f / newDiag;
  1060.  
  1061.         alpha2 *= invNewDiag;
  1062.         beta2 = p2 * alpha2;
  1063.         alpha2 *= diag;
  1064.  
  1065.         // update column below diagonal (i,i)
  1066.         ptr = clamped.ToFloatPtr() + i;
  1067.  
  1068.         for ( j = i+1; j < numClamped - 1; j += 2 ) {
  1069.  
  1070.             float sum0 = ptr[(j+0)*n];
  1071.             float sum1 = ptr[(j+1)*n];
  1072.  
  1073.             v1[j+0] -= p1 * sum0;
  1074.             v1[j+1] -= p1 * sum1;
  1075.  
  1076.             sum0 += beta1 * v1[j+0];
  1077.             sum1 += beta1 * v1[j+1];
  1078.  
  1079.             v2[j+0] -= p2 * sum0;
  1080.             v2[j+1] -= p2 * sum1;
  1081.  
  1082.             sum0 += beta2 * v2[j+0];
  1083.             sum1 += beta2 * v2[j+1];
  1084.  
  1085.             ptr[(j+0)*n] = sum0;
  1086.             ptr[(j+1)*n] = sum1;
  1087.         }
  1088.  
  1089.         for ( ; j < numClamped; j++ ) {
  1090.  
  1091.             sum = ptr[j*n];
  1092.  
  1093.             v1[j] -= p1 * sum;
  1094.             sum += beta1 * v1[j];
  1095.  
  1096.             v2[j] -= p2 * sum;
  1097.             sum += beta2 * v2[j];
  1098.  
  1099.             ptr[j*n] = sum;
  1100.         }
  1101.     }
  1102. }
  1103.  
  1104. /*
  1105. ============
  1106. idLCP_Symmetric::CalcForceDelta
  1107.  
  1108.   modifies this->delta_f
  1109. ============
  1110. */
  1111. ID_INLINE void idLCP_Symmetric::CalcForceDelta( int d, float dir ) {
  1112.     int i;
  1113.     float *ptr;
  1114.  
  1115.     delta_f[d] = dir;
  1116.  
  1117.     if ( numClamped == 0 ) {
  1118.         return;
  1119.     }
  1120.  
  1121.     // solve force delta
  1122.     SolveClamped( delta_f, rowPtrs[d] );
  1123.  
  1124.     // flip force delta based on direction
  1125.     if ( dir > 0.0f ) {
  1126.         ptr = delta_f.ToFloatPtr();
  1127.         for ( i = 0; i < numClamped; i++ ) {
  1128.             ptr[i] = - ptr[i];
  1129.         }
  1130.     }
  1131. }
  1132.  
  1133. /*
  1134. ============
  1135. idLCP_Symmetric::CalcAccelDelta
  1136.  
  1137.   modifies this->delta_a and uses this->delta_f
  1138. ============
  1139. */
  1140. ID_INLINE void idLCP_Symmetric::CalcAccelDelta( int d ) {
  1141.     int j;
  1142.     float dot;
  1143.  
  1144.     // only the not clamped variables, including the current variable, can have a change in acceleration
  1145.     for ( j = numClamped; j <= d; j++ ) {
  1146.         // only the clamped variables and the current variable have a force delta unequal zero
  1147.         SIMDProcessor->Dot( dot, rowPtrs[j], delta_f.ToFloatPtr(), numClamped );
  1148.         delta_a[j] = dot + rowPtrs[j][d] * delta_f[d];
  1149.     }
  1150. }
  1151.  
  1152. /*
  1153. ============
  1154. idLCP_Symmetric::ChangeForce
  1155.  
  1156.   modifies this->f and uses this->delta_f
  1157. ============
  1158. */
  1159. ID_INLINE void idLCP_Symmetric::ChangeForce( int d, float step ) {
  1160.     // only the clamped variables and current variable have a force delta unequal zero
  1161.     SIMDProcessor->MulAdd( f.ToFloatPtr(), step, delta_f.ToFloatPtr(), numClamped );
  1162.     f[d] += step * delta_f[d];
  1163. }
  1164.  
  1165. /*
  1166. ============
  1167. idLCP_Symmetric::ChangeAccel
  1168.  
  1169.   modifies this->a and uses this->delta_a
  1170. ============
  1171. */
  1172. ID_INLINE void idLCP_Symmetric::ChangeAccel( int d, float step ) {
  1173.     // only the not clamped variables, including the current variable, can have an acceleration unequal zero
  1174.     SIMDProcessor->MulAdd( a.ToFloatPtr() + numClamped, step, delta_a.ToFloatPtr() + numClamped, d - numClamped + 1 );
  1175. }
  1176.  
  1177. /*
  1178. ============
  1179. idLCP_Symmetric::GetMaxStep
  1180. ============
  1181. */
  1182. void idLCP_Symmetric::GetMaxStep( int d, float dir, float &maxStep, int &limit, int &limitSide ) const {
  1183.     int i;
  1184.     float s;
  1185.  
  1186.     // default to a full step for the current variable
  1187.     if ( idMath::Fabs( delta_a[d] ) > LCP_DELTA_ACCEL_EPSILON ) {
  1188.         maxStep = -a[d] / delta_a[d];
  1189.     } else {
  1190.         maxStep = 0.0f;
  1191.     }
  1192.     limit = d;
  1193.     limitSide = 0;
  1194.  
  1195.     // test the current variable
  1196.     if ( dir < 0.0f ) {
  1197.         if ( lo[d] != -idMath::INFINITY ) {
  1198.             s = ( lo[d] - f[d] ) / dir;
  1199.             if ( s < maxStep ) {
  1200.                 maxStep = s;
  1201.                 limitSide = -1;
  1202.             }
  1203.         }
  1204.     } else {
  1205.         if ( hi[d] != idMath::INFINITY ) {
  1206.             s = ( hi[d] - f[d] ) / dir;
  1207.             if ( s < maxStep ) {
  1208.                 maxStep = s;
  1209.                 limitSide = 1;
  1210.             }
  1211.         }
  1212.     }
  1213.  
  1214.     // test the clamped bounded variables
  1215.     for ( i = numUnbounded; i < numClamped; i++ ) {
  1216.         if ( delta_f[i] < -LCP_DELTA_FORCE_EPSILON ) {
  1217.             // if there is a low boundary
  1218.             if ( lo[i] != -idMath::INFINITY ) {
  1219.                 s = ( lo[i] - f[i] ) / delta_f[i];
  1220.                 if ( s < maxStep ) {
  1221.                     maxStep = s;
  1222.                     limit = i;
  1223.                     limitSide = -1;
  1224.                 }
  1225.             }
  1226.         } else if ( delta_f[i] > LCP_DELTA_FORCE_EPSILON ) {
  1227.             // if there is a high boundary
  1228.             if ( hi[i] != idMath::INFINITY ) {
  1229.                 s = ( hi[i] - f[i] ) / delta_f[i];
  1230.                 if ( s < maxStep ) {
  1231.                     maxStep = s;
  1232.                     limit = i;
  1233.                     limitSide = 1;
  1234.                 }
  1235.             }
  1236.         }
  1237.     }
  1238.  
  1239.     // test the not clamped bounded variables
  1240.     for ( i = numClamped; i < d; i++ ) {
  1241.         if ( side[i] == -1 ) {
  1242.             if ( delta_a[i] >= -LCP_DELTA_ACCEL_EPSILON ) {
  1243.                 continue;
  1244.             }
  1245.         } else if ( side[i] == 1 ) {
  1246.             if ( delta_a[i] <= LCP_DELTA_ACCEL_EPSILON ) {
  1247.                 continue;
  1248.             }
  1249.         } else {
  1250.             continue;
  1251.         }
  1252.         // ignore variables for which the force is not allowed to take any substantial value
  1253.         if ( lo[i] >= -LCP_BOUND_EPSILON && hi[i] <= LCP_BOUND_EPSILON ) {
  1254.             continue;
  1255.         }
  1256.         s = -a[i] / delta_a[i];
  1257.         if ( s < maxStep ) {
  1258.             maxStep = s;
  1259.             limit = i;
  1260.             limitSide = 0;
  1261.         }
  1262.     }
  1263. }
  1264.  
  1265. /*
  1266. ============
  1267. idLCP_Symmetric::Solve
  1268. ============
  1269. */
  1270. bool idLCP_Symmetric::Solve( const idMatX &o_m, idVecX &o_x, const idVecX &o_b, const idVecX &o_lo, const idVecX &o_hi, const int *o_boxIndex ) {
  1271.     int i, j, n, limit, limitSide, boxStartIndex;
  1272.     float dir, maxStep, dot, s;
  1273.     char *failed;
  1274.  
  1275.     // true when the matrix rows are 16 byte padded
  1276.     padded = ((o_m.GetNumRows()+3)&~3) == o_m.GetNumColumns();
  1277.  
  1278.     assert( padded || o_m.GetNumRows() == o_m.GetNumColumns() );
  1279.     assert( o_x.GetSize() == o_m.GetNumRows() );
  1280.     assert( o_b.GetSize() == o_m.GetNumRows() );
  1281.     assert( o_lo.GetSize() == o_m.GetNumRows() );
  1282.     assert( o_hi.GetSize() == o_m.GetNumRows() );
  1283.  
  1284.     // allocate memory for permuted input
  1285.     f.SetData( o_m.GetNumRows(), VECX_ALLOCA( o_m.GetNumRows() ) );
  1286.     a.SetData( o_b.GetSize(), VECX_ALLOCA( o_b.GetSize() ) );
  1287.     b.SetData( o_b.GetSize(), VECX_ALLOCA( o_b.GetSize() ) );
  1288.     lo.SetData( o_lo.GetSize(), VECX_ALLOCA( o_lo.GetSize() ) );
  1289.     hi.SetData( o_hi.GetSize(), VECX_ALLOCA( o_hi.GetSize() ) );
  1290.     if ( o_boxIndex ) {
  1291.         boxIndex = (int *)_alloca16( o_x.GetSize() * sizeof( int ) );
  1292.         memcpy( boxIndex, o_boxIndex, o_x.GetSize() * sizeof( int ) );
  1293.     } else {
  1294.         boxIndex = NULL;
  1295.     }
  1296.  
  1297.     // we override the const on o_m here but on exit the matrix is unchanged
  1298.     m.SetData( o_m.GetNumRows(), o_m.GetNumColumns(), const_cast<float *>(o_m[0]) );
  1299.     f.Zero();
  1300.     a.Zero();
  1301.     b = o_b;
  1302.     lo = o_lo;
  1303.     hi = o_hi;
  1304.  
  1305.     // pointers to the rows of m
  1306.     rowPtrs = (float **) _alloca16( m.GetNumRows() * sizeof( float * ) );
  1307.     for ( i = 0; i < m.GetNumRows(); i++ ) {
  1308.         rowPtrs[i] = m[i];
  1309.     }
  1310.  
  1311.     // tells if a variable is at the low boundary, high boundary or inbetween
  1312.     side = (int *) _alloca16( m.GetNumRows() * sizeof( int ) );
  1313.  
  1314.     // index to keep track of the permutation
  1315.     permuted = (int *) _alloca16( m.GetNumRows() * sizeof( int ) );
  1316.     for ( i = 0; i < m.GetNumRows(); i++ ) {
  1317.         permuted[i] = i;
  1318.     }
  1319.  
  1320.     // permute input so all unbounded variables come first
  1321.     numUnbounded = 0;
  1322.     for ( i = 0; i < m.GetNumRows(); i++ ) {
  1323.         if ( lo[i] == -idMath::INFINITY && hi[i] == idMath::INFINITY ) {
  1324.             if ( numUnbounded != i ) {
  1325.                 Swap( numUnbounded, i );
  1326.             }
  1327.             numUnbounded++;
  1328.         }
  1329.     }
  1330.  
  1331.     // permute input so all variables using the boxIndex come last
  1332.     boxStartIndex = m.GetNumRows();
  1333.     if ( boxIndex ) {
  1334.         for ( i = m.GetNumRows() - 1; i >= numUnbounded; i-- ) {
  1335.             if ( boxIndex[i] >= 0 && ( lo[i] != -idMath::INFINITY || hi[i] != idMath::INFINITY ) ) {
  1336.                 boxStartIndex--;
  1337.                 if ( boxStartIndex != i ) {
  1338.                     Swap( boxStartIndex, i );
  1339.                 }
  1340.             }
  1341.         }
  1342.     }
  1343.  
  1344.     // sub matrix for factorization 
  1345.     clamped.SetData( m.GetNumRows(), m.GetNumColumns(), MATX_ALLOCA( m.GetNumRows() * m.GetNumColumns() ) );
  1346.     diagonal.SetData( m.GetNumRows(), VECX_ALLOCA( m.GetNumRows() ) );
  1347.     solveCache1.SetData( m.GetNumRows(), VECX_ALLOCA( m.GetNumRows() ) );
  1348.     solveCache2.SetData( m.GetNumRows(), VECX_ALLOCA( m.GetNumRows() ) );
  1349.  
  1350.     // all unbounded variables are clamped
  1351.     numClamped = numUnbounded;
  1352.  
  1353.     // if there are unbounded variables
  1354.     if ( numUnbounded ) {
  1355.  
  1356.         // factor and solve for unbounded variables
  1357.         if ( !FactorClamped() ) {
  1358.             idLib::common->Printf( "idLCP_Symmetric::Solve: unbounded factorization failed\n" );
  1359.             return false;
  1360.         }
  1361.         SolveClamped( f, b.ToFloatPtr() );
  1362.  
  1363.         // if there are no bounded variables we are done
  1364.         if ( numUnbounded == m.GetNumRows() ) {
  1365.             o_x = f;    // the vector is not permuted
  1366.             return true;
  1367.         }
  1368.     }
  1369.  
  1370. #ifdef IGNORE_UNSATISFIABLE_VARIABLES
  1371.     int numIgnored = 0;
  1372. #endif
  1373.  
  1374.     // allocate for delta force and delta acceleration
  1375.     delta_f.SetData( m.GetNumRows(), VECX_ALLOCA( m.GetNumRows() ) );
  1376.     delta_a.SetData( m.GetNumRows(), VECX_ALLOCA( m.GetNumRows() ) );
  1377.  
  1378.     // solve for bounded variables
  1379.     failed = NULL;
  1380.     for ( i = numUnbounded; i < m.GetNumRows(); i++ ) {
  1381.  
  1382.         clampedChangeStart = 0;
  1383.  
  1384.         // once we hit the box start index we can initialize the low and high boundaries of the variables using the box index
  1385.         if ( i == boxStartIndex ) {
  1386.             for ( j = 0; j < boxStartIndex; j++ ) {
  1387.                 o_x[permuted[j]] = f[j];
  1388.             }
  1389.             for ( j = boxStartIndex; j < m.GetNumRows(); j++ ) {
  1390.                 s = o_x[boxIndex[j]];
  1391.                 if ( lo[j] != -idMath::INFINITY ) {
  1392.                     lo[j] = - idMath::Fabs( lo[j] * s );
  1393.                 }
  1394.                 if ( hi[j] != idMath::INFINITY ) {
  1395.                     hi[j] = idMath::Fabs( hi[j] * s );
  1396.                 }
  1397.             }
  1398.         }
  1399.  
  1400.         // calculate acceleration for current variable
  1401.         SIMDProcessor->Dot( dot, rowPtrs[i], f.ToFloatPtr(), i );
  1402.         a[i] = dot - b[i];
  1403.  
  1404.         // if already at the low boundary
  1405.         if ( lo[i] >= -LCP_BOUND_EPSILON && a[i] >= -LCP_ACCEL_EPSILON ) {
  1406.             side[i] = -1;
  1407.             continue;
  1408.         }
  1409.  
  1410.         // if already at the high boundary
  1411.         if ( hi[i] <= LCP_BOUND_EPSILON && a[i] <= LCP_ACCEL_EPSILON ) {
  1412.             side[i] = 1;
  1413.             continue;
  1414.         }
  1415.  
  1416.         // if inside the clamped region
  1417.         if ( idMath::Fabs( a[i] ) <= LCP_ACCEL_EPSILON ) {
  1418.             side[i] = 0;
  1419.             AddClamped( i, false );
  1420.             continue;
  1421.         }
  1422.  
  1423.         // drive the current variable into a valid region
  1424.         for ( n = 0; n < maxIterations; n++ ) {
  1425.  
  1426.             // direction to move
  1427.             if ( a[i] <= 0.0f ) {
  1428.                 dir = 1.0f;
  1429.             } else {
  1430.                 dir = -1.0f;
  1431.             }
  1432.  
  1433.             // calculate force delta
  1434.             CalcForceDelta( i, dir );
  1435.  
  1436.             // calculate acceleration delta: delta_a = m * delta_f;
  1437.             CalcAccelDelta( i );
  1438.  
  1439.             // maximum step we can take
  1440.             GetMaxStep( i, dir, maxStep, limit, limitSide );
  1441.  
  1442.             if ( maxStep <= 0.0f ) {
  1443. #ifdef IGNORE_UNSATISFIABLE_VARIABLES
  1444.                 // ignore the current variable completely
  1445.                 lo[i] = hi[i] = 0.0f;
  1446.                 f[i] = 0.0f;
  1447.                 side[i] = -1;
  1448.                 numIgnored++;
  1449. #else
  1450.                 failed = va( "invalid step size %.4f", maxStep );
  1451. #endif
  1452.                 break;
  1453.             }
  1454.  
  1455.             // change force
  1456.             ChangeForce( i, maxStep );
  1457.  
  1458.             // change acceleration
  1459.             ChangeAccel( i, maxStep );
  1460.  
  1461.             // clamp/unclamp the variable that limited this step
  1462.             side[limit] = limitSide;
  1463.             switch( limitSide ) {
  1464.                 case 0: {
  1465.                     a[limit] = 0.0f;
  1466.                     AddClamped( limit, ( limit == i ) );
  1467.                     break;
  1468.                 }
  1469.                 case -1: {
  1470.                     f[limit] = lo[limit];
  1471.                     if ( limit != i ) {
  1472.                         RemoveClamped( limit );
  1473.                     }
  1474.                     break;
  1475.                 }
  1476.                 case 1: {
  1477.                     f[limit] = hi[limit];
  1478.                     if ( limit != i ) {
  1479.                         RemoveClamped( limit );
  1480.                     }
  1481.                     break;
  1482.                 }
  1483.             }
  1484.  
  1485.             // if the current variable limited the step we can continue with the next variable
  1486.             if ( limit == i ) {
  1487.                 break;
  1488.             }
  1489.         }
  1490.  
  1491.         if ( n >= maxIterations ) {
  1492.             failed = va( "max iterations %d", maxIterations );
  1493.             break;
  1494.         }
  1495.  
  1496.         if ( failed ) {
  1497.             break;
  1498.         }
  1499.     }
  1500.  
  1501. #ifdef IGNORE_UNSATISFIABLE_VARIABLES
  1502.     if ( numIgnored ) {
  1503.         if ( lcp_showFailures.GetBool() ) {
  1504.             idLib::common->Printf( "idLCP_Symmetric::Solve: %d of %d bounded variables ignored\n", numIgnored, m.GetNumRows() - numUnbounded );
  1505.         }
  1506.     }
  1507. #endif
  1508.  
  1509.     // if failed clear remaining forces
  1510.     if ( failed ) {
  1511.         if ( lcp_showFailures.GetBool() ) {
  1512.             idLib::common->Printf( "idLCP_Symmetric::Solve: %s (%d of %d bounded variables ignored)\n", failed, m.GetNumRows() - i, m.GetNumRows() - numUnbounded );
  1513.         }
  1514.         for ( j = i; j < m.GetNumRows(); j++ ) {
  1515.             f[j] = 0.0f;
  1516.         }
  1517.     }
  1518.  
  1519. #if defined(_DEBUG) && 0
  1520.     if ( !failed ) {
  1521.         // test whether or not the solution satisfies the complementarity conditions
  1522.         for ( i = 0; i < m.GetNumRows(); i++ ) {
  1523.             a[i] = -b[i];
  1524.             for ( j = 0; j < m.GetNumRows(); j++ ) {
  1525.                 a[i] += rowPtrs[i][j] * f[j];
  1526.             }
  1527.  
  1528.             if ( f[i] == lo[i] ) {
  1529.                 if ( lo[i] != hi[i] && a[i] < -LCP_ACCEL_EPSILON ) {
  1530.                     int bah1 = 1;
  1531.                 }
  1532.             } else if ( f[i] == hi[i] ) {
  1533.                 if ( lo[i] != hi[i] && a[i] > LCP_ACCEL_EPSILON ) {
  1534.                     int bah2 = 1;
  1535.                 }
  1536.             } else if ( f[i] < lo[i] || f[i] > hi[i] || idMath::Fabs( a[i] ) > 1.0f ) {
  1537.                 int bah3 = 1;
  1538.             }
  1539.         }
  1540.     }
  1541. #endif
  1542.  
  1543.     // unpermute result
  1544.     for ( i = 0; i < f.GetSize(); i++ ) {
  1545.         o_x[permuted[i]] = f[i];
  1546.     }
  1547.  
  1548.     // unpermute original matrix
  1549.     for ( i = 0; i < m.GetNumRows(); i++ ) {
  1550.         for ( j = 0; j < m.GetNumRows(); j++ ) {
  1551.             if ( permuted[j] == i ) {
  1552.                 break;
  1553.             }
  1554.         }
  1555.         if ( i != j ) {
  1556.             m.SwapColumns( i, j );
  1557.             idSwap( permuted[i], permuted[j] );
  1558.         }
  1559.     }
  1560.  
  1561.     return true;
  1562. }
  1563.  
  1564.  
  1565. //===============================================================
  1566. //
  1567. //    idLCP
  1568. //
  1569. //===============================================================
  1570.  
  1571. /*
  1572. ============
  1573. idLCP::AllocSquare
  1574. ============
  1575. */
  1576. idLCP *idLCP::AllocSquare( void ) {
  1577.     idLCP *lcp = new idLCP_Square;
  1578.     lcp->SetMaxIterations( 32 );
  1579.     return lcp;
  1580. }
  1581.  
  1582. /*
  1583. ============
  1584. idLCP::AllocSymmetric
  1585. ============
  1586. */
  1587. idLCP *idLCP::AllocSymmetric( void ) {
  1588.     idLCP *lcp = new idLCP_Symmetric;
  1589.     lcp->SetMaxIterations( 32 );
  1590.     return lcp;
  1591. }
  1592.  
  1593. /*
  1594. ============
  1595. idLCP::~idLCP
  1596. ============
  1597. */
  1598. idLCP::~idLCP( void ) {
  1599. }
  1600.  
  1601. /*
  1602. ============
  1603. idLCP::SetMaxIterations
  1604. ============
  1605. */
  1606. void idLCP::SetMaxIterations( int max ) {
  1607.     maxIterations = max;
  1608. }
  1609.  
  1610. /*
  1611. ============
  1612. idLCP::GetMaxIterations
  1613. ============
  1614. */
  1615. int idLCP::GetMaxIterations( void ) {
  1616.     return maxIterations;
  1617. }
  1618.