122 #define btLCP_FAST              // use fast btLCP object   126 #define BTATYPE btScalar **   127 #define BTAROW(i) (m_A[i])   134 #define BTNUB_OPTIMIZATIONS   151   btScalar Z11,m11,Z21,m21,p1,q1,p2,*ex;
   155   for (i=0; i < n; i+=2) {
   163     for (j=i-2; j >= 0; j -= 2) {
   204     Z21 = ex[1] - Z21 - p1*Z11;
   223   btScalar Z11,m11,Z12,m12,Z21,m21,Z22,m22,p1,q1,p2,q2,*ex;
   227   for (i=0; i < n; i+=2) {
   237     for (j=i-2; j >= 0; j -= 2) {
   292     Z12 = ex[lskip1] - Z12;
   295     Z21 = ex[1] - Z21 - p1*Z11;
   297     Z22 = ex[1+lskip1] - Z22 - p1*Z12;
   307   btScalar sum,*ell,*dee,dd,p1,p2,q1,q2,Z11,m11,Z21,m21,Z22,m22;
   310   for (i=0; i<=n-2; i += 2) {
   320     for (j=i-6; j >= 0; j -= 6) {
   423     Z21 = ell[nskip1] - Z21;
   424     Z22 = ell[1+nskip1] - Z22;
   451     for (j=i-6; j >= 0; j -= 6) {
   529   btScalar Z11,Z21,Z31,Z41,p1,q1,p2,p3,p4,*ex;
   531   int lskip2,lskip3,i,j;
   536   for (i=0; i <= n-4; i+=4) {
   546     for (j=i-12; j >= 0; j -= 12) {
   706     Z21 = ex[1] - Z21 - p1*Z11;
   710     Z31 = ex[2] - Z31 - p1*Z11 - p2*Z21;
   715     Z41 = ex[3] - Z41 - p1*Z11 - p2*Z21 - p3*Z31;
   727     for (j=i-12; j >= 0; j -= 12) {
   822   btScalar Z11,m11,Z21,m21,Z31,m31,Z41,m41,p1,q1,p2,p3,p4,*ex;
   827   L = L + (n-1)*(lskip1+1);
   834   for (i=0; i <= n-4; i+=4) {
   844     for (j=i-4; j >= 0; j -= 4) {
   937     Z21 = ex[-1] - Z21 - p1*Z11;
   941     Z31 = ex[-2] - Z31 - p1*Z11 - p2*Z21;
   946     Z41 = ex[-3] - Z41 - p1*Z11 - p2*Z21 - p3*Z31;
   958     for (j=i-4; j >= 0; j -= 4) {
  1013   for (
int i=0; i<n; i++) {
  1020   btAssert (L && d && b && n > 0 && nskip >= n);
  1037   int do_fast_row_swaps)
  1039   btAssert (A && n > 0 && i1 >= 0 && i2 >= 0 && i1 < n && i2 < n &&
  1040     nskip >= n && i1 < i2);
  1045   for (
int i=i1+1; i<i2; ++i) {
  1050   A_i1[i2] = A_i1[i1];
  1051   A_i1[i1] = A_i2[i1];
  1052   A_i2[i1] = A_i2[i2];
  1054   if (do_fast_row_swaps) {
  1060     for (
int k = 0; k <= i2; ++k) {
  1067   for (
int j=i2+1; j<n; ++j) {
  1076   for (
int k = 0; k < i1; ++k) {
  1082   for (
int i=i1+1; i<i2; A_i+=nskip, ++i) {
  1089     A_i1[i1] = A_i2[i2];
  1093   for (
int j=i2+1; j<n; A_j+=nskip, ++j) {
  1105                          btScalar *hi, 
int *p, 
bool *state, 
int *findex,
  1106                          int n, 
int i1, 
int i2, 
int nskip,
  1107                          int do_fast_row_swaps)
  1112   btAssert (n>0 && i1 >=0 && i2 >= 0 && i1 < n && i2 < n && nskip >= n && i1 <= i2);
  1142   state[i1] = state[i2];
  1147     findex[i1] = findex[i2];
  1205                 bool *_state, 
int *_findex, 
int *p, 
int *c, 
btScalar **Arows);
  1222         void solve1 (
btScalar *a, 
int i, 
int dir=1, 
int only_transfer=0);
  1230             bool *_state, 
int *_findex, 
int *p, 
int *c, 
btScalar **Arows):
  1251     for (
int k=0; k<n; aptr+=nskip, ++k) A[k] = aptr;
  1258     for (
int k=0; k<n; ++k) p[k]=k;             
  1293     for (
int k = 
m_nub; k<n; ++k) {
  1294       if (findex && findex[k] >= 0) 
continue;
  1296         btSwapProblem (
m_A,
m_x,
m_b,
m_w,lo,hi,
m_p,
m_state,findex,n,
m_nub,k,
m_nskip,0);
  1305     const int nub = 
m_nub;
  1309       for (
int j=0; j<nub; Lrow+=nskip, ++j) memcpy(Lrow,
BTAROW(j),(j+1)*
sizeof(
btScalar));
  1317       for (
int k=0; k<nub; ++k) C[k] = k;
  1324     const int nub = 
m_nub;
  1327     for (
int k=
m_n-1; k >= nub; k--) {
  1328       if (findex[k] >= 0) {
  1329         btSwapProblem (
m_A,
m_x,
m_b,
m_w,
m_lo,
m_hi,
m_p,
m_state,findex,
m_n,k,
m_n-1-num_at_end,
m_nskip,1);
  1357         const int nC = 
m_nC;
  1359         for (
int j=0; j<nC; ++j) Ltgt[j] = ell[j];
  1361       const int nC = 
m_nC;
  1368     btSwapProblem (
m_A,
m_x,
m_b,
m_w,
m_lo,
m_hi,
m_p,
m_state,
m_findex,
m_n,
m_nC,i,
m_nskip,1);
  1370     const int nC = 
m_nC;
  1386 #   ifdef BTNUB_OPTIMIZATIONS  1388         const int nub = 
m_nub;
  1390         for ( ; j<nub; ++j) Dell[j] = aptr[j];
  1391         const int nC = 
m_nC;
  1392         for ( ; j<nC; ++j) Dell[j] = aptr[C[j]];
  1394         const int nC = 
m_nC;
  1395         for (
int j=0; j<nC; ++j) Dell[j] = aptr[C[j]];
  1400         const int nC = 
m_nC;
  1403         for (
int j=0; j<nC; ++j) Ltgt[j] = ell[j] = Dell[j] * d[j];
  1405       const int nC = 
m_nC;
  1412     btSwapProblem (
m_A,
m_x,
m_b,
m_w,
m_lo,
m_hi,
m_p,
m_state,
m_findex,
m_n,
m_nC,i,
m_nskip,1);
  1414     const int nC = 
m_nC;
  1430   btAssert(A && n > 0 && nskip >= n && r >= 0 && r < n);
  1431   if (r >= n-1) 
return;
  1434       const size_t move_size = (n-r-1)*
sizeof(
btScalar);
  1436       for (
int i=0; i<r; Adst+=nskip,++i) {
  1438         memmove (Adst,Asrc,move_size);
  1442       const size_t cpy_size = r*
sizeof(
btScalar);
  1444       for (
int i=r; i<(n-1); ++i) {
  1446         memcpy (Adst,Asrc,cpy_size);
  1452     const size_t cpy_size = (n-r-1)*
sizeof(
btScalar);
  1453     btScalar *Adst = A + r * (nskip + 1);
  1454     for (
int i=r; i<(n-1); ++i) {
  1455       btScalar *Asrc = Adst + (nskip + 1);
  1456       memcpy (Adst,Asrc,cpy_size);
  1467   btAssert (L && d && a && n > 0 && nskip >= n);
  1477   for (
int j=1; j<n; ++j) {
  1488     btScalar alphanew = alpha1 + (W11*W11)*dee;
  1494     alphanew = alpha2 - (W21*W21)*dee;
  1499     btScalar k2 = W21*gamma1*W11 - W21;
  1501     for (
int p=1; p<n; ll+=nskip, ++p) {
  1504       W1[p] =    Wp - W11*ell;
  1505       W2[p] = k1*Wp +  k2*ell;
  1510   for (
int j=1; j<n; ll+=nskip+1, ++j) {
  1515     btScalar alphanew = alpha1 + (k1*k1)*dee;
  1521     alphanew = alpha2 - (k2*k2)*dee;
  1529     for (
int p=j+1; p<n; l+=nskip, ++p) {
  1534       Wp = W2[p] - k2 * ell;
  1543 #define _BTGETA(i,j) (A[i][j])  1545 #define BTGETA(i,j) ((i > j) ? _BTGETA(i,j) : _BTGETA(j,i))  1549   return nskip * 2 * 
sizeof(
btScalar);
  1556   btAssert(A && p && L && d && n1 > 0 && n2 > 0 && r >= 0 && r < n2 &&
  1557            n1 >= n2 && nskip >= n1);
  1559         for (
int i=0; i<n2; ++i) 
  1569         scratch.
resize(nskip * 2+n2);
  1573       const int p_0 = p[0];
  1574       for (
int i=0; i<n2; ++i) {
  1575         a[i] = -
BTGETA(p[i],p_0);
  1584         for (
int i=0; i<r; ++Lcurr, ++i) {
  1586           t[i] = *Lcurr / d[i];
  1592         const int *pp_r = p + r, p_r = *pp_r;
  1593         const int n2_minus_r = n2-r;
  1594         for (
int i=0; i<n2_minus_r; Lcurr+=nskip,++i) {
  1599       btLDLTAddTL (L + r*nskip+r, d+r, a, n2-r, nskip, scratch);
  1605   if (r < (n2-1)) memmove (d+r,d+r+1,(n2-r-1)*
sizeof(
btScalar));
  1616     const int nC = 
m_nC;
  1618     for ( ; j<nC; ++j) {
  1625         if (last_idx == -1) {
  1626           for (k=j+1 ; k<nC; ++k) {
  1637         if (j < (nC-1)) memmove (C+j,C+j+1,(nC-j-1)*
sizeof(
int));
  1643     btSwapProblem (
m_A,
m_x,
m_b,
m_w,
m_lo,
m_hi,
m_p,
m_state,
m_findex,
m_n,i,nC-1,
m_nskip,1);
  1659   const int nC = 
m_nC;
  1661   const int nN = 
m_nN;
  1662   for (
int i=0; i<nN; ++i) {
  1670   const int nC = 
m_nC;
  1674     const int nN = 
m_nN;
  1675     for (
int j=0; j<nN; ++j) ptgt[j] += aptr[j];
  1678     const int nN = 
m_nN;
  1679     for (
int j=0; j<nN; ++j) ptgt[j] -= aptr[j];
  1685   const int nC = 
m_nC;
  1686   for (
int i=0; i<nC; ++i) {
  1693   const int nC = 
m_nC;
  1694   btScalar *ptgt = p + nC, *qsrc = q + nC;
  1695   const int nN = 
m_nN;
  1696   for (
int i=0; i<nN; ++i) {
  1697     ptgt[i] += s*qsrc[i];
  1714 #   ifdef BTNUB_OPTIMIZATIONS  1716       const int nub = 
m_nub;
  1718       for ( ; j<nub; ++j) Dell[j] = aptr[j];
  1719       const int nC = 
m_nC;
  1720       for ( ; j<nC; ++j) Dell[j] = aptr[C[j]];
  1722       const int nC = 
m_nC;
  1723       for (
int j=0; j<nC; ++j) Dell[j] = aptr[C[j]];
  1729       const int nC = 
m_nC;
  1730       for (
int j=0; j<nC; ++j) ell[j] = Dell[j] * d[j];
  1733     if (!only_transfer) {
  1736         const int nC = 
m_nC;
  1737         for (
int j=0; j<nC; ++j) tmp[j] = ell[j];
  1743         const int nC = 
m_nC;
  1744         for (
int j=0; j<nC; ++j) a[C[j]] = -tmp[j];
  1748         const int nC = 
m_nC;
  1749         for (
int j=0; j<nC; ++j) a[C[j]] = tmp[j];
  1764     for (
int j=0; j<n; ++j) x[p[j]] = tmp[j];
  1771     for (
int j=0; j<n; ++j) w[p[j]] = tmp[j];
  1775 #endif // btLCP_FAST  1787   btAssert (n>0 && A && x && b && lo && hi && nub >= 0 && nub <= n);
  1793     for (
int k=0; k<n; ++k) 
  1794                 btAssert (lo[k] <= 0 && hi[k] >= 0);
  1813   const int nskip = (n);
  1833   btLCP lcp(n,nskip,nub,A,x,b,w,lo,hi,&scratchMem.
L[0],&scratchMem.
d[0],&scratchMem.
Dell[0],&scratchMem.
ell[0],&scratchMem.
delta_w[0],&scratchMem.
state[0],findex,&scratchMem.
p[0],&scratchMem.
C[0],&scratchMem.
Arows[0]);
  1834   int adj_nub = lcp.
getNub();
  1845   bool hit_first_friction_index = 
false;
  1846   for (
int i=adj_nub; i<n; ++i) 
  1862     if (!hit_first_friction_index && findex && findex[i] >= 0) {
  1864       for (
int j=0; j<n; ++j) scratchMem.
delta_w[scratchMem.
p[j]] = x[j];
  1867       for (
int k=i; k<n; ++k) {
  1874           hi[k] = 
btFabs (hi[k] * wfk);
  1878       hit_first_friction_index = 
true;
  1896     if (lo[i]==0 && w[i] >= 0) {
  1898       scratchMem.
state[i] = 
false;
  1900     else if (hi[i]==0 && w[i] <= 0) {
  1902       scratchMem.
state[i] = 
true;
  1968           for (
int k=0; k < 
numN; ++k) {
  1969             const int indexN_k = lcp.
indexN(k);
  1970             if (!scratchMem.
state[indexN_k] ? scratchMem.
delta_w[indexN_k] < 0 : scratchMem.
delta_w[indexN_k] > 0) {
  1972                 if (lo[indexN_k] == 0 && hi[indexN_k] == 0) 
continue;
  1985           for (
int k=adj_nub; k < 
numC; ++k) {
  1986             const int indexC_k = lcp.
indexC(k);
  1988               btScalar s2 = (lo[indexC_k]-x[indexC_k]) / scratchMem.
delta_x[indexC_k];
  1996               btScalar s2 = (hi[indexC_k]-x[indexC_k]) / scratchMem.
delta_x[indexC_k];
  2030         w[i] += s * scratchMem.
delta_w[i];
  2041           scratchMem.
state[i] = 
false;
  2046           scratchMem.
state[i] = 
true;
  2055           scratchMem.
state[si] = 
false;
  2060           scratchMem.
state[si] = 
true;
  2065         if (cmd <= 3) 
break;
 static T sum(const btAlignedObjectArray< T > &items)
void btRemoveRowCol(btScalar *A, int n, int nskip, int r)
void btLDLTAddTL(btScalar *L, btScalar *d, const btScalar *a, int n, int nskip, btAlignedObjectArray< btScalar > &scratch)
void btFactorLDLT(btScalar *A, btScalar *d, int n, int nskip1)
btAlignedObjectArray< btScalar * > Arows
void pN_plusequals_ANi(btScalar *p, int i, int sign=1)
btAlignedObjectArray< btScalar > delta_w
btScalar AiN_times_qN(int i, btScalar *q) const 
void transfer_i_from_N_to_C(int i)
void btLDLTRemove(btScalar **A, const int *p, btScalar *L, btScalar *d, int n1, int n2, int r, int nskip, btAlignedObjectArray< btScalar > &scratch)
btScalar *const *const *const m_w
btScalar Aii(int i) const 
btScalar *const *const m_d
static void btSwapProblem(BTATYPE A, btScalar *x, btScalar *b, btScalar *w, btScalar *lo, btScalar *hi, int *p, bool *state, int *findex, int n, int i1, int i2, int nskip, int do_fast_row_swaps)
void solve1(btScalar *a, int i, int dir=1, int only_transfer=0)
btScalar *const *const m_ell
btScalar *const *const *const m_tmp
void btVectorScale(btScalar *a, const btScalar *d, int n)
btScalar *const *const m_b
btScalar *const *const *const *const *const m_hi
size_t btEstimateLDLTAddTLTmpbufSize(int nskip)
void transfer_i_to_N(int i)
void pN_equals_ANC_times_qC(btScalar *p, btScalar *q)
btAlignedObjectArray< btScalar > m_scratch
static void btSwapRowsAndCols(BTATYPE A, int n, int i1, int i2, int nskip, int do_fast_row_swaps)
btAlignedObjectArray< btScalar > Dell
void pC_plusequals_s_times_qC(btScalar *p, btScalar s, btScalar *q)
void btSetZero(T *a, int n)
void btSolveL1T(const btScalar *L, btScalar *B, int n, int lskip1)
void btSolveLDLT(const btScalar *L, const btScalar *d, btScalar *b, int n, int nskip)
static void btSolveL1_2(const btScalar *L, btScalar *B, int n, int lskip1)
btAlignedObjectArray< int > p
btAlignedObjectArray< int > C
static void btSolveL1_1(const btScalar *L, btScalar *B, int n, int lskip1)
bool btSolveDantzigLCP(int n, btScalar *A, btScalar *x, btScalar *b, btScalar *outer_w, int nub, btScalar *lo, btScalar *hi, int *findex, btDantzigScratchMemory &scratchMem)
void resize(int newsize, const T &fillData=T())
void transfer_i_from_C_to_N(int i, btAlignedObjectArray< btScalar > &scratch)
btScalar AiC_times_qC(int i, btScalar *q) const 
btAlignedObjectArray< btScalar > ell
btScalar *const *const *const *const m_lo
btAlignedObjectArray< btScalar > delta_x
void btSolveL1(const btScalar *L, btScalar *B, int n, int lskip1)
btLCP(int _n, int _nskip, int _nub, btScalar *_Adata, btScalar *_x, btScalar *_b, btScalar *_w, btScalar *_lo, btScalar *_hi, btScalar *l, btScalar *_d, btScalar *_Dell, btScalar *_ell, btScalar *_tmp, bool *_state, int *_findex, int *p, int *c, btScalar **Arows)
btAlignedObjectArray< btScalar > L
btAlignedObjectArray< btScalar > d
void pN_plusequals_s_times_qN(btScalar *p, btScalar s, btScalar *q)
btAlignedObjectArray< bool > state
int *const *const *const m_C
void transfer_i_to_C(int i)
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
btScalar btLargeDot(const btScalar *a, const btScalar *b, int n)
btScalar btFabs(btScalar x)