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)