diff options
Diffstat (limited to 'extern/bullet2/src/BulletDynamics/MLCPSolvers')
11 files changed, 2611 insertions, 2576 deletions
diff --git a/extern/bullet2/src/BulletDynamics/MLCPSolvers/btDantzigLCP.cpp b/extern/bullet2/src/BulletDynamics/MLCPSolvers/btDantzigLCP.cpp index 986f2148701..98ecdc07947 100644 --- a/extern/bullet2/src/BulletDynamics/MLCPSolvers/btDantzigLCP.cpp +++ b/extern/bullet2/src/BulletDynamics/MLCPSolvers/btDantzigLCP.cpp @@ -108,18 +108,16 @@ rows/columns and manipulate C. */ - #include "btDantzigLCP.h" -#include <string.h>//memcpy +#include <string.h> //memcpy bool s_error = false; //*************************************************************************** // code generation parameters - -#define btLCP_FAST // use fast btLCP object +#define btLCP_FAST // use fast btLCP object // option 1 : matrix row pointers (less data copying) #define BTROWPTRS @@ -133,8 +131,6 @@ bool s_error = false; #define BTNUB_OPTIMIZATIONS - - /* solve L*X=B, with B containing 1 right hand sides. * L is an n*n lower triangular matrix with ones on the diagonal. * L is stored by rows and its leading dimension is lskip. @@ -145,66 +141,69 @@ bool s_error = false; * if this is in the factorizer source file, n must be a multiple of 2. */ -static void btSolveL1_1 (const btScalar *L, btScalar *B, int n, int lskip1) -{ - /* declare variables - Z matrix, p and q vectors, etc */ - btScalar Z11,m11,Z21,m21,p1,q1,p2,*ex; - const btScalar *ell; - int i,j; - /* compute all 2 x 1 blocks of X */ - for (i=0; i < n; i+=2) { - /* compute all 2 x 1 block of X, from rows i..i+2-1 */ - /* set the Z matrix to 0 */ - Z11=0; - Z21=0; - ell = L + i*lskip1; - ex = B; - /* the inner loop that computes outer products and adds them to Z */ - for (j=i-2; j >= 0; j -= 2) { - /* compute outer product and add it to the Z matrix */ - p1=ell[0]; - q1=ex[0]; - m11 = p1 * q1; - p2=ell[lskip1]; - m21 = p2 * q1; - Z11 += m11; - Z21 += m21; - /* compute outer product and add it to the Z matrix */ - p1=ell[1]; - q1=ex[1]; - m11 = p1 * q1; - p2=ell[1+lskip1]; - m21 = p2 * q1; - /* advance pointers */ - ell += 2; - ex += 2; - Z11 += m11; - Z21 += m21; - /* end of inner loop */ - } - /* compute left-over iterations */ - j += 2; - for (; j > 0; j--) { - /* compute outer product and add it to the Z matrix */ - p1=ell[0]; - q1=ex[0]; - m11 = p1 * q1; - p2=ell[lskip1]; - m21 = p2 * q1; - /* advance pointers */ - ell += 1; - ex += 1; - Z11 += m11; - Z21 += m21; - } - /* finish computing the X(i) block */ - Z11 = ex[0] - Z11; - ex[0] = Z11; - p1 = ell[lskip1]; - Z21 = ex[1] - Z21 - p1*Z11; - ex[1] = Z21; - /* end of outer loop */ - } +static void btSolveL1_1(const btScalar *L, btScalar *B, int n, int lskip1) +{ + /* declare variables - Z matrix, p and q vectors, etc */ + btScalar Z11, m11, Z21, m21, p1, q1, p2, *ex; + const btScalar *ell; + int i, j; + /* compute all 2 x 1 blocks of X */ + for (i = 0; i < n; i += 2) + { + /* compute all 2 x 1 block of X, from rows i..i+2-1 */ + /* set the Z matrix to 0 */ + Z11 = 0; + Z21 = 0; + ell = L + i * lskip1; + ex = B; + /* the inner loop that computes outer products and adds them to Z */ + for (j = i - 2; j >= 0; j -= 2) + { + /* compute outer product and add it to the Z matrix */ + p1 = ell[0]; + q1 = ex[0]; + m11 = p1 * q1; + p2 = ell[lskip1]; + m21 = p2 * q1; + Z11 += m11; + Z21 += m21; + /* compute outer product and add it to the Z matrix */ + p1 = ell[1]; + q1 = ex[1]; + m11 = p1 * q1; + p2 = ell[1 + lskip1]; + m21 = p2 * q1; + /* advance pointers */ + ell += 2; + ex += 2; + Z11 += m11; + Z21 += m21; + /* end of inner loop */ + } + /* compute left-over iterations */ + j += 2; + for (; j > 0; j--) + { + /* compute outer product and add it to the Z matrix */ + p1 = ell[0]; + q1 = ex[0]; + m11 = p1 * q1; + p2 = ell[lskip1]; + m21 = p2 * q1; + /* advance pointers */ + ell += 1; + ex += 1; + Z11 += m11; + Z21 += m21; + } + /* finish computing the X(i) block */ + Z11 = ex[0] - Z11; + ex[0] = Z11; + p1 = ell[lskip1]; + Z21 = ex[1] - Z21 - p1 * Z11; + ex[1] = Z21; + /* end of outer loop */ + } } /* solve L*X=B, with B containing 2 right hand sides. @@ -217,300 +216,308 @@ static void btSolveL1_1 (const btScalar *L, btScalar *B, int n, int lskip1) * if this is in the factorizer source file, n must be a multiple of 2. */ -static void btSolveL1_2 (const btScalar *L, btScalar *B, int n, int lskip1) -{ - /* declare variables - Z matrix, p and q vectors, etc */ - btScalar Z11,m11,Z12,m12,Z21,m21,Z22,m22,p1,q1,p2,q2,*ex; - const btScalar *ell; - int i,j; - /* compute all 2 x 2 blocks of X */ - for (i=0; i < n; i+=2) { - /* compute all 2 x 2 block of X, from rows i..i+2-1 */ - /* set the Z matrix to 0 */ - Z11=0; - Z12=0; - Z21=0; - Z22=0; - ell = L + i*lskip1; - ex = B; - /* the inner loop that computes outer products and adds them to Z */ - for (j=i-2; j >= 0; j -= 2) { - /* compute outer product and add it to the Z matrix */ - p1=ell[0]; - q1=ex[0]; - m11 = p1 * q1; - q2=ex[lskip1]; - m12 = p1 * q2; - p2=ell[lskip1]; - m21 = p2 * q1; - m22 = p2 * q2; - Z11 += m11; - Z12 += m12; - Z21 += m21; - Z22 += m22; - /* compute outer product and add it to the Z matrix */ - p1=ell[1]; - q1=ex[1]; - m11 = p1 * q1; - q2=ex[1+lskip1]; - m12 = p1 * q2; - p2=ell[1+lskip1]; - m21 = p2 * q1; - m22 = p2 * q2; - /* advance pointers */ - ell += 2; - ex += 2; - Z11 += m11; - Z12 += m12; - Z21 += m21; - Z22 += m22; - /* end of inner loop */ - } - /* compute left-over iterations */ - j += 2; - for (; j > 0; j--) { - /* compute outer product and add it to the Z matrix */ - p1=ell[0]; - q1=ex[0]; - m11 = p1 * q1; - q2=ex[lskip1]; - m12 = p1 * q2; - p2=ell[lskip1]; - m21 = p2 * q1; - m22 = p2 * q2; - /* advance pointers */ - ell += 1; - ex += 1; - Z11 += m11; - Z12 += m12; - Z21 += m21; - Z22 += m22; - } - /* finish computing the X(i) block */ - Z11 = ex[0] - Z11; - ex[0] = Z11; - Z12 = ex[lskip1] - Z12; - ex[lskip1] = Z12; - p1 = ell[lskip1]; - Z21 = ex[1] - Z21 - p1*Z11; - ex[1] = Z21; - Z22 = ex[1+lskip1] - Z22 - p1*Z12; - ex[1+lskip1] = Z22; - /* end of outer loop */ - } +static void btSolveL1_2(const btScalar *L, btScalar *B, int n, int lskip1) +{ + /* declare variables - Z matrix, p and q vectors, etc */ + btScalar Z11, m11, Z12, m12, Z21, m21, Z22, m22, p1, q1, p2, q2, *ex; + const btScalar *ell; + int i, j; + /* compute all 2 x 2 blocks of X */ + for (i = 0; i < n; i += 2) + { + /* compute all 2 x 2 block of X, from rows i..i+2-1 */ + /* set the Z matrix to 0 */ + Z11 = 0; + Z12 = 0; + Z21 = 0; + Z22 = 0; + ell = L + i * lskip1; + ex = B; + /* the inner loop that computes outer products and adds them to Z */ + for (j = i - 2; j >= 0; j -= 2) + { + /* compute outer product and add it to the Z matrix */ + p1 = ell[0]; + q1 = ex[0]; + m11 = p1 * q1; + q2 = ex[lskip1]; + m12 = p1 * q2; + p2 = ell[lskip1]; + m21 = p2 * q1; + m22 = p2 * q2; + Z11 += m11; + Z12 += m12; + Z21 += m21; + Z22 += m22; + /* compute outer product and add it to the Z matrix */ + p1 = ell[1]; + q1 = ex[1]; + m11 = p1 * q1; + q2 = ex[1 + lskip1]; + m12 = p1 * q2; + p2 = ell[1 + lskip1]; + m21 = p2 * q1; + m22 = p2 * q2; + /* advance pointers */ + ell += 2; + ex += 2; + Z11 += m11; + Z12 += m12; + Z21 += m21; + Z22 += m22; + /* end of inner loop */ + } + /* compute left-over iterations */ + j += 2; + for (; j > 0; j--) + { + /* compute outer product and add it to the Z matrix */ + p1 = ell[0]; + q1 = ex[0]; + m11 = p1 * q1; + q2 = ex[lskip1]; + m12 = p1 * q2; + p2 = ell[lskip1]; + m21 = p2 * q1; + m22 = p2 * q2; + /* advance pointers */ + ell += 1; + ex += 1; + Z11 += m11; + Z12 += m12; + Z21 += m21; + Z22 += m22; + } + /* finish computing the X(i) block */ + Z11 = ex[0] - Z11; + ex[0] = Z11; + Z12 = ex[lskip1] - Z12; + ex[lskip1] = Z12; + p1 = ell[lskip1]; + Z21 = ex[1] - Z21 - p1 * Z11; + ex[1] = Z21; + Z22 = ex[1 + lskip1] - Z22 - p1 * Z12; + ex[1 + lskip1] = Z22; + /* end of outer loop */ + } } +void btFactorLDLT(btScalar *A, btScalar *d, int n, int nskip1) +{ + int i, j; + btScalar sum, *ell, *dee, dd, p1, p2, q1, q2, Z11, m11, Z21, m21, Z22, m22; + if (n < 1) return; -void btFactorLDLT (btScalar *A, btScalar *d, int n, int nskip1) -{ - int i,j; - btScalar sum,*ell,*dee,dd,p1,p2,q1,q2,Z11,m11,Z21,m21,Z22,m22; - if (n < 1) return; - - for (i=0; i<=n-2; i += 2) { - /* solve L*(D*l)=a, l is scaled elements in 2 x i block at A(i,0) */ - btSolveL1_2 (A,A+i*nskip1,i,nskip1); - /* scale the elements in a 2 x i block at A(i,0), and also */ - /* compute Z = the outer product matrix that we'll need. */ - Z11 = 0; - Z21 = 0; - Z22 = 0; - ell = A+i*nskip1; - dee = d; - for (j=i-6; j >= 0; j -= 6) { - p1 = ell[0]; - p2 = ell[nskip1]; - dd = dee[0]; - q1 = p1*dd; - q2 = p2*dd; - ell[0] = q1; - ell[nskip1] = q2; - m11 = p1*q1; - m21 = p2*q1; - m22 = p2*q2; - Z11 += m11; - Z21 += m21; - Z22 += m22; - p1 = ell[1]; - p2 = ell[1+nskip1]; - dd = dee[1]; - q1 = p1*dd; - q2 = p2*dd; - ell[1] = q1; - ell[1+nskip1] = q2; - m11 = p1*q1; - m21 = p2*q1; - m22 = p2*q2; - Z11 += m11; - Z21 += m21; - Z22 += m22; - p1 = ell[2]; - p2 = ell[2+nskip1]; - dd = dee[2]; - q1 = p1*dd; - q2 = p2*dd; - ell[2] = q1; - ell[2+nskip1] = q2; - m11 = p1*q1; - m21 = p2*q1; - m22 = p2*q2; - Z11 += m11; - Z21 += m21; - Z22 += m22; - p1 = ell[3]; - p2 = ell[3+nskip1]; - dd = dee[3]; - q1 = p1*dd; - q2 = p2*dd; - ell[3] = q1; - ell[3+nskip1] = q2; - m11 = p1*q1; - m21 = p2*q1; - m22 = p2*q2; - Z11 += m11; - Z21 += m21; - Z22 += m22; - p1 = ell[4]; - p2 = ell[4+nskip1]; - dd = dee[4]; - q1 = p1*dd; - q2 = p2*dd; - ell[4] = q1; - ell[4+nskip1] = q2; - m11 = p1*q1; - m21 = p2*q1; - m22 = p2*q2; - Z11 += m11; - Z21 += m21; - Z22 += m22; - p1 = ell[5]; - p2 = ell[5+nskip1]; - dd = dee[5]; - q1 = p1*dd; - q2 = p2*dd; - ell[5] = q1; - ell[5+nskip1] = q2; - m11 = p1*q1; - m21 = p2*q1; - m22 = p2*q2; - Z11 += m11; - Z21 += m21; - Z22 += m22; - ell += 6; - dee += 6; - } - /* compute left-over iterations */ - j += 6; - for (; j > 0; j--) { - p1 = ell[0]; - p2 = ell[nskip1]; - dd = dee[0]; - q1 = p1*dd; - q2 = p2*dd; - ell[0] = q1; - ell[nskip1] = q2; - m11 = p1*q1; - m21 = p2*q1; - m22 = p2*q2; - Z11 += m11; - Z21 += m21; - Z22 += m22; - ell++; - dee++; - } - /* solve for diagonal 2 x 2 block at A(i,i) */ - Z11 = ell[0] - Z11; - Z21 = ell[nskip1] - Z21; - Z22 = ell[1+nskip1] - Z22; - dee = d + i; - /* factorize 2 x 2 block Z,dee */ - /* factorize row 1 */ - dee[0] = btRecip(Z11); - /* factorize row 2 */ - sum = 0; - q1 = Z21; - q2 = q1 * dee[0]; - Z21 = q2; - sum += q1*q2; - dee[1] = btRecip(Z22 - sum); - /* done factorizing 2 x 2 block */ - ell[nskip1] = Z21; - } - /* compute the (less than 2) rows at the bottom */ - switch (n-i) { - case 0: - break; - - case 1: - btSolveL1_1 (A,A+i*nskip1,i,nskip1); - /* scale the elements in a 1 x i block at A(i,0), and also */ - /* compute Z = the outer product matrix that we'll need. */ - Z11 = 0; - ell = A+i*nskip1; - dee = d; - for (j=i-6; j >= 0; j -= 6) { - p1 = ell[0]; - dd = dee[0]; - q1 = p1*dd; - ell[0] = q1; - m11 = p1*q1; - Z11 += m11; - p1 = ell[1]; - dd = dee[1]; - q1 = p1*dd; - ell[1] = q1; - m11 = p1*q1; - Z11 += m11; - p1 = ell[2]; - dd = dee[2]; - q1 = p1*dd; - ell[2] = q1; - m11 = p1*q1; - Z11 += m11; - p1 = ell[3]; - dd = dee[3]; - q1 = p1*dd; - ell[3] = q1; - m11 = p1*q1; - Z11 += m11; - p1 = ell[4]; - dd = dee[4]; - q1 = p1*dd; - ell[4] = q1; - m11 = p1*q1; - Z11 += m11; - p1 = ell[5]; - dd = dee[5]; - q1 = p1*dd; - ell[5] = q1; - m11 = p1*q1; - Z11 += m11; - ell += 6; - dee += 6; - } - /* compute left-over iterations */ - j += 6; - for (; j > 0; j--) { - p1 = ell[0]; - dd = dee[0]; - q1 = p1*dd; - ell[0] = q1; - m11 = p1*q1; - Z11 += m11; - ell++; - dee++; - } - /* solve for diagonal 1 x 1 block at A(i,i) */ - Z11 = ell[0] - Z11; - dee = d + i; - /* factorize 1 x 1 block Z,dee */ - /* factorize row 1 */ - dee[0] = btRecip(Z11); - /* done factorizing 1 x 1 block */ - break; - - //default: *((char*)0)=0; /* this should never happen! */ - } + for (i = 0; i <= n - 2; i += 2) + { + /* solve L*(D*l)=a, l is scaled elements in 2 x i block at A(i,0) */ + btSolveL1_2(A, A + i * nskip1, i, nskip1); + /* scale the elements in a 2 x i block at A(i,0), and also */ + /* compute Z = the outer product matrix that we'll need. */ + Z11 = 0; + Z21 = 0; + Z22 = 0; + ell = A + i * nskip1; + dee = d; + for (j = i - 6; j >= 0; j -= 6) + { + p1 = ell[0]; + p2 = ell[nskip1]; + dd = dee[0]; + q1 = p1 * dd; + q2 = p2 * dd; + ell[0] = q1; + ell[nskip1] = q2; + m11 = p1 * q1; + m21 = p2 * q1; + m22 = p2 * q2; + Z11 += m11; + Z21 += m21; + Z22 += m22; + p1 = ell[1]; + p2 = ell[1 + nskip1]; + dd = dee[1]; + q1 = p1 * dd; + q2 = p2 * dd; + ell[1] = q1; + ell[1 + nskip1] = q2; + m11 = p1 * q1; + m21 = p2 * q1; + m22 = p2 * q2; + Z11 += m11; + Z21 += m21; + Z22 += m22; + p1 = ell[2]; + p2 = ell[2 + nskip1]; + dd = dee[2]; + q1 = p1 * dd; + q2 = p2 * dd; + ell[2] = q1; + ell[2 + nskip1] = q2; + m11 = p1 * q1; + m21 = p2 * q1; + m22 = p2 * q2; + Z11 += m11; + Z21 += m21; + Z22 += m22; + p1 = ell[3]; + p2 = ell[3 + nskip1]; + dd = dee[3]; + q1 = p1 * dd; + q2 = p2 * dd; + ell[3] = q1; + ell[3 + nskip1] = q2; + m11 = p1 * q1; + m21 = p2 * q1; + m22 = p2 * q2; + Z11 += m11; + Z21 += m21; + Z22 += m22; + p1 = ell[4]; + p2 = ell[4 + nskip1]; + dd = dee[4]; + q1 = p1 * dd; + q2 = p2 * dd; + ell[4] = q1; + ell[4 + nskip1] = q2; + m11 = p1 * q1; + m21 = p2 * q1; + m22 = p2 * q2; + Z11 += m11; + Z21 += m21; + Z22 += m22; + p1 = ell[5]; + p2 = ell[5 + nskip1]; + dd = dee[5]; + q1 = p1 * dd; + q2 = p2 * dd; + ell[5] = q1; + ell[5 + nskip1] = q2; + m11 = p1 * q1; + m21 = p2 * q1; + m22 = p2 * q2; + Z11 += m11; + Z21 += m21; + Z22 += m22; + ell += 6; + dee += 6; + } + /* compute left-over iterations */ + j += 6; + for (; j > 0; j--) + { + p1 = ell[0]; + p2 = ell[nskip1]; + dd = dee[0]; + q1 = p1 * dd; + q2 = p2 * dd; + ell[0] = q1; + ell[nskip1] = q2; + m11 = p1 * q1; + m21 = p2 * q1; + m22 = p2 * q2; + Z11 += m11; + Z21 += m21; + Z22 += m22; + ell++; + dee++; + } + /* solve for diagonal 2 x 2 block at A(i,i) */ + Z11 = ell[0] - Z11; + Z21 = ell[nskip1] - Z21; + Z22 = ell[1 + nskip1] - Z22; + dee = d + i; + /* factorize 2 x 2 block Z,dee */ + /* factorize row 1 */ + dee[0] = btRecip(Z11); + /* factorize row 2 */ + sum = 0; + q1 = Z21; + q2 = q1 * dee[0]; + Z21 = q2; + sum += q1 * q2; + dee[1] = btRecip(Z22 - sum); + /* done factorizing 2 x 2 block */ + ell[nskip1] = Z21; + } + /* compute the (less than 2) rows at the bottom */ + switch (n - i) + { + case 0: + break; + + case 1: + btSolveL1_1(A, A + i * nskip1, i, nskip1); + /* scale the elements in a 1 x i block at A(i,0), and also */ + /* compute Z = the outer product matrix that we'll need. */ + Z11 = 0; + ell = A + i * nskip1; + dee = d; + for (j = i - 6; j >= 0; j -= 6) + { + p1 = ell[0]; + dd = dee[0]; + q1 = p1 * dd; + ell[0] = q1; + m11 = p1 * q1; + Z11 += m11; + p1 = ell[1]; + dd = dee[1]; + q1 = p1 * dd; + ell[1] = q1; + m11 = p1 * q1; + Z11 += m11; + p1 = ell[2]; + dd = dee[2]; + q1 = p1 * dd; + ell[2] = q1; + m11 = p1 * q1; + Z11 += m11; + p1 = ell[3]; + dd = dee[3]; + q1 = p1 * dd; + ell[3] = q1; + m11 = p1 * q1; + Z11 += m11; + p1 = ell[4]; + dd = dee[4]; + q1 = p1 * dd; + ell[4] = q1; + m11 = p1 * q1; + Z11 += m11; + p1 = ell[5]; + dd = dee[5]; + q1 = p1 * dd; + ell[5] = q1; + m11 = p1 * q1; + Z11 += m11; + ell += 6; + dee += 6; + } + /* compute left-over iterations */ + j += 6; + for (; j > 0; j--) + { + p1 = ell[0]; + dd = dee[0]; + q1 = p1 * dd; + ell[0] = q1; + m11 = p1 * q1; + Z11 += m11; + ell++; + dee++; + } + /* solve for diagonal 1 x 1 block at A(i,i) */ + Z11 = ell[0] - Z11; + dee = d + i; + /* factorize 1 x 1 block Z,dee */ + /* factorize row 1 */ + dee[0] = btRecip(Z11); + /* done factorizing 1 x 1 block */ + break; + + //default: *((char*)0)=0; /* this should never happen! */ + } } /* solve L*X=B, with B containing 1 right hand sides. @@ -523,289 +530,295 @@ void btFactorLDLT (btScalar *A, btScalar *d, int n, int nskip1) * if this is in the factorizer source file, n must be a multiple of 4. */ -void btSolveL1 (const btScalar *L, btScalar *B, int n, int lskip1) -{ - /* declare variables - Z matrix, p and q vectors, etc */ - btScalar Z11,Z21,Z31,Z41,p1,q1,p2,p3,p4,*ex; - const btScalar *ell; - int lskip2,lskip3,i,j; - /* compute lskip values */ - lskip2 = 2*lskip1; - lskip3 = 3*lskip1; - /* compute all 4 x 1 blocks of X */ - for (i=0; i <= n-4; i+=4) { - /* compute all 4 x 1 block of X, from rows i..i+4-1 */ - /* set the Z matrix to 0 */ - Z11=0; - Z21=0; - Z31=0; - Z41=0; - ell = L + i*lskip1; - ex = B; - /* the inner loop that computes outer products and adds them to Z */ - for (j=i-12; j >= 0; j -= 12) { - /* load p and q values */ - p1=ell[0]; - q1=ex[0]; - p2=ell[lskip1]; - p3=ell[lskip2]; - p4=ell[lskip3]; - /* compute outer product and add it to the Z matrix */ - Z11 += p1 * q1; - Z21 += p2 * q1; - Z31 += p3 * q1; - Z41 += p4 * q1; - /* load p and q values */ - p1=ell[1]; - q1=ex[1]; - p2=ell[1+lskip1]; - p3=ell[1+lskip2]; - p4=ell[1+lskip3]; - /* compute outer product and add it to the Z matrix */ - Z11 += p1 * q1; - Z21 += p2 * q1; - Z31 += p3 * q1; - Z41 += p4 * q1; - /* load p and q values */ - p1=ell[2]; - q1=ex[2]; - p2=ell[2+lskip1]; - p3=ell[2+lskip2]; - p4=ell[2+lskip3]; - /* compute outer product and add it to the Z matrix */ - Z11 += p1 * q1; - Z21 += p2 * q1; - Z31 += p3 * q1; - Z41 += p4 * q1; - /* load p and q values */ - p1=ell[3]; - q1=ex[3]; - p2=ell[3+lskip1]; - p3=ell[3+lskip2]; - p4=ell[3+lskip3]; - /* compute outer product and add it to the Z matrix */ - Z11 += p1 * q1; - Z21 += p2 * q1; - Z31 += p3 * q1; - Z41 += p4 * q1; - /* load p and q values */ - p1=ell[4]; - q1=ex[4]; - p2=ell[4+lskip1]; - p3=ell[4+lskip2]; - p4=ell[4+lskip3]; - /* compute outer product and add it to the Z matrix */ - Z11 += p1 * q1; - Z21 += p2 * q1; - Z31 += p3 * q1; - Z41 += p4 * q1; - /* load p and q values */ - p1=ell[5]; - q1=ex[5]; - p2=ell[5+lskip1]; - p3=ell[5+lskip2]; - p4=ell[5+lskip3]; - /* compute outer product and add it to the Z matrix */ - Z11 += p1 * q1; - Z21 += p2 * q1; - Z31 += p3 * q1; - Z41 += p4 * q1; - /* load p and q values */ - p1=ell[6]; - q1=ex[6]; - p2=ell[6+lskip1]; - p3=ell[6+lskip2]; - p4=ell[6+lskip3]; - /* compute outer product and add it to the Z matrix */ - Z11 += p1 * q1; - Z21 += p2 * q1; - Z31 += p3 * q1; - Z41 += p4 * q1; - /* load p and q values */ - p1=ell[7]; - q1=ex[7]; - p2=ell[7+lskip1]; - p3=ell[7+lskip2]; - p4=ell[7+lskip3]; - /* compute outer product and add it to the Z matrix */ - Z11 += p1 * q1; - Z21 += p2 * q1; - Z31 += p3 * q1; - Z41 += p4 * q1; - /* load p and q values */ - p1=ell[8]; - q1=ex[8]; - p2=ell[8+lskip1]; - p3=ell[8+lskip2]; - p4=ell[8+lskip3]; - /* compute outer product and add it to the Z matrix */ - Z11 += p1 * q1; - Z21 += p2 * q1; - Z31 += p3 * q1; - Z41 += p4 * q1; - /* load p and q values */ - p1=ell[9]; - q1=ex[9]; - p2=ell[9+lskip1]; - p3=ell[9+lskip2]; - p4=ell[9+lskip3]; - /* compute outer product and add it to the Z matrix */ - Z11 += p1 * q1; - Z21 += p2 * q1; - Z31 += p3 * q1; - Z41 += p4 * q1; - /* load p and q values */ - p1=ell[10]; - q1=ex[10]; - p2=ell[10+lskip1]; - p3=ell[10+lskip2]; - p4=ell[10+lskip3]; - /* compute outer product and add it to the Z matrix */ - Z11 += p1 * q1; - Z21 += p2 * q1; - Z31 += p3 * q1; - Z41 += p4 * q1; - /* load p and q values */ - p1=ell[11]; - q1=ex[11]; - p2=ell[11+lskip1]; - p3=ell[11+lskip2]; - p4=ell[11+lskip3]; - /* compute outer product and add it to the Z matrix */ - Z11 += p1 * q1; - Z21 += p2 * q1; - Z31 += p3 * q1; - Z41 += p4 * q1; - /* advance pointers */ - ell += 12; - ex += 12; - /* end of inner loop */ - } - /* compute left-over iterations */ - j += 12; - for (; j > 0; j--) { - /* load p and q values */ - p1=ell[0]; - q1=ex[0]; - p2=ell[lskip1]; - p3=ell[lskip2]; - p4=ell[lskip3]; - /* compute outer product and add it to the Z matrix */ - Z11 += p1 * q1; - Z21 += p2 * q1; - Z31 += p3 * q1; - Z41 += p4 * q1; - /* advance pointers */ - ell += 1; - ex += 1; - } - /* finish computing the X(i) block */ - Z11 = ex[0] - Z11; - ex[0] = Z11; - p1 = ell[lskip1]; - Z21 = ex[1] - Z21 - p1*Z11; - ex[1] = Z21; - p1 = ell[lskip2]; - p2 = ell[1+lskip2]; - Z31 = ex[2] - Z31 - p1*Z11 - p2*Z21; - ex[2] = Z31; - p1 = ell[lskip3]; - p2 = ell[1+lskip3]; - p3 = ell[2+lskip3]; - Z41 = ex[3] - Z41 - p1*Z11 - p2*Z21 - p3*Z31; - ex[3] = Z41; - /* end of outer loop */ - } - /* compute rows at end that are not a multiple of block size */ - for (; i < n; i++) { - /* compute all 1 x 1 block of X, from rows i..i+1-1 */ - /* set the Z matrix to 0 */ - Z11=0; - ell = L + i*lskip1; - ex = B; - /* the inner loop that computes outer products and adds them to Z */ - for (j=i-12; j >= 0; j -= 12) { - /* load p and q values */ - p1=ell[0]; - q1=ex[0]; - /* compute outer product and add it to the Z matrix */ - Z11 += p1 * q1; - /* load p and q values */ - p1=ell[1]; - q1=ex[1]; - /* compute outer product and add it to the Z matrix */ - Z11 += p1 * q1; - /* load p and q values */ - p1=ell[2]; - q1=ex[2]; - /* compute outer product and add it to the Z matrix */ - Z11 += p1 * q1; - /* load p and q values */ - p1=ell[3]; - q1=ex[3]; - /* compute outer product and add it to the Z matrix */ - Z11 += p1 * q1; - /* load p and q values */ - p1=ell[4]; - q1=ex[4]; - /* compute outer product and add it to the Z matrix */ - Z11 += p1 * q1; - /* load p and q values */ - p1=ell[5]; - q1=ex[5]; - /* compute outer product and add it to the Z matrix */ - Z11 += p1 * q1; - /* load p and q values */ - p1=ell[6]; - q1=ex[6]; - /* compute outer product and add it to the Z matrix */ - Z11 += p1 * q1; - /* load p and q values */ - p1=ell[7]; - q1=ex[7]; - /* compute outer product and add it to the Z matrix */ - Z11 += p1 * q1; - /* load p and q values */ - p1=ell[8]; - q1=ex[8]; - /* compute outer product and add it to the Z matrix */ - Z11 += p1 * q1; - /* load p and q values */ - p1=ell[9]; - q1=ex[9]; - /* compute outer product and add it to the Z matrix */ - Z11 += p1 * q1; - /* load p and q values */ - p1=ell[10]; - q1=ex[10]; - /* compute outer product and add it to the Z matrix */ - Z11 += p1 * q1; - /* load p and q values */ - p1=ell[11]; - q1=ex[11]; - /* compute outer product and add it to the Z matrix */ - Z11 += p1 * q1; - /* advance pointers */ - ell += 12; - ex += 12; - /* end of inner loop */ - } - /* compute left-over iterations */ - j += 12; - for (; j > 0; j--) { - /* load p and q values */ - p1=ell[0]; - q1=ex[0]; - /* compute outer product and add it to the Z matrix */ - Z11 += p1 * q1; - /* advance pointers */ - ell += 1; - ex += 1; - } - /* finish computing the X(i) block */ - Z11 = ex[0] - Z11; - ex[0] = Z11; - } +void btSolveL1(const btScalar *L, btScalar *B, int n, int lskip1) +{ + /* declare variables - Z matrix, p and q vectors, etc */ + btScalar Z11, Z21, Z31, Z41, p1, q1, p2, p3, p4, *ex; + const btScalar *ell; + int lskip2, lskip3, i, j; + /* compute lskip values */ + lskip2 = 2 * lskip1; + lskip3 = 3 * lskip1; + /* compute all 4 x 1 blocks of X */ + for (i = 0; i <= n - 4; i += 4) + { + /* compute all 4 x 1 block of X, from rows i..i+4-1 */ + /* set the Z matrix to 0 */ + Z11 = 0; + Z21 = 0; + Z31 = 0; + Z41 = 0; + ell = L + i * lskip1; + ex = B; + /* the inner loop that computes outer products and adds them to Z */ + for (j = i - 12; j >= 0; j -= 12) + { + /* load p and q values */ + p1 = ell[0]; + q1 = ex[0]; + p2 = ell[lskip1]; + p3 = ell[lskip2]; + p4 = ell[lskip3]; + /* compute outer product and add it to the Z matrix */ + Z11 += p1 * q1; + Z21 += p2 * q1; + Z31 += p3 * q1; + Z41 += p4 * q1; + /* load p and q values */ + p1 = ell[1]; + q1 = ex[1]; + p2 = ell[1 + lskip1]; + p3 = ell[1 + lskip2]; + p4 = ell[1 + lskip3]; + /* compute outer product and add it to the Z matrix */ + Z11 += p1 * q1; + Z21 += p2 * q1; + Z31 += p3 * q1; + Z41 += p4 * q1; + /* load p and q values */ + p1 = ell[2]; + q1 = ex[2]; + p2 = ell[2 + lskip1]; + p3 = ell[2 + lskip2]; + p4 = ell[2 + lskip3]; + /* compute outer product and add it to the Z matrix */ + Z11 += p1 * q1; + Z21 += p2 * q1; + Z31 += p3 * q1; + Z41 += p4 * q1; + /* load p and q values */ + p1 = ell[3]; + q1 = ex[3]; + p2 = ell[3 + lskip1]; + p3 = ell[3 + lskip2]; + p4 = ell[3 + lskip3]; + /* compute outer product and add it to the Z matrix */ + Z11 += p1 * q1; + Z21 += p2 * q1; + Z31 += p3 * q1; + Z41 += p4 * q1; + /* load p and q values */ + p1 = ell[4]; + q1 = ex[4]; + p2 = ell[4 + lskip1]; + p3 = ell[4 + lskip2]; + p4 = ell[4 + lskip3]; + /* compute outer product and add it to the Z matrix */ + Z11 += p1 * q1; + Z21 += p2 * q1; + Z31 += p3 * q1; + Z41 += p4 * q1; + /* load p and q values */ + p1 = ell[5]; + q1 = ex[5]; + p2 = ell[5 + lskip1]; + p3 = ell[5 + lskip2]; + p4 = ell[5 + lskip3]; + /* compute outer product and add it to the Z matrix */ + Z11 += p1 * q1; + Z21 += p2 * q1; + Z31 += p3 * q1; + Z41 += p4 * q1; + /* load p and q values */ + p1 = ell[6]; + q1 = ex[6]; + p2 = ell[6 + lskip1]; + p3 = ell[6 + lskip2]; + p4 = ell[6 + lskip3]; + /* compute outer product and add it to the Z matrix */ + Z11 += p1 * q1; + Z21 += p2 * q1; + Z31 += p3 * q1; + Z41 += p4 * q1; + /* load p and q values */ + p1 = ell[7]; + q1 = ex[7]; + p2 = ell[7 + lskip1]; + p3 = ell[7 + lskip2]; + p4 = ell[7 + lskip3]; + /* compute outer product and add it to the Z matrix */ + Z11 += p1 * q1; + Z21 += p2 * q1; + Z31 += p3 * q1; + Z41 += p4 * q1; + /* load p and q values */ + p1 = ell[8]; + q1 = ex[8]; + p2 = ell[8 + lskip1]; + p3 = ell[8 + lskip2]; + p4 = ell[8 + lskip3]; + /* compute outer product and add it to the Z matrix */ + Z11 += p1 * q1; + Z21 += p2 * q1; + Z31 += p3 * q1; + Z41 += p4 * q1; + /* load p and q values */ + p1 = ell[9]; + q1 = ex[9]; + p2 = ell[9 + lskip1]; + p3 = ell[9 + lskip2]; + p4 = ell[9 + lskip3]; + /* compute outer product and add it to the Z matrix */ + Z11 += p1 * q1; + Z21 += p2 * q1; + Z31 += p3 * q1; + Z41 += p4 * q1; + /* load p and q values */ + p1 = ell[10]; + q1 = ex[10]; + p2 = ell[10 + lskip1]; + p3 = ell[10 + lskip2]; + p4 = ell[10 + lskip3]; + /* compute outer product and add it to the Z matrix */ + Z11 += p1 * q1; + Z21 += p2 * q1; + Z31 += p3 * q1; + Z41 += p4 * q1; + /* load p and q values */ + p1 = ell[11]; + q1 = ex[11]; + p2 = ell[11 + lskip1]; + p3 = ell[11 + lskip2]; + p4 = ell[11 + lskip3]; + /* compute outer product and add it to the Z matrix */ + Z11 += p1 * q1; + Z21 += p2 * q1; + Z31 += p3 * q1; + Z41 += p4 * q1; + /* advance pointers */ + ell += 12; + ex += 12; + /* end of inner loop */ + } + /* compute left-over iterations */ + j += 12; + for (; j > 0; j--) + { + /* load p and q values */ + p1 = ell[0]; + q1 = ex[0]; + p2 = ell[lskip1]; + p3 = ell[lskip2]; + p4 = ell[lskip3]; + /* compute outer product and add it to the Z matrix */ + Z11 += p1 * q1; + Z21 += p2 * q1; + Z31 += p3 * q1; + Z41 += p4 * q1; + /* advance pointers */ + ell += 1; + ex += 1; + } + /* finish computing the X(i) block */ + Z11 = ex[0] - Z11; + ex[0] = Z11; + p1 = ell[lskip1]; + Z21 = ex[1] - Z21 - p1 * Z11; + ex[1] = Z21; + p1 = ell[lskip2]; + p2 = ell[1 + lskip2]; + Z31 = ex[2] - Z31 - p1 * Z11 - p2 * Z21; + ex[2] = Z31; + p1 = ell[lskip3]; + p2 = ell[1 + lskip3]; + p3 = ell[2 + lskip3]; + Z41 = ex[3] - Z41 - p1 * Z11 - p2 * Z21 - p3 * Z31; + ex[3] = Z41; + /* end of outer loop */ + } + /* compute rows at end that are not a multiple of block size */ + for (; i < n; i++) + { + /* compute all 1 x 1 block of X, from rows i..i+1-1 */ + /* set the Z matrix to 0 */ + Z11 = 0; + ell = L + i * lskip1; + ex = B; + /* the inner loop that computes outer products and adds them to Z */ + for (j = i - 12; j >= 0; j -= 12) + { + /* load p and q values */ + p1 = ell[0]; + q1 = ex[0]; + /* compute outer product and add it to the Z matrix */ + Z11 += p1 * q1; + /* load p and q values */ + p1 = ell[1]; + q1 = ex[1]; + /* compute outer product and add it to the Z matrix */ + Z11 += p1 * q1; + /* load p and q values */ + p1 = ell[2]; + q1 = ex[2]; + /* compute outer product and add it to the Z matrix */ + Z11 += p1 * q1; + /* load p and q values */ + p1 = ell[3]; + q1 = ex[3]; + /* compute outer product and add it to the Z matrix */ + Z11 += p1 * q1; + /* load p and q values */ + p1 = ell[4]; + q1 = ex[4]; + /* compute outer product and add it to the Z matrix */ + Z11 += p1 * q1; + /* load p and q values */ + p1 = ell[5]; + q1 = ex[5]; + /* compute outer product and add it to the Z matrix */ + Z11 += p1 * q1; + /* load p and q values */ + p1 = ell[6]; + q1 = ex[6]; + /* compute outer product and add it to the Z matrix */ + Z11 += p1 * q1; + /* load p and q values */ + p1 = ell[7]; + q1 = ex[7]; + /* compute outer product and add it to the Z matrix */ + Z11 += p1 * q1; + /* load p and q values */ + p1 = ell[8]; + q1 = ex[8]; + /* compute outer product and add it to the Z matrix */ + Z11 += p1 * q1; + /* load p and q values */ + p1 = ell[9]; + q1 = ex[9]; + /* compute outer product and add it to the Z matrix */ + Z11 += p1 * q1; + /* load p and q values */ + p1 = ell[10]; + q1 = ex[10]; + /* compute outer product and add it to the Z matrix */ + Z11 += p1 * q1; + /* load p and q values */ + p1 = ell[11]; + q1 = ex[11]; + /* compute outer product and add it to the Z matrix */ + Z11 += p1 * q1; + /* advance pointers */ + ell += 12; + ex += 12; + /* end of inner loop */ + } + /* compute left-over iterations */ + j += 12; + for (; j > 0; j--) + { + /* load p and q values */ + p1 = ell[0]; + q1 = ex[0]; + /* compute outer product and add it to the Z matrix */ + Z11 += p1 * q1; + /* advance pointers */ + ell += 1; + ex += 1; + } + /* finish computing the X(i) block */ + Z11 = ex[0] - Z11; + ex[0] = Z11; + } } /* solve L^T * x=b, with b containing 1 right hand side. @@ -816,215 +829,218 @@ void btSolveL1 (const btScalar *L, btScalar *B, int n, int lskip1) * this processes blocks of 4. */ -void btSolveL1T (const btScalar *L, btScalar *B, int n, int lskip1) -{ - /* declare variables - Z matrix, p and q vectors, etc */ - btScalar Z11,m11,Z21,m21,Z31,m31,Z41,m41,p1,q1,p2,p3,p4,*ex; - const btScalar *ell; - int lskip2,i,j; -// int lskip3; - /* special handling for L and B because we're solving L1 *transpose* */ - L = L + (n-1)*(lskip1+1); - B = B + n-1; - lskip1 = -lskip1; - /* compute lskip values */ - lskip2 = 2*lskip1; - //lskip3 = 3*lskip1; - /* compute all 4 x 1 blocks of X */ - for (i=0; i <= n-4; i+=4) { - /* compute all 4 x 1 block of X, from rows i..i+4-1 */ - /* set the Z matrix to 0 */ - Z11=0; - Z21=0; - Z31=0; - Z41=0; - ell = L - i; - ex = B; - /* the inner loop that computes outer products and adds them to Z */ - for (j=i-4; j >= 0; j -= 4) { - /* load p and q values */ - p1=ell[0]; - q1=ex[0]; - p2=ell[-1]; - p3=ell[-2]; - p4=ell[-3]; - /* compute outer product and add it to the Z matrix */ - m11 = p1 * q1; - m21 = p2 * q1; - m31 = p3 * q1; - m41 = p4 * q1; - ell += lskip1; - Z11 += m11; - Z21 += m21; - Z31 += m31; - Z41 += m41; - /* load p and q values */ - p1=ell[0]; - q1=ex[-1]; - p2=ell[-1]; - p3=ell[-2]; - p4=ell[-3]; - /* compute outer product and add it to the Z matrix */ - m11 = p1 * q1; - m21 = p2 * q1; - m31 = p3 * q1; - m41 = p4 * q1; - ell += lskip1; - Z11 += m11; - Z21 += m21; - Z31 += m31; - Z41 += m41; - /* load p and q values */ - p1=ell[0]; - q1=ex[-2]; - p2=ell[-1]; - p3=ell[-2]; - p4=ell[-3]; - /* compute outer product and add it to the Z matrix */ - m11 = p1 * q1; - m21 = p2 * q1; - m31 = p3 * q1; - m41 = p4 * q1; - ell += lskip1; - Z11 += m11; - Z21 += m21; - Z31 += m31; - Z41 += m41; - /* load p and q values */ - p1=ell[0]; - q1=ex[-3]; - p2=ell[-1]; - p3=ell[-2]; - p4=ell[-3]; - /* compute outer product and add it to the Z matrix */ - m11 = p1 * q1; - m21 = p2 * q1; - m31 = p3 * q1; - m41 = p4 * q1; - ell += lskip1; - ex -= 4; - Z11 += m11; - Z21 += m21; - Z31 += m31; - Z41 += m41; - /* end of inner loop */ - } - /* compute left-over iterations */ - j += 4; - for (; j > 0; j--) { - /* load p and q values */ - p1=ell[0]; - q1=ex[0]; - p2=ell[-1]; - p3=ell[-2]; - p4=ell[-3]; - /* compute outer product and add it to the Z matrix */ - m11 = p1 * q1; - m21 = p2 * q1; - m31 = p3 * q1; - m41 = p4 * q1; - ell += lskip1; - ex -= 1; - Z11 += m11; - Z21 += m21; - Z31 += m31; - Z41 += m41; - } - /* finish computing the X(i) block */ - Z11 = ex[0] - Z11; - ex[0] = Z11; - p1 = ell[-1]; - Z21 = ex[-1] - Z21 - p1*Z11; - ex[-1] = Z21; - p1 = ell[-2]; - p2 = ell[-2+lskip1]; - Z31 = ex[-2] - Z31 - p1*Z11 - p2*Z21; - ex[-2] = Z31; - p1 = ell[-3]; - p2 = ell[-3+lskip1]; - p3 = ell[-3+lskip2]; - Z41 = ex[-3] - Z41 - p1*Z11 - p2*Z21 - p3*Z31; - ex[-3] = Z41; - /* end of outer loop */ - } - /* compute rows at end that are not a multiple of block size */ - for (; i < n; i++) { - /* compute all 1 x 1 block of X, from rows i..i+1-1 */ - /* set the Z matrix to 0 */ - Z11=0; - ell = L - i; - ex = B; - /* the inner loop that computes outer products and adds them to Z */ - for (j=i-4; j >= 0; j -= 4) { - /* load p and q values */ - p1=ell[0]; - q1=ex[0]; - /* compute outer product and add it to the Z matrix */ - m11 = p1 * q1; - ell += lskip1; - Z11 += m11; - /* load p and q values */ - p1=ell[0]; - q1=ex[-1]; - /* compute outer product and add it to the Z matrix */ - m11 = p1 * q1; - ell += lskip1; - Z11 += m11; - /* load p and q values */ - p1=ell[0]; - q1=ex[-2]; - /* compute outer product and add it to the Z matrix */ - m11 = p1 * q1; - ell += lskip1; - Z11 += m11; - /* load p and q values */ - p1=ell[0]; - q1=ex[-3]; - /* compute outer product and add it to the Z matrix */ - m11 = p1 * q1; - ell += lskip1; - ex -= 4; - Z11 += m11; - /* end of inner loop */ - } - /* compute left-over iterations */ - j += 4; - for (; j > 0; j--) { - /* load p and q values */ - p1=ell[0]; - q1=ex[0]; - /* compute outer product and add it to the Z matrix */ - m11 = p1 * q1; - ell += lskip1; - ex -= 1; - Z11 += m11; - } - /* finish computing the X(i) block */ - Z11 = ex[0] - Z11; - ex[0] = Z11; - } +void btSolveL1T(const btScalar *L, btScalar *B, int n, int lskip1) +{ + /* declare variables - Z matrix, p and q vectors, etc */ + btScalar Z11, m11, Z21, m21, Z31, m31, Z41, m41, p1, q1, p2, p3, p4, *ex; + const btScalar *ell; + int lskip2, i, j; + // int lskip3; + /* special handling for L and B because we're solving L1 *transpose* */ + L = L + (n - 1) * (lskip1 + 1); + B = B + n - 1; + lskip1 = -lskip1; + /* compute lskip values */ + lskip2 = 2 * lskip1; + //lskip3 = 3*lskip1; + /* compute all 4 x 1 blocks of X */ + for (i = 0; i <= n - 4; i += 4) + { + /* compute all 4 x 1 block of X, from rows i..i+4-1 */ + /* set the Z matrix to 0 */ + Z11 = 0; + Z21 = 0; + Z31 = 0; + Z41 = 0; + ell = L - i; + ex = B; + /* the inner loop that computes outer products and adds them to Z */ + for (j = i - 4; j >= 0; j -= 4) + { + /* load p and q values */ + p1 = ell[0]; + q1 = ex[0]; + p2 = ell[-1]; + p3 = ell[-2]; + p4 = ell[-3]; + /* compute outer product and add it to the Z matrix */ + m11 = p1 * q1; + m21 = p2 * q1; + m31 = p3 * q1; + m41 = p4 * q1; + ell += lskip1; + Z11 += m11; + Z21 += m21; + Z31 += m31; + Z41 += m41; + /* load p and q values */ + p1 = ell[0]; + q1 = ex[-1]; + p2 = ell[-1]; + p3 = ell[-2]; + p4 = ell[-3]; + /* compute outer product and add it to the Z matrix */ + m11 = p1 * q1; + m21 = p2 * q1; + m31 = p3 * q1; + m41 = p4 * q1; + ell += lskip1; + Z11 += m11; + Z21 += m21; + Z31 += m31; + Z41 += m41; + /* load p and q values */ + p1 = ell[0]; + q1 = ex[-2]; + p2 = ell[-1]; + p3 = ell[-2]; + p4 = ell[-3]; + /* compute outer product and add it to the Z matrix */ + m11 = p1 * q1; + m21 = p2 * q1; + m31 = p3 * q1; + m41 = p4 * q1; + ell += lskip1; + Z11 += m11; + Z21 += m21; + Z31 += m31; + Z41 += m41; + /* load p and q values */ + p1 = ell[0]; + q1 = ex[-3]; + p2 = ell[-1]; + p3 = ell[-2]; + p4 = ell[-3]; + /* compute outer product and add it to the Z matrix */ + m11 = p1 * q1; + m21 = p2 * q1; + m31 = p3 * q1; + m41 = p4 * q1; + ell += lskip1; + ex -= 4; + Z11 += m11; + Z21 += m21; + Z31 += m31; + Z41 += m41; + /* end of inner loop */ + } + /* compute left-over iterations */ + j += 4; + for (; j > 0; j--) + { + /* load p and q values */ + p1 = ell[0]; + q1 = ex[0]; + p2 = ell[-1]; + p3 = ell[-2]; + p4 = ell[-3]; + /* compute outer product and add it to the Z matrix */ + m11 = p1 * q1; + m21 = p2 * q1; + m31 = p3 * q1; + m41 = p4 * q1; + ell += lskip1; + ex -= 1; + Z11 += m11; + Z21 += m21; + Z31 += m31; + Z41 += m41; + } + /* finish computing the X(i) block */ + Z11 = ex[0] - Z11; + ex[0] = Z11; + p1 = ell[-1]; + Z21 = ex[-1] - Z21 - p1 * Z11; + ex[-1] = Z21; + p1 = ell[-2]; + p2 = ell[-2 + lskip1]; + Z31 = ex[-2] - Z31 - p1 * Z11 - p2 * Z21; + ex[-2] = Z31; + p1 = ell[-3]; + p2 = ell[-3 + lskip1]; + p3 = ell[-3 + lskip2]; + Z41 = ex[-3] - Z41 - p1 * Z11 - p2 * Z21 - p3 * Z31; + ex[-3] = Z41; + /* end of outer loop */ + } + /* compute rows at end that are not a multiple of block size */ + for (; i < n; i++) + { + /* compute all 1 x 1 block of X, from rows i..i+1-1 */ + /* set the Z matrix to 0 */ + Z11 = 0; + ell = L - i; + ex = B; + /* the inner loop that computes outer products and adds them to Z */ + for (j = i - 4; j >= 0; j -= 4) + { + /* load p and q values */ + p1 = ell[0]; + q1 = ex[0]; + /* compute outer product and add it to the Z matrix */ + m11 = p1 * q1; + ell += lskip1; + Z11 += m11; + /* load p and q values */ + p1 = ell[0]; + q1 = ex[-1]; + /* compute outer product and add it to the Z matrix */ + m11 = p1 * q1; + ell += lskip1; + Z11 += m11; + /* load p and q values */ + p1 = ell[0]; + q1 = ex[-2]; + /* compute outer product and add it to the Z matrix */ + m11 = p1 * q1; + ell += lskip1; + Z11 += m11; + /* load p and q values */ + p1 = ell[0]; + q1 = ex[-3]; + /* compute outer product and add it to the Z matrix */ + m11 = p1 * q1; + ell += lskip1; + ex -= 4; + Z11 += m11; + /* end of inner loop */ + } + /* compute left-over iterations */ + j += 4; + for (; j > 0; j--) + { + /* load p and q values */ + p1 = ell[0]; + q1 = ex[0]; + /* compute outer product and add it to the Z matrix */ + m11 = p1 * q1; + ell += lskip1; + ex -= 1; + Z11 += m11; + } + /* finish computing the X(i) block */ + Z11 = ex[0] - Z11; + ex[0] = Z11; + } } - - -void btVectorScale (btScalar *a, const btScalar *d, int n) +void btVectorScale(btScalar *a, const btScalar *d, int n) { - btAssert (a && d && n >= 0); - for (int i=0; i<n; i++) { - a[i] *= d[i]; - } + btAssert(a && d && n >= 0); + for (int i = 0; i < n; i++) + { + a[i] *= d[i]; + } } -void btSolveLDLT (const btScalar *L, const btScalar *d, btScalar *b, int n, int nskip) +void btSolveLDLT(const btScalar *L, const btScalar *d, btScalar *b, int n, int nskip) { - btAssert (L && d && b && n > 0 && nskip >= n); - btSolveL1 (L,b,n,nskip); - btVectorScale (b,d,n); - btSolveL1T (L,b,n,nskip); + btAssert(L && d && b && n > 0 && nskip >= n); + btSolveL1(L, b, n, nskip); + btVectorScale(b, d, n); + btSolveL1T(L, b, n, nskip); } - - //*************************************************************************** // swap row/column i1 with i2 in the n*n matrix A. the leading dimension of @@ -1033,124 +1049,129 @@ void btSolveLDLT (const btScalar *L, const btScalar *d, btScalar *b, int n, int // rows will be swapped by exchanging row pointers. otherwise the data will // be copied. -static void btSwapRowsAndCols (BTATYPE A, int n, int i1, int i2, int nskip, - int do_fast_row_swaps) +static void btSwapRowsAndCols(BTATYPE A, int n, int i1, int i2, int nskip, + int do_fast_row_swaps) { - btAssert (A && n > 0 && i1 >= 0 && i2 >= 0 && i1 < n && i2 < n && - nskip >= n && i1 < i2); - -# ifdef BTROWPTRS - btScalar *A_i1 = A[i1]; - btScalar *A_i2 = A[i2]; - for (int i=i1+1; i<i2; ++i) { - btScalar *A_i_i1 = A[i] + i1; - A_i1[i] = *A_i_i1; - *A_i_i1 = A_i2[i]; - } - A_i1[i2] = A_i1[i1]; - A_i1[i1] = A_i2[i1]; - A_i2[i1] = A_i2[i2]; - // swap rows, by swapping row pointers - if (do_fast_row_swaps) { - A[i1] = A_i2; - A[i2] = A_i1; - } - else { - // Only swap till i2 column to match A plain storage variant. - for (int k = 0; k <= i2; ++k) { - btScalar tmp = A_i1[k]; - A_i1[k] = A_i2[k]; - A_i2[k] = tmp; - } - } - // swap columns the hard way - for (int j=i2+1; j<n; ++j) { - btScalar *A_j = A[j]; - btScalar tmp = A_j[i1]; - A_j[i1] = A_j[i2]; - A_j[i2] = tmp; - } -# else - btScalar *A_i1 = A+i1*nskip; - btScalar *A_i2 = A+i2*nskip; - for (int k = 0; k < i1; ++k) { - btScalar tmp = A_i1[k]; - A_i1[k] = A_i2[k]; - A_i2[k] = tmp; - } - btScalar *A_i = A_i1 + nskip; - for (int i=i1+1; i<i2; A_i+=nskip, ++i) { - btScalar tmp = A_i2[i]; - A_i2[i] = A_i[i1]; - A_i[i1] = tmp; - } - { - btScalar tmp = A_i1[i1]; - A_i1[i1] = A_i2[i2]; - A_i2[i2] = tmp; - } - btScalar *A_j = A_i2 + nskip; - for (int j=i2+1; j<n; A_j+=nskip, ++j) { - btScalar tmp = A_j[i1]; - A_j[i1] = A_j[i2]; - A_j[i2] = tmp; - } -# endif -} + btAssert(A && n > 0 && i1 >= 0 && i2 >= 0 && i1 < n && i2 < n && + nskip >= n && i1 < i2); +#ifdef BTROWPTRS + btScalar *A_i1 = A[i1]; + btScalar *A_i2 = A[i2]; + for (int i = i1 + 1; i < i2; ++i) + { + btScalar *A_i_i1 = A[i] + i1; + A_i1[i] = *A_i_i1; + *A_i_i1 = A_i2[i]; + } + A_i1[i2] = A_i1[i1]; + A_i1[i1] = A_i2[i1]; + A_i2[i1] = A_i2[i2]; + // swap rows, by swapping row pointers + if (do_fast_row_swaps) + { + A[i1] = A_i2; + A[i2] = A_i1; + } + else + { + // Only swap till i2 column to match A plain storage variant. + for (int k = 0; k <= i2; ++k) + { + btScalar tmp = A_i1[k]; + A_i1[k] = A_i2[k]; + A_i2[k] = tmp; + } + } + // swap columns the hard way + for (int j = i2 + 1; j < n; ++j) + { + btScalar *A_j = A[j]; + btScalar tmp = A_j[i1]; + A_j[i1] = A_j[i2]; + A_j[i2] = tmp; + } +#else + btScalar *A_i1 = A + i1 * nskip; + btScalar *A_i2 = A + i2 * nskip; + for (int k = 0; k < i1; ++k) + { + btScalar tmp = A_i1[k]; + A_i1[k] = A_i2[k]; + A_i2[k] = tmp; + } + btScalar *A_i = A_i1 + nskip; + for (int i = i1 + 1; i < i2; A_i += nskip, ++i) + { + btScalar tmp = A_i2[i]; + A_i2[i] = A_i[i1]; + A_i[i1] = tmp; + } + { + btScalar tmp = A_i1[i1]; + A_i1[i1] = A_i2[i2]; + A_i2[i2] = tmp; + } + btScalar *A_j = A_i2 + nskip; + for (int j = i2 + 1; j < n; A_j += nskip, ++j) + { + btScalar tmp = A_j[i1]; + A_j[i1] = A_j[i2]; + A_j[i2] = tmp; + } +#endif +} // swap two indexes in the n*n LCP problem. i1 must be <= i2. -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) +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) { - btScalar tmpr; - int tmpi; - bool tmpb; - btAssert (n>0 && i1 >=0 && i2 >= 0 && i1 < n && i2 < n && nskip >= n && i1 <= i2); - if (i1==i2) return; - - btSwapRowsAndCols (A,n,i1,i2,nskip,do_fast_row_swaps); - - tmpr = x[i1]; - x[i1] = x[i2]; - x[i2] = tmpr; - - tmpr = b[i1]; - b[i1] = b[i2]; - b[i2] = tmpr; - - tmpr = w[i1]; - w[i1] = w[i2]; - w[i2] = tmpr; - - tmpr = lo[i1]; - lo[i1] = lo[i2]; - lo[i2] = tmpr; - - tmpr = hi[i1]; - hi[i1] = hi[i2]; - hi[i2] = tmpr; - - tmpi = p[i1]; - p[i1] = p[i2]; - p[i2] = tmpi; - - tmpb = state[i1]; - state[i1] = state[i2]; - state[i2] = tmpb; - - if (findex) { - tmpi = findex[i1]; - findex[i1] = findex[i2]; - findex[i2] = tmpi; - } -} + btScalar tmpr; + int tmpi; + bool tmpb; + btAssert(n > 0 && i1 >= 0 && i2 >= 0 && i1 < n && i2 < n && nskip >= n && i1 <= i2); + if (i1 == i2) return; + + btSwapRowsAndCols(A, n, i1, i2, nskip, do_fast_row_swaps); + + tmpr = x[i1]; + x[i1] = x[i2]; + x[i2] = tmpr; + tmpr = b[i1]; + b[i1] = b[i2]; + b[i2] = tmpr; + tmpr = w[i1]; + w[i1] = w[i2]; + w[i2] = tmpr; + tmpr = lo[i1]; + lo[i1] = lo[i2]; + lo[i2] = tmpr; + + tmpr = hi[i1]; + hi[i1] = hi[i2]; + hi[i2] = tmpr; + + tmpi = p[i1]; + p[i1] = p[i2]; + p[i2] = tmpi; + + tmpb = state[i1]; + state[i1] = state[i2]; + state[i2] = tmpb; + + if (findex) + { + tmpi = findex[i1]; + findex[i1] = findex[i2]; + findex[i2] = tmpi; + } +} //*************************************************************************** // btLCP manipulator object. this represents an n*n LCP problem. @@ -1186,79 +1207,88 @@ static void btSwapProblem (BTATYPE A, btScalar *x, btScalar *b, btScalar *w, btS #ifdef btLCP_FAST -struct btLCP +struct btLCP { const int m_n; const int m_nskip; int m_nub; - int m_nC, m_nN; // size of each index set - BTATYPE const m_A; // A rows - btScalar *const m_x, * const m_b, *const m_w, *const m_lo,* const m_hi; // permuted LCP problem data - btScalar *const m_L, *const m_d; // L*D*L' factorization of set C + int m_nC, m_nN; // size of each index set + BTATYPE const m_A; // A rows + btScalar *const m_x, *const m_b, *const m_w, *const m_lo, *const m_hi; // permuted LCP problem data + btScalar *const m_L, *const m_d; // L*D*L' factorization of set C btScalar *const m_Dell, *const m_ell, *const m_tmp; bool *const m_state; int *const m_findex, *const m_p, *const m_C; - 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); + 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); int getNub() const { return m_nub; } - void transfer_i_to_C (int i); - void transfer_i_to_N (int i) { m_nN++; } // because we can assume C and N span 1:i-1 - void transfer_i_from_N_to_C (int i); - void transfer_i_from_C_to_N (int i, btAlignedObjectArray<btScalar>& scratch); + void transfer_i_to_C(int i); + void transfer_i_to_N(int i) { m_nN++; } // because we can assume C and N span 1:i-1 + void transfer_i_from_N_to_C(int i); + void transfer_i_from_C_to_N(int i, btAlignedObjectArray<btScalar> &scratch); int numC() const { return m_nC; } int numN() const { return m_nN; } - int indexC (int i) const { return i; } - int indexN (int i) const { return i+m_nC; } - btScalar Aii (int i) const { return BTAROW(i)[i]; } - btScalar AiC_times_qC (int i, btScalar *q) const { return btLargeDot (BTAROW(i), q, m_nC); } - btScalar AiN_times_qN (int i, btScalar *q) const { return btLargeDot (BTAROW(i)+m_nC, q+m_nC, m_nN); } - void pN_equals_ANC_times_qC (btScalar *p, btScalar *q); - void pN_plusequals_ANi (btScalar *p, int i, int sign=1); - void pC_plusequals_s_times_qC (btScalar *p, btScalar s, btScalar *q); - void pN_plusequals_s_times_qN (btScalar *p, btScalar s, btScalar *q); - void solve1 (btScalar *a, int i, int dir=1, int only_transfer=0); + int indexC(int i) const { return i; } + int indexN(int i) const { return i + m_nC; } + btScalar Aii(int i) const { return BTAROW(i)[i]; } + btScalar AiC_times_qC(int i, btScalar *q) const { return btLargeDot(BTAROW(i), q, m_nC); } + btScalar AiN_times_qN(int i, btScalar *q) const { return btLargeDot(BTAROW(i) + m_nC, q + m_nC, m_nN); } + void pN_equals_ANC_times_qC(btScalar *p, btScalar *q); + void pN_plusequals_ANi(btScalar *p, int i, int sign = 1); + void pC_plusequals_s_times_qC(btScalar *p, btScalar s, btScalar *q); + void pN_plusequals_s_times_qN(btScalar *p, btScalar s, btScalar *q); + void solve1(btScalar *a, int i, int dir = 1, int only_transfer = 0); void unpermute(); }; - -btLCP::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): - m_n(_n), m_nskip(_nskip), m_nub(_nub), m_nC(0), m_nN(0), -# ifdef BTROWPTRS - m_A(Arows), +btLCP::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) : m_n(_n), m_nskip(_nskip), m_nub(_nub), m_nC(0), m_nN(0), +#ifdef BTROWPTRS + m_A(Arows), #else - m_A(_Adata), + m_A(_Adata), #endif - m_x(_x), m_b(_b), m_w(_w), m_lo(_lo), m_hi(_hi), - m_L(l), m_d(_d), m_Dell(_Dell), m_ell(_ell), m_tmp(_tmp), - m_state(_state), m_findex(_findex), m_p(p), m_C(c) + m_x(_x), + m_b(_b), + m_w(_w), + m_lo(_lo), + m_hi(_hi), + m_L(l), + m_d(_d), + m_Dell(_Dell), + m_ell(_ell), + m_tmp(_tmp), + m_state(_state), + m_findex(_findex), + m_p(p), + m_C(c) { - { - btSetZero (m_x,m_n); - } + { + btSetZero(m_x, m_n); + } - { -# ifdef BTROWPTRS - // make matrix row pointers - btScalar *aptr = _Adata; - BTATYPE A = m_A; - const int n = m_n, nskip = m_nskip; - for (int k=0; k<n; aptr+=nskip, ++k) A[k] = aptr; -# endif - } + { +#ifdef BTROWPTRS + // make matrix row pointers + btScalar *aptr = _Adata; + BTATYPE A = m_A; + const int n = m_n, nskip = m_nskip; + for (int k = 0; k < n; aptr += nskip, ++k) A[k] = aptr; +#endif + } - { - int *p = m_p; - const int n = m_n; - for (int k=0; k<n; ++k) p[k]=k; // initially unpermuted - } + { + int *p = m_p; + const int n = m_n; + for (int k = 0; k < n; ++k) p[k] = k; // initially unpermuted + } - /* + /* // for testing, we can do some random swaps in the area i > nub { const int n = m_n; @@ -1277,63 +1307,69 @@ btLCP::btLCP (int _n, int _nskip, int _nub, btScalar *_Adata, btScalar *_x, btSc } */ - // permute the problem so that *all* the unbounded variables are at the - // start, i.e. look for unbounded variables not included in `nub'. we can - // potentially push up `nub' this way and get a bigger initial factorization. - // note that when we swap rows/cols here we must not just swap row pointers, - // as the initial factorization relies on the data being all in one chunk. - // variables that have findex >= 0 are *not* considered to be unbounded even - // if lo=-inf and hi=inf - this is because these limits may change during the - // solution process. - - { - int *findex = m_findex; - btScalar *lo = m_lo, *hi = m_hi; - const int n = m_n; - for (int k = m_nub; k<n; ++k) { - if (findex && findex[k] >= 0) continue; - if (lo[k]==-BT_INFINITY && hi[k]==BT_INFINITY) { - btSwapProblem (m_A,m_x,m_b,m_w,lo,hi,m_p,m_state,findex,n,m_nub,k,m_nskip,0); - m_nub++; - } - } - } - - // if there are unbounded variables at the start, factorize A up to that - // point and solve for x. this puts all indexes 0..nub-1 into C. - if (m_nub > 0) { - const int nub = m_nub; - { - btScalar *Lrow = m_L; - const int nskip = m_nskip; - for (int j=0; j<nub; Lrow+=nskip, ++j) memcpy(Lrow,BTAROW(j),(j+1)*sizeof(btScalar)); - } - btFactorLDLT (m_L,m_d,nub,m_nskip); - memcpy (m_x,m_b,nub*sizeof(btScalar)); - btSolveLDLT (m_L,m_d,m_x,nub,m_nskip); - btSetZero (m_w,nub); - { - int *C = m_C; - for (int k=0; k<nub; ++k) C[k] = k; - } - m_nC = nub; - } - - // permute the indexes > nub such that all findex variables are at the end - if (m_findex) { - const int nub = m_nub; - int *findex = m_findex; - int num_at_end = 0; - for (int k=m_n-1; k >= nub; k--) { - if (findex[k] >= 0) { - 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); - num_at_end++; - } - } - } + // permute the problem so that *all* the unbounded variables are at the + // start, i.e. look for unbounded variables not included in `nub'. we can + // potentially push up `nub' this way and get a bigger initial factorization. + // note that when we swap rows/cols here we must not just swap row pointers, + // as the initial factorization relies on the data being all in one chunk. + // variables that have findex >= 0 are *not* considered to be unbounded even + // if lo=-inf and hi=inf - this is because these limits may change during the + // solution process. - // print info about indexes - /* + { + int *findex = m_findex; + btScalar *lo = m_lo, *hi = m_hi; + const int n = m_n; + for (int k = m_nub; k < n; ++k) + { + if (findex && findex[k] >= 0) continue; + if (lo[k] == -BT_INFINITY && hi[k] == BT_INFINITY) + { + btSwapProblem(m_A, m_x, m_b, m_w, lo, hi, m_p, m_state, findex, n, m_nub, k, m_nskip, 0); + m_nub++; + } + } + } + + // if there are unbounded variables at the start, factorize A up to that + // point and solve for x. this puts all indexes 0..nub-1 into C. + if (m_nub > 0) + { + const int nub = m_nub; + { + btScalar *Lrow = m_L; + const int nskip = m_nskip; + for (int j = 0; j < nub; Lrow += nskip, ++j) memcpy(Lrow, BTAROW(j), (j + 1) * sizeof(btScalar)); + } + btFactorLDLT(m_L, m_d, nub, m_nskip); + memcpy(m_x, m_b, nub * sizeof(btScalar)); + btSolveLDLT(m_L, m_d, m_x, nub, m_nskip); + btSetZero(m_w, nub); + { + int *C = m_C; + for (int k = 0; k < nub; ++k) C[k] = k; + } + m_nC = nub; + } + + // permute the indexes > nub such that all findex variables are at the end + if (m_findex) + { + const int nub = m_nub; + int *findex = m_findex; + int num_at_end = 0; + for (int k = m_n - 1; k >= nub; k--) + { + if (findex[k] >= 0) + { + 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); + num_at_end++; + } + } + } + + // print info about indexes + /* { const int n = m_n; const int nub = m_nub; @@ -1347,734 +1383,776 @@ btLCP::btLCP (int _n, int _nskip, int _nub, btScalar *_Adata, btScalar *_x, btSc */ } - -void btLCP::transfer_i_to_C (int i) +void btLCP::transfer_i_to_C(int i) { - { - if (m_nC > 0) { - // ell,Dell were computed by solve1(). note, ell = D \ L1solve (L,A(i,C)) - { - const int nC = m_nC; - btScalar *const Ltgt = m_L + nC*m_nskip, *ell = m_ell; - for (int j=0; j<nC; ++j) Ltgt[j] = ell[j]; - } - const int nC = m_nC; - m_d[nC] = btRecip (BTAROW(i)[i] - btLargeDot(m_ell,m_Dell,nC)); - } - else { - m_d[0] = btRecip (BTAROW(i)[i]); - } - - 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); + { + if (m_nC > 0) + { + // ell,Dell were computed by solve1(). note, ell = D \ L1solve (L,A(i,C)) + { + const int nC = m_nC; + btScalar *const Ltgt = m_L + nC * m_nskip, *ell = m_ell; + for (int j = 0; j < nC; ++j) Ltgt[j] = ell[j]; + } + const int nC = m_nC; + m_d[nC] = btRecip(BTAROW(i)[i] - btLargeDot(m_ell, m_Dell, nC)); + } + else + { + m_d[0] = btRecip(BTAROW(i)[i]); + } - const int nC = m_nC; - m_C[nC] = nC; - m_nC = nC + 1; // nC value is outdated after this line - } + 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); + const int nC = m_nC; + m_C[nC] = nC; + m_nC = nC + 1; // nC value is outdated after this line + } } - -void btLCP::transfer_i_from_N_to_C (int i) +void btLCP::transfer_i_from_N_to_C(int i) { - { - if (m_nC > 0) { - { - btScalar *const aptr = BTAROW(i); - btScalar *Dell = m_Dell; - const int *C = m_C; -# ifdef BTNUB_OPTIMIZATIONS - // if nub>0, initial part of aptr unpermuted - const int nub = m_nub; - int j=0; - for ( ; j<nub; ++j) Dell[j] = aptr[j]; - const int nC = m_nC; - for ( ; j<nC; ++j) Dell[j] = aptr[C[j]]; -# else - const int nC = m_nC; - for (int j=0; j<nC; ++j) Dell[j] = aptr[C[j]]; -# endif - } - btSolveL1 (m_L,m_Dell,m_nC,m_nskip); - { - const int nC = m_nC; - btScalar *const Ltgt = m_L + nC*m_nskip; - btScalar *ell = m_ell, *Dell = m_Dell, *d = m_d; - for (int j=0; j<nC; ++j) Ltgt[j] = ell[j] = Dell[j] * d[j]; - } - const int nC = m_nC; - m_d[nC] = btRecip (BTAROW(i)[i] - btLargeDot(m_ell,m_Dell,nC)); - } - else { - m_d[0] = btRecip (BTAROW(i)[i]); - } - - 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); - - const int nC = m_nC; - m_C[nC] = nC; - m_nN--; - m_nC = nC + 1; // nC value is outdated after this line - } - - // @@@ TO DO LATER - // if we just finish here then we'll go back and re-solve for - // delta_x. but actually we can be more efficient and incrementally - // update delta_x here. but if we do this, we wont have ell and Dell - // to use in updating the factorization later. - + { + if (m_nC > 0) + { + { + btScalar *const aptr = BTAROW(i); + btScalar *Dell = m_Dell; + const int *C = m_C; +#ifdef BTNUB_OPTIMIZATIONS + // if nub>0, initial part of aptr unpermuted + const int nub = m_nub; + int j = 0; + for (; j < nub; ++j) Dell[j] = aptr[j]; + const int nC = m_nC; + for (; j < nC; ++j) Dell[j] = aptr[C[j]]; +#else + const int nC = m_nC; + for (int j = 0; j < nC; ++j) Dell[j] = aptr[C[j]]; +#endif + } + btSolveL1(m_L, m_Dell, m_nC, m_nskip); + { + const int nC = m_nC; + btScalar *const Ltgt = m_L + nC * m_nskip; + btScalar *ell = m_ell, *Dell = m_Dell, *d = m_d; + for (int j = 0; j < nC; ++j) Ltgt[j] = ell[j] = Dell[j] * d[j]; + } + const int nC = m_nC; + m_d[nC] = btRecip(BTAROW(i)[i] - btLargeDot(m_ell, m_Dell, nC)); + } + else + { + m_d[0] = btRecip(BTAROW(i)[i]); + } + + 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); + + const int nC = m_nC; + m_C[nC] = nC; + m_nN--; + m_nC = nC + 1; // nC value is outdated after this line + } + + // @@@ TO DO LATER + // if we just finish here then we'll go back and re-solve for + // delta_x. but actually we can be more efficient and incrementally + // update delta_x here. but if we do this, we wont have ell and Dell + // to use in updating the factorization later. } -void btRemoveRowCol (btScalar *A, int n, int nskip, int r) +void btRemoveRowCol(btScalar *A, int n, int nskip, int r) { - btAssert(A && n > 0 && nskip >= n && r >= 0 && r < n); - if (r >= n-1) return; - if (r > 0) { - { - const size_t move_size = (n-r-1)*sizeof(btScalar); - btScalar *Adst = A + r; - for (int i=0; i<r; Adst+=nskip,++i) { - btScalar *Asrc = Adst + 1; - memmove (Adst,Asrc,move_size); - } - } - { - const size_t cpy_size = r*sizeof(btScalar); - btScalar *Adst = A + r * nskip; - for (int i=r; i<(n-1); ++i) { - btScalar *Asrc = Adst + nskip; - memcpy (Adst,Asrc,cpy_size); - Adst = Asrc; - } - } - } - { - const size_t cpy_size = (n-r-1)*sizeof(btScalar); - btScalar *Adst = A + r * (nskip + 1); - for (int i=r; i<(n-1); ++i) { - btScalar *Asrc = Adst + (nskip + 1); - memcpy (Adst,Asrc,cpy_size); - Adst = Asrc - 1; - } - } + btAssert(A && n > 0 && nskip >= n && r >= 0 && r < n); + if (r >= n - 1) return; + if (r > 0) + { + { + const size_t move_size = (n - r - 1) * sizeof(btScalar); + btScalar *Adst = A + r; + for (int i = 0; i < r; Adst += nskip, ++i) + { + btScalar *Asrc = Adst + 1; + memmove(Adst, Asrc, move_size); + } + } + { + const size_t cpy_size = r * sizeof(btScalar); + btScalar *Adst = A + r * nskip; + for (int i = r; i < (n - 1); ++i) + { + btScalar *Asrc = Adst + nskip; + memcpy(Adst, Asrc, cpy_size); + Adst = Asrc; + } + } + } + { + const size_t cpy_size = (n - r - 1) * sizeof(btScalar); + btScalar *Adst = A + r * (nskip + 1); + for (int i = r; i < (n - 1); ++i) + { + btScalar *Asrc = Adst + (nskip + 1); + memcpy(Adst, Asrc, cpy_size); + Adst = Asrc - 1; + } + } } +void btLDLTAddTL(btScalar *L, btScalar *d, const btScalar *a, int n, int nskip, btAlignedObjectArray<btScalar> &scratch) +{ + btAssert(L && d && a && n > 0 && nskip >= n); + if (n < 2) return; + scratch.resize(2 * nskip); + btScalar *W1 = &scratch[0]; + btScalar *W2 = W1 + nskip; -void btLDLTAddTL (btScalar *L, btScalar *d, const btScalar *a, int n, int nskip, btAlignedObjectArray<btScalar>& scratch) -{ - btAssert (L && d && a && n > 0 && nskip >= n); - - if (n < 2) return; - scratch.resize(2*nskip); - btScalar *W1 = &scratch[0]; - - btScalar *W2 = W1 + nskip; - - W1[0] = btScalar(0.0); - W2[0] = btScalar(0.0); - for (int j=1; j<n; ++j) { - W1[j] = W2[j] = (btScalar) (a[j] * SIMDSQRT12); - } - btScalar W11 = (btScalar) ((btScalar(0.5)*a[0]+1)*SIMDSQRT12); - btScalar W21 = (btScalar) ((btScalar(0.5)*a[0]-1)*SIMDSQRT12); - - btScalar alpha1 = btScalar(1.0); - btScalar alpha2 = btScalar(1.0); + W1[0] = btScalar(0.0); + W2[0] = btScalar(0.0); + for (int j = 1; j < n; ++j) + { + W1[j] = W2[j] = (btScalar)(a[j] * SIMDSQRT12); + } + btScalar W11 = (btScalar)((btScalar(0.5) * a[0] + 1) * SIMDSQRT12); + btScalar W21 = (btScalar)((btScalar(0.5) * a[0] - 1) * SIMDSQRT12); - { - btScalar dee = d[0]; - btScalar alphanew = alpha1 + (W11*W11)*dee; - btAssert(alphanew != btScalar(0.0)); - dee /= alphanew; - btScalar gamma1 = W11 * dee; - dee *= alpha1; - alpha1 = alphanew; - alphanew = alpha2 - (W21*W21)*dee; - dee /= alphanew; - //btScalar gamma2 = W21 * dee; - alpha2 = alphanew; - btScalar k1 = btScalar(1.0) - W21*gamma1; - btScalar k2 = W21*gamma1*W11 - W21; - btScalar *ll = L + nskip; - for (int p=1; p<n; ll+=nskip, ++p) { - btScalar Wp = W1[p]; - btScalar ell = *ll; - W1[p] = Wp - W11*ell; - W2[p] = k1*Wp + k2*ell; - } - } + btScalar alpha1 = btScalar(1.0); + btScalar alpha2 = btScalar(1.0); - btScalar *ll = L + (nskip + 1); - for (int j=1; j<n; ll+=nskip+1, ++j) { - btScalar k1 = W1[j]; - btScalar k2 = W2[j]; - - btScalar dee = d[j]; - btScalar alphanew = alpha1 + (k1*k1)*dee; - btAssert(alphanew != btScalar(0.0)); - dee /= alphanew; - btScalar gamma1 = k1 * dee; - dee *= alpha1; - alpha1 = alphanew; - alphanew = alpha2 - (k2*k2)*dee; - dee /= alphanew; - btScalar gamma2 = k2 * dee; - dee *= alpha2; - d[j] = dee; - alpha2 = alphanew; - - btScalar *l = ll + nskip; - for (int p=j+1; p<n; l+=nskip, ++p) { - btScalar ell = *l; - btScalar Wp = W1[p] - k1 * ell; - ell += gamma1 * Wp; - W1[p] = Wp; - Wp = W2[p] - k2 * ell; - ell -= gamma2 * Wp; - W2[p] = Wp; - *l = ell; - } - } + { + btScalar dee = d[0]; + btScalar alphanew = alpha1 + (W11 * W11) * dee; + btAssert(alphanew != btScalar(0.0)); + dee /= alphanew; + btScalar gamma1 = W11 * dee; + dee *= alpha1; + alpha1 = alphanew; + alphanew = alpha2 - (W21 * W21) * dee; + dee /= alphanew; + //btScalar gamma2 = W21 * dee; + alpha2 = alphanew; + btScalar k1 = btScalar(1.0) - W21 * gamma1; + btScalar k2 = W21 * gamma1 * W11 - W21; + btScalar *ll = L + nskip; + for (int p = 1; p < n; ll += nskip, ++p) + { + btScalar Wp = W1[p]; + btScalar ell = *ll; + W1[p] = Wp - W11 * ell; + W2[p] = k1 * Wp + k2 * ell; + } + } + + btScalar *ll = L + (nskip + 1); + for (int j = 1; j < n; ll += nskip + 1, ++j) + { + btScalar k1 = W1[j]; + btScalar k2 = W2[j]; + + btScalar dee = d[j]; + btScalar alphanew = alpha1 + (k1 * k1) * dee; + btAssert(alphanew != btScalar(0.0)); + dee /= alphanew; + btScalar gamma1 = k1 * dee; + dee *= alpha1; + alpha1 = alphanew; + alphanew = alpha2 - (k2 * k2) * dee; + dee /= alphanew; + btScalar gamma2 = k2 * dee; + dee *= alpha2; + d[j] = dee; + alpha2 = alphanew; + + btScalar *l = ll + nskip; + for (int p = j + 1; p < n; l += nskip, ++p) + { + btScalar ell = *l; + btScalar Wp = W1[p] - k1 * ell; + ell += gamma1 * Wp; + W1[p] = Wp; + Wp = W2[p] - k2 * ell; + ell -= gamma2 * Wp; + W2[p] = Wp; + *l = ell; + } + } } - -#define _BTGETA(i,j) (A[i][j]) +#define _BTGETA(i, j) (A[i][j]) //#define _GETA(i,j) (A[(i)*nskip+(j)]) -#define BTGETA(i,j) ((i > j) ? _BTGETA(i,j) : _BTGETA(j,i)) +#define BTGETA(i, j) ((i > j) ? _BTGETA(i, j) : _BTGETA(j, i)) inline size_t btEstimateLDLTAddTLTmpbufSize(int nskip) { - return nskip * 2 * sizeof(btScalar); + return nskip * 2 * sizeof(btScalar); } - -void btLDLTRemove (btScalar **A, const int *p, btScalar *L, btScalar *d, - int n1, int n2, int r, int nskip, btAlignedObjectArray<btScalar>& scratch) +void btLDLTRemove(btScalar **A, const int *p, btScalar *L, btScalar *d, + int n1, int n2, int r, int nskip, btAlignedObjectArray<btScalar> &scratch) { - btAssert(A && p && L && d && n1 > 0 && n2 > 0 && r >= 0 && r < n2 && - n1 >= n2 && nskip >= n1); - #ifdef BT_DEBUG - for (int i=0; i<n2; ++i) + btAssert(A && p && L && d && n1 > 0 && n2 > 0 && r >= 0 && r < n2 && + n1 >= n2 && nskip >= n1); +#ifdef BT_DEBUG + for (int i = 0; i < n2; ++i) btAssert(p[i] >= 0 && p[i] < n1); - #endif - - if (r==n2-1) { - return; // deleting last row/col is easy - } - else { - size_t LDLTAddTL_size = btEstimateLDLTAddTLTmpbufSize(nskip); - btAssert(LDLTAddTL_size % sizeof(btScalar) == 0); - scratch.resize(nskip * 2+n2); - btScalar *tmp = &scratch[0]; - if (r==0) { - btScalar *a = (btScalar *)((char *)tmp + LDLTAddTL_size); - const int p_0 = p[0]; - for (int i=0; i<n2; ++i) { - a[i] = -BTGETA(p[i],p_0); - } - a[0] += btScalar(1.0); - btLDLTAddTL (L,d,a,n2,nskip,scratch); - } - else { - btScalar *t = (btScalar *)((char *)tmp + LDLTAddTL_size); - { - btScalar *Lcurr = L + r*nskip; - for (int i=0; i<r; ++Lcurr, ++i) { - btAssert(d[i] != btScalar(0.0)); - t[i] = *Lcurr / d[i]; - } - } - btScalar *a = t + r; - { - btScalar *Lcurr = L + r*nskip; - const int *pp_r = p + r, p_r = *pp_r; - const int n2_minus_r = n2-r; - for (int i=0; i<n2_minus_r; Lcurr+=nskip,++i) { - a[i] = btLargeDot(Lcurr,t,r) - BTGETA(pp_r[i],p_r); - } - } - a[0] += btScalar(1.0); - btLDLTAddTL (L + r*nskip+r, d+r, a, n2-r, nskip, scratch); - } - } +#endif - // snip out row/column r from L and d - btRemoveRowCol (L,n2,nskip,r); - if (r < (n2-1)) memmove (d+r,d+r+1,(n2-r-1)*sizeof(btScalar)); + if (r == n2 - 1) + { + return; // deleting last row/col is easy + } + else + { + size_t LDLTAddTL_size = btEstimateLDLTAddTLTmpbufSize(nskip); + btAssert(LDLTAddTL_size % sizeof(btScalar) == 0); + scratch.resize(nskip * 2 + n2); + btScalar *tmp = &scratch[0]; + if (r == 0) + { + btScalar *a = (btScalar *)((char *)tmp + LDLTAddTL_size); + const int p_0 = p[0]; + for (int i = 0; i < n2; ++i) + { + a[i] = -BTGETA(p[i], p_0); + } + a[0] += btScalar(1.0); + btLDLTAddTL(L, d, a, n2, nskip, scratch); + } + else + { + btScalar *t = (btScalar *)((char *)tmp + LDLTAddTL_size); + { + btScalar *Lcurr = L + r * nskip; + for (int i = 0; i < r; ++Lcurr, ++i) + { + btAssert(d[i] != btScalar(0.0)); + t[i] = *Lcurr / d[i]; + } + } + btScalar *a = t + r; + { + btScalar *Lcurr = L + r * nskip; + const int *pp_r = p + r, p_r = *pp_r; + const int n2_minus_r = n2 - r; + for (int i = 0; i < n2_minus_r; Lcurr += nskip, ++i) + { + a[i] = btLargeDot(Lcurr, t, r) - BTGETA(pp_r[i], p_r); + } + } + a[0] += btScalar(1.0); + btLDLTAddTL(L + r * nskip + r, d + r, a, n2 - r, nskip, scratch); + } + } + + // snip out row/column r from L and d + btRemoveRowCol(L, n2, nskip, r); + if (r < (n2 - 1)) memmove(d + r, d + r + 1, (n2 - r - 1) * sizeof(btScalar)); } - -void btLCP::transfer_i_from_C_to_N (int i, btAlignedObjectArray<btScalar>& scratch) +void btLCP::transfer_i_from_C_to_N(int i, btAlignedObjectArray<btScalar> &scratch) { - { - int *C = m_C; - // remove a row/column from the factorization, and adjust the - // indexes (black magic!) - int last_idx = -1; - const int nC = m_nC; - int j = 0; - for ( ; j<nC; ++j) { - if (C[j]==nC-1) { - last_idx = j; - } - if (C[j]==i) { - btLDLTRemove (m_A,C,m_L,m_d,m_n,nC,j,m_nskip,scratch); - int k; - if (last_idx == -1) { - for (k=j+1 ; k<nC; ++k) { - if (C[k]==nC-1) { - break; - } - } - btAssert (k < nC); - } - else { - k = last_idx; - } - C[k] = C[j]; - if (j < (nC-1)) memmove (C+j,C+j+1,(nC-j-1)*sizeof(int)); - break; - } - } - btAssert (j < nC); - - 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); - - m_nN++; - m_nC = nC - 1; // nC value is outdated after this line - } - + { + int *C = m_C; + // remove a row/column from the factorization, and adjust the + // indexes (black magic!) + int last_idx = -1; + const int nC = m_nC; + int j = 0; + for (; j < nC; ++j) + { + if (C[j] == nC - 1) + { + last_idx = j; + } + if (C[j] == i) + { + btLDLTRemove(m_A, C, m_L, m_d, m_n, nC, j, m_nskip, scratch); + int k; + if (last_idx == -1) + { + for (k = j + 1; k < nC; ++k) + { + if (C[k] == nC - 1) + { + break; + } + } + btAssert(k < nC); + } + else + { + k = last_idx; + } + C[k] = C[j]; + if (j < (nC - 1)) memmove(C + j, C + j + 1, (nC - j - 1) * sizeof(int)); + break; + } + } + btAssert(j < nC); + + 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); + + m_nN++; + m_nC = nC - 1; // nC value is outdated after this line + } } - -void btLCP::pN_equals_ANC_times_qC (btScalar *p, btScalar *q) +void btLCP::pN_equals_ANC_times_qC(btScalar *p, btScalar *q) { - // we could try to make this matrix-vector multiplication faster using - // outer product matrix tricks, e.g. with the dMultidotX() functions. - // but i tried it and it actually made things slower on random 100x100 - // problems because of the overhead involved. so we'll stick with the - // simple method for now. - const int nC = m_nC; - btScalar *ptgt = p + nC; - const int nN = m_nN; - for (int i=0; i<nN; ++i) { - ptgt[i] = btLargeDot (BTAROW(i+nC),q,nC); - } + // we could try to make this matrix-vector multiplication faster using + // outer product matrix tricks, e.g. with the dMultidotX() functions. + // but i tried it and it actually made things slower on random 100x100 + // problems because of the overhead involved. so we'll stick with the + // simple method for now. + const int nC = m_nC; + btScalar *ptgt = p + nC; + const int nN = m_nN; + for (int i = 0; i < nN; ++i) + { + ptgt[i] = btLargeDot(BTAROW(i + nC), q, nC); + } } - -void btLCP::pN_plusequals_ANi (btScalar *p, int i, int sign) +void btLCP::pN_plusequals_ANi(btScalar *p, int i, int sign) { - const int nC = m_nC; - btScalar *aptr = BTAROW(i) + nC; - btScalar *ptgt = p + nC; - if (sign > 0) { - const int nN = m_nN; - for (int j=0; j<nN; ++j) ptgt[j] += aptr[j]; - } - else { - const int nN = m_nN; - for (int j=0; j<nN; ++j) ptgt[j] -= aptr[j]; - } + const int nC = m_nC; + btScalar *aptr = BTAROW(i) + nC; + btScalar *ptgt = p + nC; + if (sign > 0) + { + const int nN = m_nN; + for (int j = 0; j < nN; ++j) ptgt[j] += aptr[j]; + } + else + { + const int nN = m_nN; + for (int j = 0; j < nN; ++j) ptgt[j] -= aptr[j]; + } } -void btLCP::pC_plusequals_s_times_qC (btScalar *p, btScalar s, btScalar *q) +void btLCP::pC_plusequals_s_times_qC(btScalar *p, btScalar s, btScalar *q) { - const int nC = m_nC; - for (int i=0; i<nC; ++i) { - p[i] += s*q[i]; - } + const int nC = m_nC; + for (int i = 0; i < nC; ++i) + { + p[i] += s * q[i]; + } } -void btLCP::pN_plusequals_s_times_qN (btScalar *p, btScalar s, btScalar *q) +void btLCP::pN_plusequals_s_times_qN(btScalar *p, btScalar s, btScalar *q) { - const int nC = m_nC; - btScalar *ptgt = p + nC, *qsrc = q + nC; - const int nN = m_nN; - for (int i=0; i<nN; ++i) { - ptgt[i] += s*qsrc[i]; - } + const int nC = m_nC; + btScalar *ptgt = p + nC, *qsrc = q + nC; + const int nN = m_nN; + for (int i = 0; i < nN; ++i) + { + ptgt[i] += s * qsrc[i]; + } } -void btLCP::solve1 (btScalar *a, int i, int dir, int only_transfer) +void btLCP::solve1(btScalar *a, int i, int dir, int only_transfer) { - // the `Dell' and `ell' that are computed here are saved. if index i is - // later added to the factorization then they can be reused. - // - // @@@ question: do we need to solve for entire delta_x??? yes, but - // only if an x goes below 0 during the step. - - if (m_nC > 0) { - { - btScalar *Dell = m_Dell; - int *C = m_C; - btScalar *aptr = BTAROW(i); -# ifdef BTNUB_OPTIMIZATIONS - // if nub>0, initial part of aptr[] is guaranteed unpermuted - const int nub = m_nub; - int j=0; - for ( ; j<nub; ++j) Dell[j] = aptr[j]; - const int nC = m_nC; - for ( ; j<nC; ++j) Dell[j] = aptr[C[j]]; -# else - const int nC = m_nC; - for (int j=0; j<nC; ++j) Dell[j] = aptr[C[j]]; -# endif - } - btSolveL1 (m_L,m_Dell,m_nC,m_nskip); - { - btScalar *ell = m_ell, *Dell = m_Dell, *d = m_d; - const int nC = m_nC; - for (int j=0; j<nC; ++j) ell[j] = Dell[j] * d[j]; - } + // the `Dell' and `ell' that are computed here are saved. if index i is + // later added to the factorization then they can be reused. + // + // @@@ question: do we need to solve for entire delta_x??? yes, but + // only if an x goes below 0 during the step. - if (!only_transfer) { - btScalar *tmp = m_tmp, *ell = m_ell; - { - const int nC = m_nC; - for (int j=0; j<nC; ++j) tmp[j] = ell[j]; - } - btSolveL1T (m_L,tmp,m_nC,m_nskip); - if (dir > 0) { - int *C = m_C; - btScalar *tmp = m_tmp; - const int nC = m_nC; - for (int j=0; j<nC; ++j) a[C[j]] = -tmp[j]; - } else { - int *C = m_C; - btScalar *tmp = m_tmp; - const int nC = m_nC; - for (int j=0; j<nC; ++j) a[C[j]] = tmp[j]; - } - } - } -} + if (m_nC > 0) + { + { + btScalar *Dell = m_Dell; + int *C = m_C; + btScalar *aptr = BTAROW(i); +#ifdef BTNUB_OPTIMIZATIONS + // if nub>0, initial part of aptr[] is guaranteed unpermuted + const int nub = m_nub; + int j = 0; + for (; j < nub; ++j) Dell[j] = aptr[j]; + const int nC = m_nC; + for (; j < nC; ++j) Dell[j] = aptr[C[j]]; +#else + const int nC = m_nC; + for (int j = 0; j < nC; ++j) Dell[j] = aptr[C[j]]; +#endif + } + btSolveL1(m_L, m_Dell, m_nC, m_nskip); + { + btScalar *ell = m_ell, *Dell = m_Dell, *d = m_d; + const int nC = m_nC; + for (int j = 0; j < nC; ++j) ell[j] = Dell[j] * d[j]; + } + if (!only_transfer) + { + btScalar *tmp = m_tmp, *ell = m_ell; + { + const int nC = m_nC; + for (int j = 0; j < nC; ++j) tmp[j] = ell[j]; + } + btSolveL1T(m_L, tmp, m_nC, m_nskip); + if (dir > 0) + { + int *C = m_C; + btScalar *tmp = m_tmp; + const int nC = m_nC; + for (int j = 0; j < nC; ++j) a[C[j]] = -tmp[j]; + } + else + { + int *C = m_C; + btScalar *tmp = m_tmp; + const int nC = m_nC; + for (int j = 0; j < nC; ++j) a[C[j]] = tmp[j]; + } + } + } +} void btLCP::unpermute() { - // now we have to un-permute x and w - { - memcpy (m_tmp,m_x,m_n*sizeof(btScalar)); - btScalar *x = m_x, *tmp = m_tmp; - const int *p = m_p; - const int n = m_n; - for (int j=0; j<n; ++j) x[p[j]] = tmp[j]; - } - { - memcpy (m_tmp,m_w,m_n*sizeof(btScalar)); - btScalar *w = m_w, *tmp = m_tmp; - const int *p = m_p; - const int n = m_n; - for (int j=0; j<n; ++j) w[p[j]] = tmp[j]; - } + // now we have to un-permute x and w + { + memcpy(m_tmp, m_x, m_n * sizeof(btScalar)); + btScalar *x = m_x, *tmp = m_tmp; + const int *p = m_p; + const int n = m_n; + for (int j = 0; j < n; ++j) x[p[j]] = tmp[j]; + } + { + memcpy(m_tmp, m_w, m_n * sizeof(btScalar)); + btScalar *w = m_w, *tmp = m_tmp; + const int *p = m_p; + const int n = m_n; + for (int j = 0; j < n; ++j) w[p[j]] = tmp[j]; + } } -#endif // btLCP_FAST - +#endif // btLCP_FAST //*************************************************************************** // an optimized Dantzig LCP driver routine for the lo-hi LCP problem. -bool btSolveDantzigLCP (int n, btScalar *A, btScalar *x, btScalar *b, - btScalar* outer_w, int nub, btScalar *lo, btScalar *hi, int *findex, btDantzigScratchMemory& scratchMem) +bool btSolveDantzigLCP(int n, btScalar *A, btScalar *x, btScalar *b, + btScalar *outer_w, int nub, btScalar *lo, btScalar *hi, int *findex, btDantzigScratchMemory &scratchMem) { s_error = false; -// printf("btSolveDantzigLCP n=%d\n",n); - btAssert (n>0 && A && x && b && lo && hi && nub >= 0 && nub <= n); - btAssert(outer_w); + // printf("btSolveDantzigLCP n=%d\n",n); + btAssert(n > 0 && A && x && b && lo && hi && nub >= 0 && nub <= n); + btAssert(outer_w); #ifdef BT_DEBUG - { - // check restrictions on lo and hi - for (int k=0; k<n; ++k) - btAssert (lo[k] <= 0 && hi[k] >= 0); - } -# endif - - - // if all the variables are unbounded then we can just factor, solve, - // and return - if (nub >= n) - { - - - int nskip = (n); - btFactorLDLT (A, outer_w, n, nskip); - btSolveLDLT (A, outer_w, b, n, nskip); - memcpy (x, b, n*sizeof(btScalar)); - - return !s_error; - } - - const int nskip = (n); - scratchMem.L.resize(n*nskip); - - scratchMem.d.resize(n); - - btScalar *w = outer_w; - scratchMem.delta_w.resize(n); - scratchMem.delta_x.resize(n); - scratchMem.Dell.resize(n); - scratchMem.ell.resize(n); - scratchMem.Arows.resize(n); - scratchMem.p.resize(n); - scratchMem.C.resize(n); - - // for i in N, state[i] is 0 if x(i)==lo(i) or 1 if x(i)==hi(i) - scratchMem.state.resize(n); - - - // create LCP object. note that tmp is set to delta_w to save space, this - // optimization relies on knowledge of how tmp is used, so be careful! - 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]); - int adj_nub = lcp.getNub(); - - // loop over all indexes adj_nub..n-1. for index i, if x(i),w(i) satisfy the - // LCP conditions then i is added to the appropriate index set. otherwise - // x(i),w(i) is driven either +ve or -ve to force it to the valid region. - // as we drive x(i), x(C) is also adjusted to keep w(C) at zero. - // while driving x(i) we maintain the LCP conditions on the other variables - // 0..i-1. we do this by watching out for other x(i),w(i) values going - // outside the valid region, and then switching them between index sets - // when that happens. - - bool hit_first_friction_index = false; - for (int i=adj_nub; i<n; ++i) - { - s_error = false; - // the index i is the driving index and indexes i+1..n-1 are "dont care", - // i.e. when we make changes to the system those x's will be zero and we - // don't care what happens to those w's. in other words, we only consider - // an (i+1)*(i+1) sub-problem of A*x=b+w. - - // if we've hit the first friction index, we have to compute the lo and - // hi values based on the values of x already computed. we have been - // permuting the indexes, so the values stored in the findex vector are - // no longer valid. thus we have to temporarily unpermute the x vector. - // for the purposes of this computation, 0*infinity = 0 ... so if the - // contact constraint's normal force is 0, there should be no tangential - // force applied. - - if (!hit_first_friction_index && findex && findex[i] >= 0) { - // un-permute x into delta_w, which is not being used at the moment - for (int j=0; j<n; ++j) scratchMem.delta_w[scratchMem.p[j]] = x[j]; - - // set lo and hi values - for (int k=i; k<n; ++k) { - btScalar wfk = scratchMem.delta_w[findex[k]]; - if (wfk == 0) { - hi[k] = 0; - lo[k] = 0; - } - else { - hi[k] = btFabs (hi[k] * wfk); - lo[k] = -hi[k]; - } - } - hit_first_friction_index = true; - } - - // thus far we have not even been computing the w values for indexes - // greater than i, so compute w[i] now. - w[i] = lcp.AiC_times_qC (i,x) + lcp.AiN_times_qN (i,x) - b[i]; - - // if lo=hi=0 (which can happen for tangential friction when normals are - // 0) then the index will be assigned to set N with some state. however, - // set C's line has zero size, so the index will always remain in set N. - // with the "normal" switching logic, if w changed sign then the index - // would have to switch to set C and then back to set N with an inverted - // state. this is pointless, and also computationally expensive. to - // prevent this from happening, we use the rule that indexes with lo=hi=0 - // will never be checked for set changes. this means that the state for - // these indexes may be incorrect, but that doesn't matter. - - // see if x(i),w(i) is in a valid region - if (lo[i]==0 && w[i] >= 0) { - lcp.transfer_i_to_N (i); - scratchMem.state[i] = false; - } - else if (hi[i]==0 && w[i] <= 0) { - lcp.transfer_i_to_N (i); - scratchMem.state[i] = true; - } - else if (w[i]==0) { - // this is a degenerate case. by the time we get to this test we know - // that lo != 0, which means that lo < 0 as lo is not allowed to be +ve, - // and similarly that hi > 0. this means that the line segment - // corresponding to set C is at least finite in extent, and we are on it. - // NOTE: we must call lcp.solve1() before lcp.transfer_i_to_C() - lcp.solve1 (&scratchMem.delta_x[0],i,0,1); - - lcp.transfer_i_to_C (i); - } - else { - // we must push x(i) and w(i) - for (;;) { - int dir; - btScalar dirf; - // find direction to push on x(i) - if (w[i] <= 0) { - dir = 1; - dirf = btScalar(1.0); - } - else { - dir = -1; - dirf = btScalar(-1.0); - } - - // compute: delta_x(C) = -dir*A(C,C)\A(C,i) - lcp.solve1 (&scratchMem.delta_x[0],i,dir); - - // note that delta_x[i] = dirf, but we wont bother to set it - - // compute: delta_w = A*delta_x ... note we only care about - // delta_w(N) and delta_w(i), the rest is ignored - lcp.pN_equals_ANC_times_qC (&scratchMem.delta_w[0],&scratchMem.delta_x[0]); - lcp.pN_plusequals_ANi (&scratchMem.delta_w[0],i,dir); - scratchMem.delta_w[i] = lcp.AiC_times_qC (i,&scratchMem.delta_x[0]) + lcp.Aii(i)*dirf; - - // find largest step we can take (size=s), either to drive x(i),w(i) - // to the valid LCP region or to drive an already-valid variable - // outside the valid region. - - int cmd = 1; // index switching command - int si = 0; // si = index to switch if cmd>3 - btScalar s = -w[i]/scratchMem.delta_w[i]; - if (dir > 0) { - if (hi[i] < BT_INFINITY) { - btScalar s2 = (hi[i]-x[i])*dirf; // was (hi[i]-x[i])/dirf // step to x(i)=hi(i) - if (s2 < s) { - s = s2; - cmd = 3; - } - } - } - else { - if (lo[i] > -BT_INFINITY) { - btScalar s2 = (lo[i]-x[i])*dirf; // was (lo[i]-x[i])/dirf // step to x(i)=lo(i) - if (s2 < s) { - s = s2; - cmd = 2; - } - } - } - - { - const int numN = lcp.numN(); - for (int k=0; k < numN; ++k) { - const int indexN_k = lcp.indexN(k); - if (!scratchMem.state[indexN_k] ? scratchMem.delta_w[indexN_k] < 0 : scratchMem.delta_w[indexN_k] > 0) { - // don't bother checking if lo=hi=0 - if (lo[indexN_k] == 0 && hi[indexN_k] == 0) continue; - btScalar s2 = -w[indexN_k] / scratchMem.delta_w[indexN_k]; - if (s2 < s) { - s = s2; - cmd = 4; - si = indexN_k; - } - } - } - } - - { - const int numC = lcp.numC(); - for (int k=adj_nub; k < numC; ++k) { - const int indexC_k = lcp.indexC(k); - if (scratchMem.delta_x[indexC_k] < 0 && lo[indexC_k] > -BT_INFINITY) { - btScalar s2 = (lo[indexC_k]-x[indexC_k]) / scratchMem.delta_x[indexC_k]; - if (s2 < s) { - s = s2; - cmd = 5; - si = indexC_k; - } - } - if (scratchMem.delta_x[indexC_k] > 0 && hi[indexC_k] < BT_INFINITY) { - btScalar s2 = (hi[indexC_k]-x[indexC_k]) / scratchMem.delta_x[indexC_k]; - if (s2 < s) { - s = s2; - cmd = 6; - si = indexC_k; - } - } - } - } - - //static char* cmdstring[8] = {0,"->C","->NL","->NH","N->C", - // "C->NL","C->NH"}; - //printf ("cmd=%d (%s), si=%d\n",cmd,cmdstring[cmd],(cmd>3) ? si : i); - - // if s <= 0 then we've got a problem. if we just keep going then - // we're going to get stuck in an infinite loop. instead, just cross - // our fingers and exit with the current solution. - if (s <= btScalar(0.0)) - { -// printf("LCP internal error, s <= 0 (s=%.4e)",(double)s); - if (i < n) { - btSetZero (x+i,n-i); - btSetZero (w+i,n-i); - } - s_error = true; - break; - } - - // apply x = x + s * delta_x - lcp.pC_plusequals_s_times_qC (x, s, &scratchMem.delta_x[0]); - x[i] += s * dirf; - - // apply w = w + s * delta_w - lcp.pN_plusequals_s_times_qN (w, s, &scratchMem.delta_w[0]); - w[i] += s * scratchMem.delta_w[i]; - -// void *tmpbuf; - // switch indexes between sets if necessary - switch (cmd) { - case 1: // done - w[i] = 0; - lcp.transfer_i_to_C (i); - break; - case 2: // done - x[i] = lo[i]; - scratchMem.state[i] = false; - lcp.transfer_i_to_N (i); - break; - case 3: // done - x[i] = hi[i]; - scratchMem.state[i] = true; - lcp.transfer_i_to_N (i); - break; - case 4: // keep going - w[si] = 0; - lcp.transfer_i_from_N_to_C (si); - break; - case 5: // keep going - x[si] = lo[si]; - scratchMem.state[si] = false; - lcp.transfer_i_from_C_to_N (si, scratchMem.m_scratch); - break; - case 6: // keep going - x[si] = hi[si]; - scratchMem.state[si] = true; - lcp.transfer_i_from_C_to_N (si, scratchMem.m_scratch); - break; - } - - if (cmd <= 3) break; - } // for (;;) - } // else - - if (s_error) { - break; - } - } // for (int i=adj_nub; i<n; ++i) + // check restrictions on lo and hi + for (int k = 0; k < n; ++k) + btAssert(lo[k] <= 0 && hi[k] >= 0); + } +#endif - lcp.unpermute(); + // if all the variables are unbounded then we can just factor, solve, + // and return + if (nub >= n) + { + int nskip = (n); + btFactorLDLT(A, outer_w, n, nskip); + btSolveLDLT(A, outer_w, b, n, nskip); + memcpy(x, b, n * sizeof(btScalar)); + + return !s_error; + } + + const int nskip = (n); + scratchMem.L.resize(n * nskip); + + scratchMem.d.resize(n); + + btScalar *w = outer_w; + scratchMem.delta_w.resize(n); + scratchMem.delta_x.resize(n); + scratchMem.Dell.resize(n); + scratchMem.ell.resize(n); + scratchMem.Arows.resize(n); + scratchMem.p.resize(n); + scratchMem.C.resize(n); + + // for i in N, state[i] is 0 if x(i)==lo(i) or 1 if x(i)==hi(i) + scratchMem.state.resize(n); + + // create LCP object. note that tmp is set to delta_w to save space, this + // optimization relies on knowledge of how tmp is used, so be careful! + 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]); + int adj_nub = lcp.getNub(); + + // loop over all indexes adj_nub..n-1. for index i, if x(i),w(i) satisfy the + // LCP conditions then i is added to the appropriate index set. otherwise + // x(i),w(i) is driven either +ve or -ve to force it to the valid region. + // as we drive x(i), x(C) is also adjusted to keep w(C) at zero. + // while driving x(i) we maintain the LCP conditions on the other variables + // 0..i-1. we do this by watching out for other x(i),w(i) values going + // outside the valid region, and then switching them between index sets + // when that happens. + + bool hit_first_friction_index = false; + for (int i = adj_nub; i < n; ++i) + { + s_error = false; + // the index i is the driving index and indexes i+1..n-1 are "dont care", + // i.e. when we make changes to the system those x's will be zero and we + // don't care what happens to those w's. in other words, we only consider + // an (i+1)*(i+1) sub-problem of A*x=b+w. + + // if we've hit the first friction index, we have to compute the lo and + // hi values based on the values of x already computed. we have been + // permuting the indexes, so the values stored in the findex vector are + // no longer valid. thus we have to temporarily unpermute the x vector. + // for the purposes of this computation, 0*infinity = 0 ... so if the + // contact constraint's normal force is 0, there should be no tangential + // force applied. + + if (!hit_first_friction_index && findex && findex[i] >= 0) + { + // un-permute x into delta_w, which is not being used at the moment + for (int j = 0; j < n; ++j) scratchMem.delta_w[scratchMem.p[j]] = x[j]; + + // set lo and hi values + for (int k = i; k < n; ++k) + { + btScalar wfk = scratchMem.delta_w[findex[k]]; + if (wfk == 0) + { + hi[k] = 0; + lo[k] = 0; + } + else + { + hi[k] = btFabs(hi[k] * wfk); + lo[k] = -hi[k]; + } + } + hit_first_friction_index = true; + } + + // thus far we have not even been computing the w values for indexes + // greater than i, so compute w[i] now. + w[i] = lcp.AiC_times_qC(i, x) + lcp.AiN_times_qN(i, x) - b[i]; + + // if lo=hi=0 (which can happen for tangential friction when normals are + // 0) then the index will be assigned to set N with some state. however, + // set C's line has zero size, so the index will always remain in set N. + // with the "normal" switching logic, if w changed sign then the index + // would have to switch to set C and then back to set N with an inverted + // state. this is pointless, and also computationally expensive. to + // prevent this from happening, we use the rule that indexes with lo=hi=0 + // will never be checked for set changes. this means that the state for + // these indexes may be incorrect, but that doesn't matter. + + // see if x(i),w(i) is in a valid region + if (lo[i] == 0 && w[i] >= 0) + { + lcp.transfer_i_to_N(i); + scratchMem.state[i] = false; + } + else if (hi[i] == 0 && w[i] <= 0) + { + lcp.transfer_i_to_N(i); + scratchMem.state[i] = true; + } + else if (w[i] == 0) + { + // this is a degenerate case. by the time we get to this test we know + // that lo != 0, which means that lo < 0 as lo is not allowed to be +ve, + // and similarly that hi > 0. this means that the line segment + // corresponding to set C is at least finite in extent, and we are on it. + // NOTE: we must call lcp.solve1() before lcp.transfer_i_to_C() + lcp.solve1(&scratchMem.delta_x[0], i, 0, 1); + + lcp.transfer_i_to_C(i); + } + else + { + // we must push x(i) and w(i) + for (;;) + { + int dir; + btScalar dirf; + // find direction to push on x(i) + if (w[i] <= 0) + { + dir = 1; + dirf = btScalar(1.0); + } + else + { + dir = -1; + dirf = btScalar(-1.0); + } + + // compute: delta_x(C) = -dir*A(C,C)\A(C,i) + lcp.solve1(&scratchMem.delta_x[0], i, dir); + + // note that delta_x[i] = dirf, but we wont bother to set it + + // compute: delta_w = A*delta_x ... note we only care about + // delta_w(N) and delta_w(i), the rest is ignored + lcp.pN_equals_ANC_times_qC(&scratchMem.delta_w[0], &scratchMem.delta_x[0]); + lcp.pN_plusequals_ANi(&scratchMem.delta_w[0], i, dir); + scratchMem.delta_w[i] = lcp.AiC_times_qC(i, &scratchMem.delta_x[0]) + lcp.Aii(i) * dirf; + + // find largest step we can take (size=s), either to drive x(i),w(i) + // to the valid LCP region or to drive an already-valid variable + // outside the valid region. + + int cmd = 1; // index switching command + int si = 0; // si = index to switch if cmd>3 + btScalar s = -w[i] / scratchMem.delta_w[i]; + if (dir > 0) + { + if (hi[i] < BT_INFINITY) + { + btScalar s2 = (hi[i] - x[i]) * dirf; // was (hi[i]-x[i])/dirf // step to x(i)=hi(i) + if (s2 < s) + { + s = s2; + cmd = 3; + } + } + } + else + { + if (lo[i] > -BT_INFINITY) + { + btScalar s2 = (lo[i] - x[i]) * dirf; // was (lo[i]-x[i])/dirf // step to x(i)=lo(i) + if (s2 < s) + { + s = s2; + cmd = 2; + } + } + } + + { + const int numN = lcp.numN(); + for (int k = 0; k < numN; ++k) + { + const int indexN_k = lcp.indexN(k); + if (!scratchMem.state[indexN_k] ? scratchMem.delta_w[indexN_k] < 0 : scratchMem.delta_w[indexN_k] > 0) + { + // don't bother checking if lo=hi=0 + if (lo[indexN_k] == 0 && hi[indexN_k] == 0) continue; + btScalar s2 = -w[indexN_k] / scratchMem.delta_w[indexN_k]; + if (s2 < s) + { + s = s2; + cmd = 4; + si = indexN_k; + } + } + } + } + + { + const int numC = lcp.numC(); + for (int k = adj_nub; k < numC; ++k) + { + const int indexC_k = lcp.indexC(k); + if (scratchMem.delta_x[indexC_k] < 0 && lo[indexC_k] > -BT_INFINITY) + { + btScalar s2 = (lo[indexC_k] - x[indexC_k]) / scratchMem.delta_x[indexC_k]; + if (s2 < s) + { + s = s2; + cmd = 5; + si = indexC_k; + } + } + if (scratchMem.delta_x[indexC_k] > 0 && hi[indexC_k] < BT_INFINITY) + { + btScalar s2 = (hi[indexC_k] - x[indexC_k]) / scratchMem.delta_x[indexC_k]; + if (s2 < s) + { + s = s2; + cmd = 6; + si = indexC_k; + } + } + } + } + + //static char* cmdstring[8] = {0,"->C","->NL","->NH","N->C", + // "C->NL","C->NH"}; + //printf ("cmd=%d (%s), si=%d\n",cmd,cmdstring[cmd],(cmd>3) ? si : i); + + // if s <= 0 then we've got a problem. if we just keep going then + // we're going to get stuck in an infinite loop. instead, just cross + // our fingers and exit with the current solution. + if (s <= btScalar(0.0)) + { + // printf("LCP internal error, s <= 0 (s=%.4e)",(double)s); + if (i < n) + { + btSetZero(x + i, n - i); + btSetZero(w + i, n - i); + } + s_error = true; + break; + } + + // apply x = x + s * delta_x + lcp.pC_plusequals_s_times_qC(x, s, &scratchMem.delta_x[0]); + x[i] += s * dirf; + + // apply w = w + s * delta_w + lcp.pN_plusequals_s_times_qN(w, s, &scratchMem.delta_w[0]); + w[i] += s * scratchMem.delta_w[i]; + + // void *tmpbuf; + // switch indexes between sets if necessary + switch (cmd) + { + case 1: // done + w[i] = 0; + lcp.transfer_i_to_C(i); + break; + case 2: // done + x[i] = lo[i]; + scratchMem.state[i] = false; + lcp.transfer_i_to_N(i); + break; + case 3: // done + x[i] = hi[i]; + scratchMem.state[i] = true; + lcp.transfer_i_to_N(i); + break; + case 4: // keep going + w[si] = 0; + lcp.transfer_i_from_N_to_C(si); + break; + case 5: // keep going + x[si] = lo[si]; + scratchMem.state[si] = false; + lcp.transfer_i_from_C_to_N(si, scratchMem.m_scratch); + break; + case 6: // keep going + x[si] = hi[si]; + scratchMem.state[si] = true; + lcp.transfer_i_from_C_to_N(si, scratchMem.m_scratch); + break; + } + + if (cmd <= 3) break; + } // for (;;) + } // else + + if (s_error) + { + break; + } + } // for (int i=adj_nub; i<n; ++i) + lcp.unpermute(); - return !s_error; + return !s_error; } - diff --git a/extern/bullet2/src/BulletDynamics/MLCPSolvers/btDantzigLCP.h b/extern/bullet2/src/BulletDynamics/MLCPSolvers/btDantzigLCP.h index 903832770ae..8d9b2a13e99 100644 --- a/extern/bullet2/src/BulletDynamics/MLCPSolvers/btDantzigLCP.h +++ b/extern/bullet2/src/BulletDynamics/MLCPSolvers/btDantzigLCP.h @@ -41,7 +41,6 @@ to be implemented. the first `nub' variables are assumed to have findex < 0. */ - #ifndef _BT_LCP_H_ #define _BT_LCP_H_ @@ -49,7 +48,6 @@ to be implemented. the first `nub' variables are assumed to have findex < 0. #include <stdio.h> #include <assert.h> - #include "LinearMath/btScalar.h" #include "LinearMath/btAlignedObjectArray.h" @@ -62,16 +60,14 @@ struct btDantzigScratchMemory btAlignedObjectArray<btScalar> delta_x; btAlignedObjectArray<btScalar> Dell; btAlignedObjectArray<btScalar> ell; - btAlignedObjectArray<btScalar*> Arows; + btAlignedObjectArray<btScalar *> Arows; btAlignedObjectArray<int> p; btAlignedObjectArray<int> C; btAlignedObjectArray<bool> state; }; //return false if solving failed -bool btSolveDantzigLCP (int n, btScalar *A, btScalar *x, btScalar *b, btScalar *w, - int nub, btScalar *lo, btScalar *hi, int *findex,btDantzigScratchMemory& scratch); - - +bool btSolveDantzigLCP(int n, btScalar *A, btScalar *x, btScalar *b, btScalar *w, + int nub, btScalar *lo, btScalar *hi, int *findex, btDantzigScratchMemory &scratch); -#endif //_BT_LCP_H_ +#endif //_BT_LCP_H_ diff --git a/extern/bullet2/src/BulletDynamics/MLCPSolvers/btDantzigSolver.h b/extern/bullet2/src/BulletDynamics/MLCPSolvers/btDantzigSolver.h index 2a2f2d3d32d..1f669751ce7 100644 --- a/extern/bullet2/src/BulletDynamics/MLCPSolvers/btDantzigSolver.h +++ b/extern/bullet2/src/BulletDynamics/MLCPSolvers/btDantzigSolver.h @@ -20,30 +20,28 @@ subject to the following restrictions: #include "btMLCPSolverInterface.h" #include "btDantzigLCP.h" - class btDantzigSolver : public btMLCPSolverInterface { protected: - btScalar m_acceptableUpperLimitSolution; - btAlignedObjectArray<char> m_tempBuffer; + btAlignedObjectArray<char> m_tempBuffer; btAlignedObjectArray<btScalar> m_A; btAlignedObjectArray<btScalar> m_b; btAlignedObjectArray<btScalar> m_x; btAlignedObjectArray<btScalar> m_lo; btAlignedObjectArray<btScalar> m_hi; - btAlignedObjectArray<int> m_dependencies; + btAlignedObjectArray<int> m_dependencies; btDantzigScratchMemory m_scratchMemory; -public: +public: btDantzigSolver() - :m_acceptableUpperLimitSolution(btScalar(1000)) + : m_acceptableUpperLimitSolution(btScalar(1000)) { } - virtual bool solveMLCP(const btMatrixXu & A, const btVectorXu & b, btVectorXu& x, const btVectorXu & lo,const btVectorXu & hi,const btAlignedObjectArray<int>& limitDependency, int numIterations, bool useSparsity = true) + virtual bool solveMLCP(const btMatrixXu& A, const btVectorXu& b, btVectorXu& x, const btVectorXu& lo, const btVectorXu& hi, const btAlignedObjectArray<int>& limitDependency, int numIterations, bool useSparsity = true) { bool result = true; int n = b.rows(); @@ -52,14 +50,12 @@ public: int nub = 0; btAlignedObjectArray<btScalar> ww; ww.resize(n); - const btScalar* Aptr = A.getBufferPointer(); - m_A.resize(n*n); - for (int i=0;i<n*n;i++) + m_A.resize(n * n); + for (int i = 0; i < n * n; i++) { m_A[i] = Aptr[i]; - } m_b.resize(n); @@ -67,7 +63,7 @@ public: m_lo.resize(n); m_hi.resize(n); m_dependencies.resize(n); - for (int i=0;i<n;i++) + for (int i = 0; i < n; i++) { m_lo[i] = lo[i]; m_hi[i] = hi[i]; @@ -76,13 +72,12 @@ public: m_dependencies[i] = limitDependency[i]; } - - result = btSolveDantzigLCP (n,&m_A[0],&m_x[0],&m_b[0],&ww[0],nub,&m_lo[0],&m_hi[0],&m_dependencies[0],m_scratchMemory); + result = btSolveDantzigLCP(n, &m_A[0], &m_x[0], &m_b[0], &ww[0], nub, &m_lo[0], &m_hi[0], &m_dependencies[0], m_scratchMemory); if (!result) return result; -// printf("numAllocas = %d\n",numAllocas); - for (int i=0;i<n;i++) + // printf("numAllocas = %d\n",numAllocas); + for (int i = 0; i < n; i++) { volatile btScalar xx = m_x[i]; if (xx != m_x[i]) @@ -98,15 +93,14 @@ public: } } - for (int i=0;i<n;i++) + for (int i = 0; i < n; i++) { x[i] = m_x[i]; } - } return result; } }; -#endif //BT_DANTZIG_SOLVER_H +#endif //BT_DANTZIG_SOLVER_H diff --git a/extern/bullet2/src/BulletDynamics/MLCPSolvers/btLemkeAlgorithm.cpp b/extern/bullet2/src/BulletDynamics/MLCPSolvers/btLemkeAlgorithm.cpp index 1f4015c7c72..954ffaed759 100644 --- a/extern/bullet2/src/BulletDynamics/MLCPSolvers/btLemkeAlgorithm.cpp +++ b/extern/bullet2/src/BulletDynamics/MLCPSolvers/btLemkeAlgorithm.cpp @@ -19,64 +19,60 @@ subject to the following restrictions: //Math library was replaced from fmatvec to a the file src/LinearMath/btMatrixX.h //STL/std::vector replaced by btAlignedObjectArray - - #include "btLemkeAlgorithm.h" #undef BT_DEBUG_OSTREAM #ifdef BT_DEBUG_OSTREAM using namespace std; -#endif //BT_DEBUG_OSTREAM +#endif //BT_DEBUG_OSTREAM btScalar btMachEps() { - static bool calculated=false; + static bool calculated = false; static btScalar machEps = btScalar(1.); if (!calculated) { - do { + do + { machEps /= btScalar(2.0); // If next epsilon yields 1, then break, because current // epsilon is the machine epsilon. - } - while ((btScalar)(1.0 + (machEps/btScalar(2.0))) != btScalar(1.0)); -// printf( "\nCalculated Machine epsilon: %G\n", machEps ); - calculated=true; + } while ((btScalar)(1.0 + (machEps / btScalar(2.0))) != btScalar(1.0)); + // printf( "\nCalculated Machine epsilon: %G\n", machEps ); + calculated = true; } return machEps; } -btScalar btEpsRoot() { - +btScalar btEpsRoot() +{ static btScalar epsroot = 0.; static bool alreadyCalculated = false; - - if (!alreadyCalculated) { + + if (!alreadyCalculated) + { epsroot = btSqrt(btMachEps()); alreadyCalculated = true; } return epsroot; } - - - btVectorXu btLemkeAlgorithm::solve(unsigned int maxloops /* = 0*/) +btVectorXu btLemkeAlgorithm::solve(unsigned int maxloops /* = 0*/) { - - - steps = 0; + steps = 0; - int dim = m_q.size(); + int dim = m_q.size(); #ifdef BT_DEBUG_OSTREAM - if(DEBUGLEVEL >= 1) { - cout << "Dimension = " << dim << endl; - } -#endif //BT_DEBUG_OSTREAM + if (DEBUGLEVEL >= 1) + { + cout << "Dimension = " << dim << endl; + } +#endif //BT_DEBUG_OSTREAM btVectorXu solutionVector(2 * dim); solutionVector.setZero(); - - //, INIT, 0.); + + //, INIT, 0.); btMatrixXu ident(dim, dim); ident.setIdentity(); @@ -85,287 +81,289 @@ btScalar btEpsRoot() { #endif btMatrixXu mNeg = m_M.negative(); - - btMatrixXu A(dim, 2 * dim + 2); + + btMatrixXu A(dim, 2 * dim + 2); // - A.setSubMatrix(0, 0, dim - 1, dim - 1,ident); - A.setSubMatrix(0, dim, dim - 1, 2 * dim - 1,mNeg); + A.setSubMatrix(0, 0, dim - 1, dim - 1, ident); + A.setSubMatrix(0, dim, dim - 1, 2 * dim - 1, mNeg); A.setSubMatrix(0, 2 * dim, dim - 1, 2 * dim, -1.f); - A.setSubMatrix(0, 2 * dim + 1, dim - 1, 2 * dim + 1,m_q); + A.setSubMatrix(0, 2 * dim + 1, dim - 1, 2 * dim + 1, m_q); #ifdef BT_DEBUG_OSTREAM cout << A << std::endl; -#endif //BT_DEBUG_OSTREAM +#endif //BT_DEBUG_OSTREAM + // btVectorXu q_; + // q_ >> A(0, 2 * dim + 1, dim - 1, 2 * dim + 1); - // btVectorXu q_; - // q_ >> A(0, 2 * dim + 1, dim - 1, 2 * dim + 1); - - btAlignedObjectArray<int> basis; - //At first, all w-values are in the basis - for (int i = 0; i < dim; i++) - basis.push_back(i); + btAlignedObjectArray<int> basis; + //At first, all w-values are in the basis + for (int i = 0; i < dim; i++) + basis.push_back(i); int pivotRowIndex = -1; btScalar minValue = 1e30f; bool greaterZero = true; - for (int i=0;i<dim;i++) + for (int i = 0; i < dim; i++) { - btScalar v =A(i,2*dim+1); - if (v<minValue) + btScalar v = A(i, 2 * dim + 1); + if (v < minValue) { - minValue=v; + minValue = v; pivotRowIndex = i; } - if (v<0) + if (v < 0) greaterZero = false; } - - - // int pivotRowIndex = q_.minIndex();//minIndex(q_); // first row is that with lowest q-value - int z0Row = pivotRowIndex; // remember the col of z0 for ending algorithm afterwards - int pivotColIndex = 2 * dim; // first col is that of z0 + // int pivotRowIndex = q_.minIndex();//minIndex(q_); // first row is that with lowest q-value + int z0Row = pivotRowIndex; // remember the col of z0 for ending algorithm afterwards + int pivotColIndex = 2 * dim; // first col is that of z0 #ifdef BT_DEBUG_OSTREAM - if (DEBUGLEVEL >= 3) + if (DEBUGLEVEL >= 3) { - // cout << "A: " << A << endl; - cout << "pivotRowIndex " << pivotRowIndex << endl; - cout << "pivotColIndex " << pivotColIndex << endl; - cout << "Basis: "; - for (int i = 0; i < basis.size(); i++) - cout << basis[i] << " "; - cout << endl; - } -#endif //BT_DEBUG_OSTREAM + // cout << "A: " << A << endl; + cout << "pivotRowIndex " << pivotRowIndex << endl; + cout << "pivotColIndex " << pivotColIndex << endl; + cout << "Basis: "; + for (int i = 0; i < basis.size(); i++) + cout << basis[i] << " "; + cout << endl; + } +#endif //BT_DEBUG_OSTREAM if (!greaterZero) { + if (maxloops == 0) + { + maxloops = 100; + // maxloops = UINT_MAX; //TODO: not a really nice way, problem is: maxloops should be 2^dim (=1<<dim), but this could exceed UINT_MAX and thus the result would be 0 and therefore the lemke algorithm wouldn't start but probably would find a solution within less then UINT_MAX steps. Therefore this constant is used as a upper border right now... + } - if (maxloops == 0) { - maxloops = 100; -// maxloops = UINT_MAX; //TODO: not a really nice way, problem is: maxloops should be 2^dim (=1<<dim), but this could exceed UINT_MAX and thus the result would be 0 and therefore the lemke algorithm wouldn't start but probably would find a solution within less then UINT_MAX steps. Therefore this constant is used as a upper border right now... - } - - /*start looping*/ - for(steps = 0; steps < maxloops; steps++) { - - GaussJordanEliminationStep(A, pivotRowIndex, pivotColIndex, basis); -#ifdef BT_DEBUG_OSTREAM - if (DEBUGLEVEL >= 3) { - // cout << "A: " << A << endl; - cout << "pivotRowIndex " << pivotRowIndex << endl; - cout << "pivotColIndex " << pivotColIndex << endl; - cout << "Basis: "; - for (int i = 0; i < basis.size(); i++) - cout << basis[i] << " "; - cout << endl; - } -#endif //BT_DEBUG_OSTREAM - - int pivotColIndexOld = pivotColIndex; - - /*find new column index */ - if (basis[pivotRowIndex] < dim) //if a w-value left the basis get in the correspondent z-value - pivotColIndex = basis[pivotRowIndex] + dim; - else - //else do it the other way round and get in the corresponding w-value - pivotColIndex = basis[pivotRowIndex] - dim; - - /*the column becomes part of the basis*/ - basis[pivotRowIndex] = pivotColIndexOld; - - pivotRowIndex = findLexicographicMinimum(A, pivotColIndex); - - if(z0Row == pivotRowIndex) { //if z0 leaves the basis the solution is found --> one last elimination step is necessary - GaussJordanEliminationStep(A, pivotRowIndex, pivotColIndex, basis); - basis[pivotRowIndex] = pivotColIndex; //update basis - break; - } - - } + /*start looping*/ + for (steps = 0; steps < maxloops; steps++) + { + GaussJordanEliminationStep(A, pivotRowIndex, pivotColIndex, basis); #ifdef BT_DEBUG_OSTREAM - if(DEBUGLEVEL >= 1) { - cout << "Number of loops: " << steps << endl; - cout << "Number of maximal loops: " << maxloops << endl; - } -#endif //BT_DEBUG_OSTREAM - - if(!validBasis(basis)) { - info = -1; + if (DEBUGLEVEL >= 3) + { + // cout << "A: " << A << endl; + cout << "pivotRowIndex " << pivotRowIndex << endl; + cout << "pivotColIndex " << pivotColIndex << endl; + cout << "Basis: "; + for (int i = 0; i < basis.size(); i++) + cout << basis[i] << " "; + cout << endl; + } +#endif //BT_DEBUG_OSTREAM + + int pivotColIndexOld = pivotColIndex; + + /*find new column index */ + if (basis[pivotRowIndex] < dim) //if a w-value left the basis get in the correspondent z-value + pivotColIndex = basis[pivotRowIndex] + dim; + else + //else do it the other way round and get in the corresponding w-value + pivotColIndex = basis[pivotRowIndex] - dim; + + /*the column becomes part of the basis*/ + basis[pivotRowIndex] = pivotColIndexOld; + + pivotRowIndex = findLexicographicMinimum(A, pivotColIndex); + + if (z0Row == pivotRowIndex) + { //if z0 leaves the basis the solution is found --> one last elimination step is necessary + GaussJordanEliminationStep(A, pivotRowIndex, pivotColIndex, basis); + basis[pivotRowIndex] = pivotColIndex; //update basis + break; + } + } #ifdef BT_DEBUG_OSTREAM - if(DEBUGLEVEL >= 1) - cerr << "Lemke-Algorithm ended with Ray-Termination (no valid solution)." << endl; -#endif //BT_DEBUG_OSTREAM + if (DEBUGLEVEL >= 1) + { + cout << "Number of loops: " << steps << endl; + cout << "Number of maximal loops: " << maxloops << endl; + } +#endif //BT_DEBUG_OSTREAM - return solutionVector; - } + if (!validBasis(basis)) + { + info = -1; +#ifdef BT_DEBUG_OSTREAM + if (DEBUGLEVEL >= 1) + cerr << "Lemke-Algorithm ended with Ray-Termination (no valid solution)." << endl; +#endif //BT_DEBUG_OSTREAM - } + return solutionVector; + } + } #ifdef BT_DEBUG_OSTREAM - if (DEBUGLEVEL >= 2) { - // cout << "A: " << A << endl; - cout << "pivotRowIndex " << pivotRowIndex << endl; - cout << "pivotColIndex " << pivotColIndex << endl; - } -#endif //BT_DEBUG_OSTREAM - - for (int i = 0; i < basis.size(); i++) + if (DEBUGLEVEL >= 2) { - solutionVector[basis[i]] = A(i,2*dim+1);//q_[i]; + // cout << "A: " << A << endl; + cout << "pivotRowIndex " << pivotRowIndex << endl; + cout << "pivotColIndex " << pivotColIndex << endl; } +#endif //BT_DEBUG_OSTREAM - info = 0; + for (int i = 0; i < basis.size(); i++) + { + solutionVector[basis[i]] = A(i, 2 * dim + 1); //q_[i]; + } - return solutionVector; - } + info = 0; - int btLemkeAlgorithm::findLexicographicMinimum(const btMatrixXu& A, const int & pivotColIndex) { - int RowIndex = 0; - int dim = A.rows(); - btAlignedObjectArray<btVectorXu> Rows; - for (int row = 0; row < dim; row++) - { + return solutionVector; +} - btVectorXu vec(dim + 1); - vec.setZero();//, INIT, 0.) - Rows.push_back(vec); - btScalar a = A(row, pivotColIndex); - if (a > 0) { - Rows[row][0] = A(row, 2 * dim + 1) / a; - Rows[row][1] = A(row, 2 * dim) / a; - for (int j = 2; j < dim + 1; j++) - Rows[row][j] = A(row, j - 1) / a; +int btLemkeAlgorithm::findLexicographicMinimum(const btMatrixXu& A, const int& pivotColIndex) +{ + int RowIndex = 0; + int dim = A.rows(); + btAlignedObjectArray<btVectorXu> Rows; + for (int row = 0; row < dim; row++) + { + btVectorXu vec(dim + 1); + vec.setZero(); //, INIT, 0.) + Rows.push_back(vec); + btScalar a = A(row, pivotColIndex); + if (a > 0) + { + Rows[row][0] = A(row, 2 * dim + 1) / a; + Rows[row][1] = A(row, 2 * dim) / a; + for (int j = 2; j < dim + 1; j++) + Rows[row][j] = A(row, j - 1) / a; #ifdef BT_DEBUG_OSTREAM - // if (DEBUGLEVEL) { - // cout << "Rows(" << row << ") = " << Rows[row] << endl; + // if (DEBUGLEVEL) { + // cout << "Rows(" << row << ") = " << Rows[row] << endl; // } -#endif - } - } - - for (int i = 0; i < Rows.size(); i++) - { - if (Rows[i].nrm2() > 0.) { - - int j = 0; - for (; j < Rows.size(); j++) - { - if(i != j) - { - if(Rows[j].nrm2() > 0.) - { - btVectorXu test(dim + 1); - for (int ii=0;ii<dim+1;ii++) - { - test[ii] = Rows[j][ii] - Rows[i][ii]; - } - - //=Rows[j] - Rows[i] - if (! LexicographicPositive(test)) - break; - } - } - } - - if (j == Rows.size()) - { - RowIndex += i; - break; - } - } - } - - return RowIndex; - } - - bool btLemkeAlgorithm::LexicographicPositive(const btVectorXu & v) -{ - int i = 0; - // if (DEBUGLEVEL) - // cout << "v " << v << endl; +#endif + } + } - while(i < v.size()-1 && fabs(v[i]) < btMachEps()) - i++; - if (v[i] > 0) - return true; + for (int i = 0; i < Rows.size(); i++) + { + if (Rows[i].nrm2() > 0.) + { + int j = 0; + for (; j < Rows.size(); j++) + { + if (i != j) + { + if (Rows[j].nrm2() > 0.) + { + btVectorXu test(dim + 1); + for (int ii = 0; ii < dim + 1; ii++) + { + test[ii] = Rows[j][ii] - Rows[i][ii]; + } + + //=Rows[j] - Rows[i] + if (!LexicographicPositive(test)) + break; + } + } + } + + if (j == Rows.size()) + { + RowIndex += i; + break; + } + } + } - return false; - } + return RowIndex; +} -void btLemkeAlgorithm::GaussJordanEliminationStep(btMatrixXu& A, int pivotRowIndex, int pivotColumnIndex, const btAlignedObjectArray<int>& basis) +bool btLemkeAlgorithm::LexicographicPositive(const btVectorXu& v) { + int i = 0; + // if (DEBUGLEVEL) + // cout << "v " << v << endl; + + while (i < v.size() - 1 && fabs(v[i]) < btMachEps()) + i++; + if (v[i] > 0) + return true; + return false; +} + +void btLemkeAlgorithm::GaussJordanEliminationStep(btMatrixXu& A, int pivotRowIndex, int pivotColumnIndex, const btAlignedObjectArray<int>& basis) +{ btScalar a = -1 / A(pivotRowIndex, pivotColumnIndex); #ifdef BT_DEBUG_OSTREAM cout << A << std::endl; #endif - for (int i = 0; i < A.rows(); i++) + for (int i = 0; i < A.rows(); i++) { - if (i != pivotRowIndex) - { - for (int j = 0; j < A.cols(); j++) + if (i != pivotRowIndex) { - if (j != pivotColumnIndex) - { - btScalar v = A(i, j); - v += A(pivotRowIndex, j) * A(i, pivotColumnIndex) * a; - A.setElem(i, j, v); - } + for (int j = 0; j < A.cols(); j++) + { + if (j != pivotColumnIndex) + { + btScalar v = A(i, j); + v += A(pivotRowIndex, j) * A(i, pivotColumnIndex) * a; + A.setElem(i, j, v); + } + } } - } } #ifdef BT_DEBUG_OSTREAM cout << A << std::endl; -#endif //BT_DEBUG_OSTREAM - for (int i = 0; i < A.cols(); i++) +#endif //BT_DEBUG_OSTREAM + for (int i = 0; i < A.cols(); i++) { - A.mulElem(pivotRowIndex, i,-a); - } + A.mulElem(pivotRowIndex, i, -a); + } #ifdef BT_DEBUG_OSTREAM cout << A << std::endl; -#endif //#ifdef BT_DEBUG_OSTREAM +#endif //#ifdef BT_DEBUG_OSTREAM - for (int i = 0; i < A.rows(); i++) + for (int i = 0; i < A.rows(); i++) { - if (i != pivotRowIndex) - { - A.setElem(i, pivotColumnIndex,0); - } + if (i != pivotRowIndex) + { + A.setElem(i, pivotColumnIndex, 0); + } } #ifdef BT_DEBUG_OSTREAM cout << A << std::endl; -#endif //#ifdef BT_DEBUG_OSTREAM - } +#endif //#ifdef BT_DEBUG_OSTREAM +} - bool btLemkeAlgorithm::greaterZero(const btVectorXu & vector) +bool btLemkeAlgorithm::greaterZero(const btVectorXu& vector) { - bool isGreater = true; - for (int i = 0; i < vector.size(); i++) { - if (vector[i] < 0) { - isGreater = false; - break; - } - } - - return isGreater; - } - - bool btLemkeAlgorithm::validBasis(const btAlignedObjectArray<int>& basis) - { - bool isValid = true; - for (int i = 0; i < basis.size(); i++) { - if (basis[i] >= basis.size() * 2) { //then z0 is in the base - isValid = false; - break; - } - } - - return isValid; - } + bool isGreater = true; + for (int i = 0; i < vector.size(); i++) + { + if (vector[i] < 0) + { + isGreater = false; + break; + } + } + return isGreater; +} +bool btLemkeAlgorithm::validBasis(const btAlignedObjectArray<int>& basis) +{ + bool isValid = true; + for (int i = 0; i < basis.size(); i++) + { + if (basis[i] >= basis.size() * 2) + { //then z0 is in the base + isValid = false; + break; + } + } + + return isValid; +} diff --git a/extern/bullet2/src/BulletDynamics/MLCPSolvers/btLemkeAlgorithm.h b/extern/bullet2/src/BulletDynamics/MLCPSolvers/btLemkeAlgorithm.h index 7555cd9d207..3c6bf72a233 100644 --- a/extern/bullet2/src/BulletDynamics/MLCPSolvers/btLemkeAlgorithm.h +++ b/extern/bullet2/src/BulletDynamics/MLCPSolvers/btLemkeAlgorithm.h @@ -19,90 +19,84 @@ subject to the following restrictions: //Math library was replaced from fmatvec to a the file src/LinearMath/btMatrixX.h //STL/std::vector replaced by btAlignedObjectArray - - #ifndef BT_NUMERICS_LEMKE_ALGORITHM_H_ #define BT_NUMERICS_LEMKE_ALGORITHM_H_ #include "LinearMath/btMatrixX.h" - -#include <vector> //todo: replace by btAlignedObjectArray +#include <vector> //todo: replace by btAlignedObjectArray class btLemkeAlgorithm { public: - - - btLemkeAlgorithm(const btMatrixXu& M_, const btVectorXu& q_, const int & DEBUGLEVEL_ = 0) : - DEBUGLEVEL(DEBUGLEVEL_) - { - setSystem(M_, q_); - } + btLemkeAlgorithm(const btMatrixXu& M_, const btVectorXu& q_, const int& DEBUGLEVEL_ = 0) : DEBUGLEVEL(DEBUGLEVEL_) + { + setSystem(M_, q_); + } - /* GETTER / SETTER */ - /** + /* GETTER / SETTER */ + /** * \brief return info of solution process */ - int getInfo() { - return info; - } + int getInfo() + { + return info; + } - /** + /** * \brief get the number of steps until the solution was found */ - int getSteps(void) { - return steps; - } - - + int getSteps(void) + { + return steps; + } - /** + /** * \brief set system with Matrix M and vector q */ - void setSystem(const btMatrixXu & M_, const btVectorXu & q_) + void setSystem(const btMatrixXu& M_, const btVectorXu& q_) { m_M = M_; m_q = q_; - } - /***************************************************/ + } + /***************************************************/ - /** + /** * \brief solve algorithm adapted from : Fast Implementation of Lemke’s Algorithm for Rigid Body Contact Simulation (John E. Lloyd) */ - btVectorXu solve(unsigned int maxloops = 0); + btVectorXu solve(unsigned int maxloops = 0); - virtual ~btLemkeAlgorithm() { - } + virtual ~btLemkeAlgorithm() + { + } protected: - int findLexicographicMinimum(const btMatrixXu &A, const int & pivotColIndex); - bool LexicographicPositive(const btVectorXu & v); - void GaussJordanEliminationStep(btMatrixXu &A, int pivotRowIndex, int pivotColumnIndex, const btAlignedObjectArray<int>& basis); - bool greaterZero(const btVectorXu & vector); - bool validBasis(const btAlignedObjectArray<int>& basis); + int findLexicographicMinimum(const btMatrixXu& A, const int& pivotColIndex); + bool LexicographicPositive(const btVectorXu& v); + void GaussJordanEliminationStep(btMatrixXu& A, int pivotRowIndex, int pivotColumnIndex, const btAlignedObjectArray<int>& basis); + bool greaterZero(const btVectorXu& vector); + bool validBasis(const btAlignedObjectArray<int>& basis); - btMatrixXu m_M; - btVectorXu m_q; + btMatrixXu m_M; + btVectorXu m_q; - /** + /** * \brief number of steps until the Lemke algorithm found a solution */ - unsigned int steps; + unsigned int steps; - /** + /** * \brief define level of debug output */ - int DEBUGLEVEL; + int DEBUGLEVEL; - /** + /** * \brief did the algorithm find a solution * * -1 : not successful * 0 : successful */ - int info; + int info; }; - #endif /* BT_NUMERICS_LEMKE_ALGORITHM_H_ */ diff --git a/extern/bullet2/src/BulletDynamics/MLCPSolvers/btLemkeSolver.h b/extern/bullet2/src/BulletDynamics/MLCPSolvers/btLemkeSolver.h index 98484c37964..f18c4ea41b0 100644 --- a/extern/bullet2/src/BulletDynamics/MLCPSolvers/btLemkeSolver.h +++ b/extern/bullet2/src/BulletDynamics/MLCPSolvers/btLemkeSolver.h @@ -17,334 +17,322 @@ subject to the following restrictions: #ifndef BT_LEMKE_SOLVER_H #define BT_LEMKE_SOLVER_H - #include "btMLCPSolverInterface.h" #include "btLemkeAlgorithm.h" - - - -///The btLemkeSolver is based on "Fast Implementation of Lemke’s Algorithm for Rigid Body Contact Simulation (John E. Lloyd) " +///The btLemkeSolver is based on "Fast Implementation of Lemke’s Algorithm for Rigid Body Contact Simulation (John E. Lloyd) " ///It is a slower but more accurate solver. Increase the m_maxLoops for better convergence, at the cost of more CPU time. ///The original implementation of the btLemkeAlgorithm was done by Kilian Grundl from the MBSim team class btLemkeSolver : public btMLCPSolverInterface { protected: - public: - - btScalar m_maxValue; - int m_debugLevel; - int m_maxLoops; - bool m_useLoHighBounds; - - + btScalar m_maxValue; + int m_debugLevel; + int m_maxLoops; + bool m_useLoHighBounds; btLemkeSolver() - :m_maxValue(100000), - m_debugLevel(0), - m_maxLoops(1000), - m_useLoHighBounds(true) + : m_maxValue(100000), + m_debugLevel(0), + m_maxLoops(1000), + m_useLoHighBounds(true) { } - virtual bool solveMLCP(const btMatrixXu & A, const btVectorXu & b, btVectorXu& x, const btVectorXu & lo,const btVectorXu & hi,const btAlignedObjectArray<int>& limitDependency, int numIterations, bool useSparsity = true) + virtual bool solveMLCP(const btMatrixXu& A, const btVectorXu& b, btVectorXu& x, const btVectorXu& lo, const btVectorXu& hi, const btAlignedObjectArray<int>& limitDependency, int numIterations, bool useSparsity = true) { - if (m_useLoHighBounds) { + BT_PROFILE("btLemkeSolver::solveMLCP"); + int n = A.rows(); + if (0 == n) + return true; - BT_PROFILE("btLemkeSolver::solveMLCP"); - int n = A.rows(); - if (0==n) - return true; - - bool fail = false; - - btVectorXu solution(n); - btVectorXu q1; - q1.resize(n); - for (int row=0;row<n;row++) - { - q1[row] = -b[row]; - } + bool fail = false; + + btVectorXu solution(n); + btVectorXu q1; + q1.resize(n); + for (int row = 0; row < n; row++) + { + q1[row] = -b[row]; + } - // cout << "A" << endl; - // cout << A << endl; + // cout << "A" << endl; + // cout << A << endl; ///////////////////////////////////// //slow matrix inversion, replace with LU decomposition btMatrixXu A1; - btMatrixXu B(n,n); + btMatrixXu B(n, n); { - BT_PROFILE("inverse(slow)"); - A1.resize(A.rows(),A.cols()); - for (int row=0;row<A.rows();row++) + //BT_PROFILE("inverse(slow)"); + A1.resize(A.rows(), A.cols()); + for (int row = 0; row < A.rows(); row++) { - for (int col=0;col<A.cols();col++) + for (int col = 0; col < A.cols(); col++) { - A1.setElem(row,col,A(row,col)); + A1.setElem(row, col, A(row, col)); } } btMatrixXu matrix; - matrix.resize(n,2*n); - for (int row=0;row<n;row++) + matrix.resize(n, 2 * n); + for (int row = 0; row < n; row++) { - for (int col=0;col<n;col++) + for (int col = 0; col < n; col++) { - matrix.setElem(row,col,A1(row,col)); + matrix.setElem(row, col, A1(row, col)); } } - - btScalar ratio,a; - int i,j,k; - for(i = 0; i < n; i++){ - for(j = n; j < 2*n; j++){ - if(i==(j-n)) - matrix.setElem(i,j,1.0); - else - matrix.setElem(i,j,0.0); + btScalar ratio, a; + int i, j, k; + for (i = 0; i < n; i++) + { + for (j = n; j < 2 * n; j++) + { + if (i == (j - n)) + matrix.setElem(i, j, 1.0); + else + matrix.setElem(i, j, 0.0); + } } - } - for(i = 0; i < n; i++){ - for(j = 0; j < n; j++){ - if(i!=j) + for (i = 0; i < n; i++) + { + for (j = 0; j < n; j++) { - btScalar v = matrix(i,i); - if (btFuzzyZero(v)) + if (i != j) { - a = 0.000001f; - } - ratio = matrix(j,i)/matrix(i,i); - for(k = 0; k < 2*n; k++){ - matrix.addElem(j,k,- ratio * matrix(i,k)); + btScalar v = matrix(i, i); + if (btFuzzyZero(v)) + { + a = 0.000001f; + } + ratio = matrix(j, i) / matrix(i, i); + for (k = 0; k < 2 * n; k++) + { + matrix.addElem(j, k, -ratio * matrix(i, k)); + } } } } - } - for(i = 0; i < n; i++){ - a = matrix(i,i); - if (btFuzzyZero(a)) + for (i = 0; i < n; i++) { - a = 0.000001f; - } - btScalar invA = 1.f/a; - for(j = 0; j < 2*n; j++){ - matrix.mulElem(i,j,invA); + a = matrix(i, i); + if (btFuzzyZero(a)) + { + a = 0.000001f; + } + btScalar invA = 1.f / a; + for (j = 0; j < 2 * n; j++) + { + matrix.mulElem(i, j, invA); + } } - } - - - - - for (int row=0;row<n;row++) + for (int row = 0; row < n; row++) { - for (int col=0;col<n;col++) + for (int col = 0; col < n; col++) { - B.setElem(row,col,matrix(row,n+col)); + B.setElem(row, col, matrix(row, n + col)); } } } - btMatrixXu b1(n,1); + btMatrixXu b1(n, 1); - btMatrixXu M(n*2,n*2); - for (int row=0;row<n;row++) - { - b1.setElem(row,0,-b[row]); - for (int col=0;col<n;col++) + btMatrixXu M(n * 2, n * 2); + for (int row = 0; row < n; row++) { - btScalar v =B(row,col); - M.setElem(row,col,v); - M.setElem(n+row,n+col,v); - M.setElem(n+row,col,-v); - M.setElem(row,n+col,-v); - + b1.setElem(row, 0, -b[row]); + for (int col = 0; col < n; col++) + { + btScalar v = B(row, col); + M.setElem(row, col, v); + M.setElem(n + row, n + col, v); + M.setElem(n + row, col, -v); + M.setElem(row, n + col, -v); + } } - } - btMatrixXu Bb1 = B*b1; -// q = [ (-B*b1 - lo)' (hi + B*b1)' ]' + btMatrixXu Bb1 = B * b1; + // q = [ (-B*b1 - lo)' (hi + B*b1)' ]' - btVectorXu qq; - qq.resize(n*2); - for (int row=0;row<n;row++) - { - qq[row] = -Bb1(row,0)-lo[row]; - qq[n+row] = Bb1(row,0)+hi[row]; - } + btVectorXu qq; + qq.resize(n * 2); + for (int row = 0; row < n; row++) + { + qq[row] = -Bb1(row, 0) - lo[row]; + qq[n + row] = Bb1(row, 0) + hi[row]; + } - btVectorXu z1; + btVectorXu z1; - btMatrixXu y1; - y1.resize(n,1); - btLemkeAlgorithm lemke(M,qq,m_debugLevel); - { - BT_PROFILE("lemke.solve"); - lemke.setSystem(M,qq); - z1 = lemke.solve(m_maxLoops); - } - for (int row=0;row<n;row++) - { - y1.setElem(row,0,z1[2*n+row]-z1[3*n+row]); - } - btMatrixXu y1_b1(n,1); - for (int i=0;i<n;i++) - { - y1_b1.setElem(i,0,y1(i,0)-b1(i,0)); - } + btMatrixXu y1; + y1.resize(n, 1); + btLemkeAlgorithm lemke(M, qq, m_debugLevel); + { + //BT_PROFILE("lemke.solve"); + lemke.setSystem(M, qq); + z1 = lemke.solve(m_maxLoops); + } + for (int row = 0; row < n; row++) + { + y1.setElem(row, 0, z1[2 * n + row] - z1[3 * n + row]); + } + btMatrixXu y1_b1(n, 1); + for (int i = 0; i < n; i++) + { + y1_b1.setElem(i, 0, y1(i, 0) - b1(i, 0)); + } - btMatrixXu x1; + btMatrixXu x1; - x1 = B*(y1_b1); - - for (int row=0;row<n;row++) - { - solution[row] = x1(row,0);//n]; - } + x1 = B * (y1_b1); - int errorIndexMax = -1; - int errorIndexMin = -1; - float errorValueMax = -1e30; - float errorValueMin = 1e30; - - for (int i=0;i<n;i++) - { - x[i] = solution[i]; - volatile btScalar check = x[i]; - if (x[i] != check) + for (int row = 0; row < n; row++) { - //printf("Lemke result is #NAN\n"); - x.setZero(); - return false; + solution[row] = x1(row, 0); //n]; } - - //this is some hack/safety mechanism, to discard invalid solutions from the Lemke solver - //we need to figure out why it happens, and fix it, or detect it properly) - if (x[i]>m_maxValue) + + int errorIndexMax = -1; + int errorIndexMin = -1; + float errorValueMax = -1e30; + float errorValueMin = 1e30; + + for (int i = 0; i < n; i++) { - if (x[i]> errorValueMax) + x[i] = solution[i]; + volatile btScalar check = x[i]; + if (x[i] != check) + { + //printf("Lemke result is #NAN\n"); + x.setZero(); + return false; + } + + //this is some hack/safety mechanism, to discard invalid solutions from the Lemke solver + //we need to figure out why it happens, and fix it, or detect it properly) + if (x[i] > m_maxValue) + { + if (x[i] > errorValueMax) + { + fail = true; + errorIndexMax = i; + errorValueMax = x[i]; + } + ////printf("x[i] = %f,",x[i]); + } + if (x[i] < -m_maxValue) { - fail = true; - errorIndexMax = i; - errorValueMax = x[i]; + if (x[i] < errorValueMin) + { + errorIndexMin = i; + errorValueMin = x[i]; + fail = true; + //printf("x[i] = %f,",x[i]); + } } - ////printf("x[i] = %f,",x[i]); } - if (x[i]<-m_maxValue) + if (fail) { - if (x[i]<errorValueMin) + int m_errorCountTimes = 0; + if (errorIndexMin < 0) + errorValueMin = 0.f; + if (errorIndexMax < 0) + errorValueMax = 0.f; + m_errorCountTimes++; + // printf("Error (x[%d] = %f, x[%d] = %f), resetting %d times\n", errorIndexMin,errorValueMin, errorIndexMax, errorValueMax, errorCountTimes++); + for (int i = 0; i < n; i++) { - errorIndexMin = i; - errorValueMin = x[i]; - fail = true; - //printf("x[i] = %f,",x[i]); + x[i] = 0.f; } } + return !fail; } - if (fail) + else + { - int m_errorCountTimes = 0; - if (errorIndexMin<0) - errorValueMin = 0.f; - if (errorIndexMax<0) - errorValueMax = 0.f; - m_errorCountTimes++; - // printf("Error (x[%d] = %f, x[%d] = %f), resetting %d times\n", errorIndexMin,errorValueMin, errorIndexMax, errorValueMax, errorCountTimes++); - for (int i=0;i<n;i++) - { - x[i]=0.f; - } - } - return !fail; - } else - - { int dimension = A.rows(); - if (0==dimension) - return true; - -// printf("================ solving using Lemke/Newton/Fixpoint\n"); - - btVectorXu q; - q.resize(dimension); - for (int row=0;row<dimension;row++) - { - q[row] = -b[row]; - } - - btLemkeAlgorithm lemke(A,q,m_debugLevel); - - - lemke.setSystem(A,q); - - btVectorXu solution = lemke.solve(m_maxLoops); - - //check solution - - bool fail = false; - int errorIndexMax = -1; - int errorIndexMin = -1; - float errorValueMax = -1e30; - float errorValueMin = 1e30; - - for (int i=0;i<dimension;i++) - { - x[i] = solution[i+dimension]; - volatile btScalar check = x[i]; - if (x[i] != check) + if (0 == dimension) + return true; + + // printf("================ solving using Lemke/Newton/Fixpoint\n"); + + btVectorXu q; + q.resize(dimension); + for (int row = 0; row < dimension; row++) { - x.setZero(); - return false; + q[row] = -b[row]; } - - //this is some hack/safety mechanism, to discard invalid solutions from the Lemke solver - //we need to figure out why it happens, and fix it, or detect it properly) - if (x[i]>m_maxValue) + + btLemkeAlgorithm lemke(A, q, m_debugLevel); + + lemke.setSystem(A, q); + + btVectorXu solution = lemke.solve(m_maxLoops); + + //check solution + + bool fail = false; + int errorIndexMax = -1; + int errorIndexMin = -1; + float errorValueMax = -1e30; + float errorValueMin = 1e30; + + for (int i = 0; i < dimension; i++) { - if (x[i]> errorValueMax) + x[i] = solution[i + dimension]; + volatile btScalar check = x[i]; + if (x[i] != check) { - fail = true; - errorIndexMax = i; - errorValueMax = x[i]; + x.setZero(); + return false; } - ////printf("x[i] = %f,",x[i]); - } - if (x[i]<-m_maxValue) - { - if (x[i]<errorValueMin) + + //this is some hack/safety mechanism, to discard invalid solutions from the Lemke solver + //we need to figure out why it happens, and fix it, or detect it properly) + if (x[i] > m_maxValue) + { + if (x[i] > errorValueMax) + { + fail = true; + errorIndexMax = i; + errorValueMax = x[i]; + } + ////printf("x[i] = %f,",x[i]); + } + if (x[i] < -m_maxValue) { - errorIndexMin = i; - errorValueMin = x[i]; - fail = true; - //printf("x[i] = %f,",x[i]); + if (x[i] < errorValueMin) + { + errorIndexMin = i; + errorValueMin = x[i]; + fail = true; + //printf("x[i] = %f,",x[i]); + } } } - } - if (fail) - { - static int errorCountTimes = 0; - if (errorIndexMin<0) - errorValueMin = 0.f; - if (errorIndexMax<0) - errorValueMax = 0.f; - printf("Error (x[%d] = %f, x[%d] = %f), resetting %d times\n", errorIndexMin,errorValueMin, errorIndexMax, errorValueMax, errorCountTimes++); - for (int i=0;i<dimension;i++) + if (fail) { - x[i]=0.f; + static int errorCountTimes = 0; + if (errorIndexMin < 0) + errorValueMin = 0.f; + if (errorIndexMax < 0) + errorValueMax = 0.f; + printf("Error (x[%d] = %f, x[%d] = %f), resetting %d times\n", errorIndexMin, errorValueMin, errorIndexMax, errorValueMax, errorCountTimes++); + for (int i = 0; i < dimension; i++) + { + x[i] = 0.f; + } } - } - - - return !fail; - } - return true; + return !fail; + } + return true; } - }; -#endif //BT_LEMKE_SOLVER_H +#endif //BT_LEMKE_SOLVER_H diff --git a/extern/bullet2/src/BulletDynamics/MLCPSolvers/btMLCPSolver.cpp b/extern/bullet2/src/BulletDynamics/MLCPSolvers/btMLCPSolver.cpp index 6688694a928..5d864f27578 100644 --- a/extern/bullet2/src/BulletDynamics/MLCPSolvers/btMLCPSolver.cpp +++ b/extern/bullet2/src/BulletDynamics/MLCPSolvers/btMLCPSolver.cpp @@ -19,11 +19,9 @@ subject to the following restrictions: #include "LinearMath/btQuickprof.h" #include "btSolveProjectedGaussSeidel.h" - -btMLCPSolver::btMLCPSolver( btMLCPSolverInterface* solver) -:m_solver(solver), -m_fallback(0), -m_cfm(0.000001)//0.0000001 +btMLCPSolver::btMLCPSolver(btMLCPSolverInterface* solver) + : m_solver(solver), + m_fallback(0) { } @@ -34,67 +32,65 @@ btMLCPSolver::~btMLCPSolver() bool gUseMatrixMultiply = false; bool interleaveContactAndFriction = false; -btScalar btMLCPSolver::solveGroupCacheFriendlySetup(btCollisionObject** bodies, int numBodiesUnUsed, btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer) +btScalar btMLCPSolver::solveGroupCacheFriendlySetup(btCollisionObject** bodies, int numBodiesUnUsed, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer) { - btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup( bodies, numBodiesUnUsed, manifoldPtr, numManifolds,constraints,numConstraints,infoGlobal,debugDrawer); + btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(bodies, numBodiesUnUsed, manifoldPtr, numManifolds, constraints, numConstraints, infoGlobal, debugDrawer); { BT_PROFILE("gather constraint data"); - int numFrictionPerContact = m_tmpSolverContactConstraintPool.size()==m_tmpSolverContactFrictionConstraintPool.size()? 1 : 2; - + int numFrictionPerContact = m_tmpSolverContactConstraintPool.size() == m_tmpSolverContactFrictionConstraintPool.size() ? 1 : 2; - // int numBodies = m_tmpSolverBodyPool.size(); + // int numBodies = m_tmpSolverBodyPool.size(); m_allConstraintPtrArray.resize(0); - m_limitDependencies.resize(m_tmpSolverNonContactConstraintPool.size()+m_tmpSolverContactConstraintPool.size()+m_tmpSolverContactFrictionConstraintPool.size()); - btAssert(m_limitDependencies.size() == m_tmpSolverNonContactConstraintPool.size()+m_tmpSolverContactConstraintPool.size()+m_tmpSolverContactFrictionConstraintPool.size()); - // printf("m_limitDependencies.size() = %d\n",m_limitDependencies.size()); + m_limitDependencies.resize(m_tmpSolverNonContactConstraintPool.size() + m_tmpSolverContactConstraintPool.size() + m_tmpSolverContactFrictionConstraintPool.size()); + btAssert(m_limitDependencies.size() == m_tmpSolverNonContactConstraintPool.size() + m_tmpSolverContactConstraintPool.size() + m_tmpSolverContactFrictionConstraintPool.size()); + // printf("m_limitDependencies.size() = %d\n",m_limitDependencies.size()); int dindex = 0; - for (int i=0;i<m_tmpSolverNonContactConstraintPool.size();i++) + for (int i = 0; i < m_tmpSolverNonContactConstraintPool.size(); i++) { m_allConstraintPtrArray.push_back(&m_tmpSolverNonContactConstraintPool[i]); m_limitDependencies[dindex++] = -1; } - + ///The btSequentialImpulseConstraintSolver moves all friction constraints at the very end, we can also interleave them instead - - int firstContactConstraintOffset=dindex; + + int firstContactConstraintOffset = dindex; if (interleaveContactAndFriction) { - for (int i=0;i<m_tmpSolverContactConstraintPool.size();i++) + for (int i = 0; i < m_tmpSolverContactConstraintPool.size(); i++) { m_allConstraintPtrArray.push_back(&m_tmpSolverContactConstraintPool[i]); m_limitDependencies[dindex++] = -1; - m_allConstraintPtrArray.push_back(&m_tmpSolverContactFrictionConstraintPool[i*numFrictionPerContact]); - int findex = (m_tmpSolverContactFrictionConstraintPool[i*numFrictionPerContact].m_frictionIndex*(1+numFrictionPerContact)); - m_limitDependencies[dindex++] = findex +firstContactConstraintOffset; - if (numFrictionPerContact==2) + m_allConstraintPtrArray.push_back(&m_tmpSolverContactFrictionConstraintPool[i * numFrictionPerContact]); + int findex = (m_tmpSolverContactFrictionConstraintPool[i * numFrictionPerContact].m_frictionIndex * (1 + numFrictionPerContact)); + m_limitDependencies[dindex++] = findex + firstContactConstraintOffset; + if (numFrictionPerContact == 2) { - m_allConstraintPtrArray.push_back(&m_tmpSolverContactFrictionConstraintPool[i*numFrictionPerContact+1]); - m_limitDependencies[dindex++] = findex+firstContactConstraintOffset; + m_allConstraintPtrArray.push_back(&m_tmpSolverContactFrictionConstraintPool[i * numFrictionPerContact + 1]); + m_limitDependencies[dindex++] = findex + firstContactConstraintOffset; } } - } else + } + else { - for (int i=0;i<m_tmpSolverContactConstraintPool.size();i++) + for (int i = 0; i < m_tmpSolverContactConstraintPool.size(); i++) { m_allConstraintPtrArray.push_back(&m_tmpSolverContactConstraintPool[i]); m_limitDependencies[dindex++] = -1; } - for (int i=0;i<m_tmpSolverContactFrictionConstraintPool.size();i++) + for (int i = 0; i < m_tmpSolverContactFrictionConstraintPool.size(); i++) { m_allConstraintPtrArray.push_back(&m_tmpSolverContactFrictionConstraintPool[i]); - m_limitDependencies[dindex++] = m_tmpSolverContactFrictionConstraintPool[i].m_frictionIndex+firstContactConstraintOffset; + m_limitDependencies[dindex++] = m_tmpSolverContactFrictionConstraintPool[i].m_frictionIndex + firstContactConstraintOffset; } - } - if (!m_allConstraintPtrArray.size()) { - m_A.resize(0,0); + m_A.resize(0, 0); m_b.resize(0); m_x.resize(0); m_lo.resize(0); @@ -103,7 +99,6 @@ btScalar btMLCPSolver::solveGroupCacheFriendlySetup(btCollisionObject** bodies, } } - if (gUseMatrixMultiply) { BT_PROFILE("createMLCP"); @@ -122,7 +117,7 @@ bool btMLCPSolver::solveMLCP(const btContactSolverInfo& infoGlobal) { bool result = true; - if (m_A.rows()==0) + if (m_A.rows() == 0) return true; //if using split impulse, we solve 2 separate (M)LCPs @@ -130,28 +125,26 @@ bool btMLCPSolver::solveMLCP(const btContactSolverInfo& infoGlobal) { btMatrixXu Acopy = m_A; btAlignedObjectArray<int> limitDependenciesCopy = m_limitDependencies; -// printf("solve first LCP\n"); - result = m_solver->solveMLCP(m_A, m_b, m_x, m_lo,m_hi, m_limitDependencies,infoGlobal.m_numIterations ); + // printf("solve first LCP\n"); + result = m_solver->solveMLCP(m_A, m_b, m_x, m_lo, m_hi, m_limitDependencies, infoGlobal.m_numIterations); if (result) - result = m_solver->solveMLCP(Acopy, m_bSplit, m_xSplit, m_lo,m_hi, limitDependenciesCopy,infoGlobal.m_numIterations ); - - } else + result = m_solver->solveMLCP(Acopy, m_bSplit, m_xSplit, m_lo, m_hi, limitDependenciesCopy, infoGlobal.m_numIterations); + } + else { - result = m_solver->solveMLCP(m_A, m_b, m_x, m_lo,m_hi, m_limitDependencies,infoGlobal.m_numIterations ); + result = m_solver->solveMLCP(m_A, m_b, m_x, m_lo, m_hi, m_limitDependencies, infoGlobal.m_numIterations); } return result; } struct btJointNode { - int jointIndex; // pointer to enclosing dxJoint object - int otherBodyIndex; // *other* body this joint is connected to - int nextJointNodeIndex;//-1 for null + int jointIndex; // pointer to enclosing dxJoint object + int otherBodyIndex; // *other* body this joint is connected to + int nextJointNodeIndex; //-1 for null int constraintRowIndex; }; - - void btMLCPSolver::createMLCPFast(const btContactSolverInfo& infoGlobal) { int numContactRows = interleaveContactAndFriction ? 3 : 1; @@ -164,36 +157,36 @@ void btMLCPSolver::createMLCPFast(const btContactSolverInfo& infoGlobal) m_bSplit.resize(numConstraintRows); m_b.setZero(); m_bSplit.setZero(); - for (int i=0;i<numConstraintRows ;i++) + for (int i = 0; i < numConstraintRows; i++) { btScalar jacDiag = m_allConstraintPtrArray[i]->m_jacDiagABInv; if (!btFuzzyZero(jacDiag)) { btScalar rhs = m_allConstraintPtrArray[i]->m_rhs; btScalar rhsPenetration = m_allConstraintPtrArray[i]->m_rhsPenetration; - m_b[i]=rhs/jacDiag; - m_bSplit[i] = rhsPenetration/jacDiag; + m_b[i] = rhs / jacDiag; + m_bSplit[i] = rhsPenetration / jacDiag; } - } } -// btScalar* w = 0; -// int nub = 0; + // btScalar* w = 0; + // int nub = 0; m_lo.resize(numConstraintRows); m_hi.resize(numConstraintRows); - + { BT_PROFILE("init lo/ho"); - for (int i=0;i<numConstraintRows;i++) + for (int i = 0; i < numConstraintRows; i++) { - if (0)//m_limitDependencies[i]>=0) + if (0) //m_limitDependencies[i]>=0) { m_lo[i] = -BT_INFINITY; m_hi[i] = BT_INFINITY; - } else + } + else { m_lo[i] = m_allConstraintPtrArray[i]->m_lowerLimit; m_hi[i] = m_allConstraintPtrArray[i]->m_upperLimit; @@ -202,48 +195,48 @@ void btMLCPSolver::createMLCPFast(const btContactSolverInfo& infoGlobal) } // - int m=m_allConstraintPtrArray.size(); + int m = m_allConstraintPtrArray.size(); int numBodies = m_tmpSolverBodyPool.size(); btAlignedObjectArray<int> bodyJointNodeArray; { BT_PROFILE("bodyJointNodeArray.resize"); - bodyJointNodeArray.resize(numBodies,-1); + bodyJointNodeArray.resize(numBodies, -1); } btAlignedObjectArray<btJointNode> jointNodeArray; { BT_PROFILE("jointNodeArray.reserve"); - jointNodeArray.reserve(2*m_allConstraintPtrArray.size()); + jointNodeArray.reserve(2 * m_allConstraintPtrArray.size()); } - static btMatrixXu J3; + btMatrixXu& J3 = m_scratchJ3; { BT_PROFILE("J3.resize"); - J3.resize(2*m,8); + J3.resize(2 * m, 8); } - static btMatrixXu JinvM3; + btMatrixXu& JinvM3 = m_scratchJInvM3; { BT_PROFILE("JinvM3.resize/setZero"); - JinvM3.resize(2*m,8); + JinvM3.resize(2 * m, 8); JinvM3.setZero(); J3.setZero(); } - int cur=0; + int cur = 0; int rowOffset = 0; - static btAlignedObjectArray<int> ofs; + btAlignedObjectArray<int>& ofs = m_scratchOfs; { BT_PROFILE("ofs resize"); ofs.resize(0); ofs.resizeNoInitialize(m_allConstraintPtrArray.size()); - } + } { BT_PROFILE("Compute J and JinvM"); - int c=0; + int c = 0; int numRows = 0; - for (int i=0;i<m_allConstraintPtrArray.size();i+=numRows,c++) + for (int i = 0; i < m_allConstraintPtrArray.size(); i += numRows, c++) { ofs[c] = rowOffset; int sbA = m_allConstraintPtrArray[i]->m_solverBodyIdA; @@ -251,14 +244,14 @@ void btMLCPSolver::createMLCPFast(const btContactSolverInfo& infoGlobal) btRigidBody* orgBodyA = m_tmpSolverBodyPool[sbA].m_originalBody; btRigidBody* orgBodyB = m_tmpSolverBodyPool[sbB].m_originalBody; - numRows = i<m_tmpSolverNonContactConstraintPool.size() ? m_tmpConstraintSizesPool[c].m_numConstraintRows : numContactRows ; + numRows = i < m_tmpSolverNonContactConstraintPool.size() ? m_tmpConstraintSizesPool[c].m_numConstraintRows : numContactRows; if (orgBodyA) { { - int slotA=-1; + int slotA = -1; //find free jointNode slot for sbA - slotA =jointNodeArray.size(); - jointNodeArray.expand();//NonInitializing(); + slotA = jointNodeArray.size(); + jointNodeArray.expand(); //NonInitializing(); int prevSlot = bodyJointNodeArray[sbA]; bodyJointNodeArray[sbA] = slotA; jointNodeArray[slotA].nextJointNodeIndex = prevSlot; @@ -266,35 +259,35 @@ void btMLCPSolver::createMLCPFast(const btContactSolverInfo& infoGlobal) jointNodeArray[slotA].constraintRowIndex = i; jointNodeArray[slotA].otherBodyIndex = orgBodyB ? sbB : -1; } - for (int row=0;row<numRows;row++,cur++) + for (int row = 0; row < numRows; row++, cur++) { - btVector3 normalInvMass = m_allConstraintPtrArray[i+row]->m_contactNormal1 * orgBodyA->getInvMass(); - btVector3 relPosCrossNormalInvInertia = m_allConstraintPtrArray[i+row]->m_relpos1CrossNormal * orgBodyA->getInvInertiaTensorWorld(); + btVector3 normalInvMass = m_allConstraintPtrArray[i + row]->m_contactNormal1 * orgBodyA->getInvMass(); + btVector3 relPosCrossNormalInvInertia = m_allConstraintPtrArray[i + row]->m_relpos1CrossNormal * orgBodyA->getInvInertiaTensorWorld(); - for (int r=0;r<3;r++) + for (int r = 0; r < 3; r++) { - J3.setElem(cur,r,m_allConstraintPtrArray[i+row]->m_contactNormal1[r]); - J3.setElem(cur,r+4,m_allConstraintPtrArray[i+row]->m_relpos1CrossNormal[r]); - JinvM3.setElem(cur,r,normalInvMass[r]); - JinvM3.setElem(cur,r+4,relPosCrossNormalInvInertia[r]); + J3.setElem(cur, r, m_allConstraintPtrArray[i + row]->m_contactNormal1[r]); + J3.setElem(cur, r + 4, m_allConstraintPtrArray[i + row]->m_relpos1CrossNormal[r]); + JinvM3.setElem(cur, r, normalInvMass[r]); + JinvM3.setElem(cur, r + 4, relPosCrossNormalInvInertia[r]); } - J3.setElem(cur,3,0); - JinvM3.setElem(cur,3,0); - J3.setElem(cur,7,0); - JinvM3.setElem(cur,7,0); + J3.setElem(cur, 3, 0); + JinvM3.setElem(cur, 3, 0); + J3.setElem(cur, 7, 0); + JinvM3.setElem(cur, 7, 0); } - } else + } + else { cur += numRows; } if (orgBodyB) { - { - int slotB=-1; + int slotB = -1; //find free jointNode slot for sbA - slotB =jointNodeArray.size(); - jointNodeArray.expand();//NonInitializing(); + slotB = jointNodeArray.size(); + jointNodeArray.expand(); //NonInitializing(); int prevSlot = bodyJointNodeArray[sbB]; bodyJointNodeArray[sbB] = slotB; jointNodeArray[slotB].nextJointNodeIndex = prevSlot; @@ -303,78 +296,74 @@ void btMLCPSolver::createMLCPFast(const btContactSolverInfo& infoGlobal) jointNodeArray[slotB].constraintRowIndex = i; } - for (int row=0;row<numRows;row++,cur++) + for (int row = 0; row < numRows; row++, cur++) { - btVector3 normalInvMassB = m_allConstraintPtrArray[i+row]->m_contactNormal2*orgBodyB->getInvMass(); - btVector3 relPosInvInertiaB = m_allConstraintPtrArray[i+row]->m_relpos2CrossNormal * orgBodyB->getInvInertiaTensorWorld(); + btVector3 normalInvMassB = m_allConstraintPtrArray[i + row]->m_contactNormal2 * orgBodyB->getInvMass(); + btVector3 relPosInvInertiaB = m_allConstraintPtrArray[i + row]->m_relpos2CrossNormal * orgBodyB->getInvInertiaTensorWorld(); - for (int r=0;r<3;r++) + for (int r = 0; r < 3; r++) { - J3.setElem(cur,r,m_allConstraintPtrArray[i+row]->m_contactNormal2[r]); - J3.setElem(cur,r+4,m_allConstraintPtrArray[i+row]->m_relpos2CrossNormal[r]); - JinvM3.setElem(cur,r,normalInvMassB[r]); - JinvM3.setElem(cur,r+4,relPosInvInertiaB[r]); + J3.setElem(cur, r, m_allConstraintPtrArray[i + row]->m_contactNormal2[r]); + J3.setElem(cur, r + 4, m_allConstraintPtrArray[i + row]->m_relpos2CrossNormal[r]); + JinvM3.setElem(cur, r, normalInvMassB[r]); + JinvM3.setElem(cur, r + 4, relPosInvInertiaB[r]); } - J3.setElem(cur,3,0); - JinvM3.setElem(cur,3,0); - J3.setElem(cur,7,0); - JinvM3.setElem(cur,7,0); + J3.setElem(cur, 3, 0); + JinvM3.setElem(cur, 3, 0); + J3.setElem(cur, 7, 0); + JinvM3.setElem(cur, 7, 0); } } else { cur += numRows; } - rowOffset+=numRows; - + rowOffset += numRows; } - } - //compute JinvM = J*invM. const btScalar* JinvM = JinvM3.getBufferPointer(); const btScalar* Jptr = J3.getBufferPointer(); { BT_PROFILE("m_A.resize"); - m_A.resize(n,n); + m_A.resize(n, n); } - + { BT_PROFILE("m_A.setZero"); m_A.setZero(); } - int c=0; + int c = 0; { int numRows = 0; BT_PROFILE("Compute A"); - for (int i=0;i<m_allConstraintPtrArray.size();i+= numRows,c++) + for (int i = 0; i < m_allConstraintPtrArray.size(); i += numRows, c++) { int row__ = ofs[c]; int sbA = m_allConstraintPtrArray[i]->m_solverBodyIdA; int sbB = m_allConstraintPtrArray[i]->m_solverBodyIdB; - // btRigidBody* orgBodyA = m_tmpSolverBodyPool[sbA].m_originalBody; - // btRigidBody* orgBodyB = m_tmpSolverBodyPool[sbB].m_originalBody; + // btRigidBody* orgBodyA = m_tmpSolverBodyPool[sbA].m_originalBody; + // btRigidBody* orgBodyB = m_tmpSolverBodyPool[sbB].m_originalBody; + + numRows = i < m_tmpSolverNonContactConstraintPool.size() ? m_tmpConstraintSizesPool[c].m_numConstraintRows : numContactRows; - numRows = i<m_tmpSolverNonContactConstraintPool.size() ? m_tmpConstraintSizesPool[c].m_numConstraintRows : numContactRows ; - - const btScalar *JinvMrow = JinvM + 2*8*(size_t)row__; + const btScalar* JinvMrow = JinvM + 2 * 8 * (size_t)row__; { int startJointNodeA = bodyJointNodeArray[sbA]; - while (startJointNodeA>=0) + while (startJointNodeA >= 0) { int j0 = jointNodeArray[startJointNodeA].jointIndex; int cr0 = jointNodeArray[startJointNodeA].constraintRowIndex; - if (j0<c) + if (j0 < c) { - int numRowsOther = cr0 < m_tmpSolverNonContactConstraintPool.size() ? m_tmpConstraintSizesPool[j0].m_numConstraintRows : numContactRows; - size_t ofsother = (m_allConstraintPtrArray[cr0]->m_solverBodyIdB == sbA) ? 8*numRowsOther : 0; + size_t ofsother = (m_allConstraintPtrArray[cr0]->m_solverBodyIdB == sbA) ? 8 * numRowsOther : 0; //printf("%d joint i %d and j0: %d: ",count++,i,j0); - m_A.multiplyAdd2_p8r ( JinvMrow, - Jptr + 2*8*(size_t)ofs[j0] + ofsother, numRows, numRowsOther, row__,ofs[j0]); + m_A.multiplyAdd2_p8r(JinvMrow, + Jptr + 2 * 8 * (size_t)ofs[j0] + ofsother, numRows, numRowsOther, row__, ofs[j0]); } startJointNodeA = jointNodeArray[startJointNodeA].nextJointNodeIndex; } @@ -382,17 +371,17 @@ void btMLCPSolver::createMLCPFast(const btContactSolverInfo& infoGlobal) { int startJointNodeB = bodyJointNodeArray[sbB]; - while (startJointNodeB>=0) + while (startJointNodeB >= 0) { int j1 = jointNodeArray[startJointNodeB].jointIndex; int cj1 = jointNodeArray[startJointNodeB].constraintRowIndex; - if (j1<c) + if (j1 < c) { - int numRowsOther = cj1 < m_tmpSolverNonContactConstraintPool.size() ? m_tmpConstraintSizesPool[j1].m_numConstraintRows : numContactRows; - size_t ofsother = (m_allConstraintPtrArray[cj1]->m_solverBodyIdB == sbB) ? 8*numRowsOther : 0; - m_A.multiplyAdd2_p8r ( JinvMrow + 8*(size_t)numRows, - Jptr + 2*8*(size_t)ofs[j1] + ofsother, numRows, numRowsOther, row__,ofs[j1]); + int numRowsOther = cj1 < m_tmpSolverNonContactConstraintPool.size() ? m_tmpConstraintSizesPool[j1].m_numConstraintRows : numContactRows; + size_t ofsother = (m_allConstraintPtrArray[cj1]->m_solverBodyIdB == sbB) ? 8 * numRowsOther : 0; + m_A.multiplyAdd2_p8r(JinvMrow + 8 * (size_t)numRows, + Jptr + 2 * 8 * (size_t)ofs[j1] + ofsother, numRows, numRowsOther, row__, ofs[j1]); } startJointNodeB = jointNodeArray[startJointNodeB].nextJointNodeIndex; } @@ -403,27 +392,25 @@ void btMLCPSolver::createMLCPFast(const btContactSolverInfo& infoGlobal) BT_PROFILE("compute diagonal"); // compute diagonal blocks of m_A - int row__ = 0; + int row__ = 0; int numJointRows = m_allConstraintPtrArray.size(); - int jj=0; - for (;row__<numJointRows;) + int jj = 0; + for (; row__ < numJointRows;) { - //int sbA = m_allConstraintPtrArray[row__]->m_solverBodyIdA; int sbB = m_allConstraintPtrArray[row__]->m_solverBodyIdB; - // btRigidBody* orgBodyA = m_tmpSolverBodyPool[sbA].m_originalBody; + // btRigidBody* orgBodyA = m_tmpSolverBodyPool[sbA].m_originalBody; btRigidBody* orgBodyB = m_tmpSolverBodyPool[sbB].m_originalBody; + const unsigned int infom = row__ < m_tmpSolverNonContactConstraintPool.size() ? m_tmpConstraintSizesPool[jj].m_numConstraintRows : numContactRows; - const unsigned int infom = row__ < m_tmpSolverNonContactConstraintPool.size() ? m_tmpConstraintSizesPool[jj].m_numConstraintRows : numContactRows; - - const btScalar *JinvMrow = JinvM + 2*8*(size_t)row__; - const btScalar *Jrow = Jptr + 2*8*(size_t)row__; - m_A.multiply2_p8r (JinvMrow, Jrow, infom, infom, row__,row__); - if (orgBodyB) + const btScalar* JinvMrow = JinvM + 2 * 8 * (size_t)row__; + const btScalar* Jrow = Jptr + 2 * 8 * (size_t)row__; + m_A.multiply2_p8r(JinvMrow, Jrow, infom, infom, row__, row__); + if (orgBodyB) { - m_A.multiplyAdd2_p8r (JinvMrow + 8*(size_t)infom, Jrow + 8*(size_t)infom, infom, infom, row__,row__); + m_A.multiplyAdd2_p8r(JinvMrow + 8 * (size_t)infom, Jrow + 8 * (size_t)infom, infom, infom, row__, row__); } row__ += infom; jj++; @@ -434,12 +421,12 @@ void btMLCPSolver::createMLCPFast(const btContactSolverInfo& infoGlobal) if (1) { // add cfm to the diagonal of m_A - for ( int i=0; i<m_A.rows(); ++i) + for (int i = 0; i < m_A.rows(); ++i) { - m_A.setElem(i,i,m_A(i,i)+ m_cfm / infoGlobal.m_timeStep); + m_A.setElem(i, i, m_A(i, i) + infoGlobal.m_globalCfm / infoGlobal.m_timeStep); } } - + ///fill the upper triangle of the matrix, to make it symmetric { BT_PROFILE("fill the upper triangle "); @@ -451,21 +438,21 @@ void btMLCPSolver::createMLCPFast(const btContactSolverInfo& infoGlobal) m_x.resize(numConstraintRows); m_xSplit.resize(numConstraintRows); - if (infoGlobal.m_solverMode&SOLVER_USE_WARMSTARTING) + if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING) { - for (int i=0;i<m_allConstraintPtrArray.size();i++) + for (int i = 0; i < m_allConstraintPtrArray.size(); i++) { const btSolverConstraint& c = *m_allConstraintPtrArray[i]; - m_x[i]=c.m_appliedImpulse; + m_x[i] = c.m_appliedImpulse; m_xSplit[i] = c.m_appliedPushImpulse; } - } else + } + else { m_x.setZero(); m_xSplit.setZero(); } } - } void btMLCPSolver::createMLCP(const btContactSolverInfo& infoGlobal) @@ -476,120 +463,116 @@ void btMLCPSolver::createMLCP(const btContactSolverInfo& infoGlobal) m_b.resize(numConstraintRows); if (infoGlobal.m_splitImpulse) m_bSplit.resize(numConstraintRows); - + m_bSplit.setZero(); m_b.setZero(); - for (int i=0;i<numConstraintRows ;i++) + for (int i = 0; i < numConstraintRows; i++) { if (m_allConstraintPtrArray[i]->m_jacDiagABInv) { - m_b[i]=m_allConstraintPtrArray[i]->m_rhs/m_allConstraintPtrArray[i]->m_jacDiagABInv; + m_b[i] = m_allConstraintPtrArray[i]->m_rhs / m_allConstraintPtrArray[i]->m_jacDiagABInv; if (infoGlobal.m_splitImpulse) - m_bSplit[i] = m_allConstraintPtrArray[i]->m_rhsPenetration/m_allConstraintPtrArray[i]->m_jacDiagABInv; + m_bSplit[i] = m_allConstraintPtrArray[i]->m_rhsPenetration / m_allConstraintPtrArray[i]->m_jacDiagABInv; } } - - static btMatrixXu Minv; - Minv.resize(6*numBodies,6*numBodies); + + btMatrixXu& Minv = m_scratchMInv; + Minv.resize(6 * numBodies, 6 * numBodies); Minv.setZero(); - for (int i=0;i<numBodies;i++) + for (int i = 0; i < numBodies; i++) { const btSolverBody& rb = m_tmpSolverBodyPool[i]; const btVector3& invMass = rb.m_invMass; - setElem(Minv,i*6+0,i*6+0,invMass[0]); - setElem(Minv,i*6+1,i*6+1,invMass[1]); - setElem(Minv,i*6+2,i*6+2,invMass[2]); + setElem(Minv, i * 6 + 0, i * 6 + 0, invMass[0]); + setElem(Minv, i * 6 + 1, i * 6 + 1, invMass[1]); + setElem(Minv, i * 6 + 2, i * 6 + 2, invMass[2]); btRigidBody* orgBody = m_tmpSolverBodyPool[i].m_originalBody; - - for (int r=0;r<3;r++) - for (int c=0;c<3;c++) - setElem(Minv,i*6+3+r,i*6+3+c,orgBody? orgBody->getInvInertiaTensorWorld()[r][c] : 0); + + for (int r = 0; r < 3; r++) + for (int c = 0; c < 3; c++) + setElem(Minv, i * 6 + 3 + r, i * 6 + 3 + c, orgBody ? orgBody->getInvInertiaTensorWorld()[r][c] : 0); } - - static btMatrixXu J; - J.resize(numConstraintRows,6*numBodies); + + btMatrixXu& J = m_scratchJ; + J.resize(numConstraintRows, 6 * numBodies); J.setZero(); - + m_lo.resize(numConstraintRows); m_hi.resize(numConstraintRows); - - for (int i=0;i<numConstraintRows;i++) - { + for (int i = 0; i < numConstraintRows; i++) + { m_lo[i] = m_allConstraintPtrArray[i]->m_lowerLimit; m_hi[i] = m_allConstraintPtrArray[i]->m_upperLimit; - + int bodyIndex0 = m_allConstraintPtrArray[i]->m_solverBodyIdA; int bodyIndex1 = m_allConstraintPtrArray[i]->m_solverBodyIdB; if (m_tmpSolverBodyPool[bodyIndex0].m_originalBody) { - setElem(J,i,6*bodyIndex0+0,m_allConstraintPtrArray[i]->m_contactNormal1[0]); - setElem(J,i,6*bodyIndex0+1,m_allConstraintPtrArray[i]->m_contactNormal1[1]); - setElem(J,i,6*bodyIndex0+2,m_allConstraintPtrArray[i]->m_contactNormal1[2]); - setElem(J,i,6*bodyIndex0+3,m_allConstraintPtrArray[i]->m_relpos1CrossNormal[0]); - setElem(J,i,6*bodyIndex0+4,m_allConstraintPtrArray[i]->m_relpos1CrossNormal[1]); - setElem(J,i,6*bodyIndex0+5,m_allConstraintPtrArray[i]->m_relpos1CrossNormal[2]); + setElem(J, i, 6 * bodyIndex0 + 0, m_allConstraintPtrArray[i]->m_contactNormal1[0]); + setElem(J, i, 6 * bodyIndex0 + 1, m_allConstraintPtrArray[i]->m_contactNormal1[1]); + setElem(J, i, 6 * bodyIndex0 + 2, m_allConstraintPtrArray[i]->m_contactNormal1[2]); + setElem(J, i, 6 * bodyIndex0 + 3, m_allConstraintPtrArray[i]->m_relpos1CrossNormal[0]); + setElem(J, i, 6 * bodyIndex0 + 4, m_allConstraintPtrArray[i]->m_relpos1CrossNormal[1]); + setElem(J, i, 6 * bodyIndex0 + 5, m_allConstraintPtrArray[i]->m_relpos1CrossNormal[2]); } if (m_tmpSolverBodyPool[bodyIndex1].m_originalBody) { - setElem(J,i,6*bodyIndex1+0,m_allConstraintPtrArray[i]->m_contactNormal2[0]); - setElem(J,i,6*bodyIndex1+1,m_allConstraintPtrArray[i]->m_contactNormal2[1]); - setElem(J,i,6*bodyIndex1+2,m_allConstraintPtrArray[i]->m_contactNormal2[2]); - setElem(J,i,6*bodyIndex1+3,m_allConstraintPtrArray[i]->m_relpos2CrossNormal[0]); - setElem(J,i,6*bodyIndex1+4,m_allConstraintPtrArray[i]->m_relpos2CrossNormal[1]); - setElem(J,i,6*bodyIndex1+5,m_allConstraintPtrArray[i]->m_relpos2CrossNormal[2]); + setElem(J, i, 6 * bodyIndex1 + 0, m_allConstraintPtrArray[i]->m_contactNormal2[0]); + setElem(J, i, 6 * bodyIndex1 + 1, m_allConstraintPtrArray[i]->m_contactNormal2[1]); + setElem(J, i, 6 * bodyIndex1 + 2, m_allConstraintPtrArray[i]->m_contactNormal2[2]); + setElem(J, i, 6 * bodyIndex1 + 3, m_allConstraintPtrArray[i]->m_relpos2CrossNormal[0]); + setElem(J, i, 6 * bodyIndex1 + 4, m_allConstraintPtrArray[i]->m_relpos2CrossNormal[1]); + setElem(J, i, 6 * bodyIndex1 + 5, m_allConstraintPtrArray[i]->m_relpos2CrossNormal[2]); } } - - static btMatrixXu J_transpose; - J_transpose= J.transpose(); - static btMatrixXu tmp; + btMatrixXu& J_transpose = m_scratchJTranspose; + J_transpose = J.transpose(); + + btMatrixXu& tmp = m_scratchTmp; { { BT_PROFILE("J*Minv"); - tmp = J*Minv; - + tmp = J * Minv; } { BT_PROFILE("J*tmp"); - m_A = tmp*J_transpose; + m_A = tmp * J_transpose; } } if (1) { // add cfm to the diagonal of m_A - for ( int i=0; i<m_A.rows(); ++i) + for (int i = 0; i < m_A.rows(); ++i) { - m_A.setElem(i,i,m_A(i,i)+ m_cfm / infoGlobal.m_timeStep); + m_A.setElem(i, i, m_A(i, i) + infoGlobal.m_globalCfm / infoGlobal.m_timeStep); } } m_x.resize(numConstraintRows); if (infoGlobal.m_splitImpulse) m_xSplit.resize(numConstraintRows); -// m_x.setZero(); + // m_x.setZero(); - for (int i=0;i<m_allConstraintPtrArray.size();i++) + for (int i = 0; i < m_allConstraintPtrArray.size(); i++) { const btSolverConstraint& c = *m_allConstraintPtrArray[i]; - m_x[i]=c.m_appliedImpulse; + m_x[i] = c.m_appliedImpulse; if (infoGlobal.m_splitImpulse) m_xSplit[i] = c.m_appliedPushImpulse; } - } - -btScalar btMLCPSolver::solveGroupCacheFriendlyIterations(btCollisionObject** bodies ,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer) +btScalar btMLCPSolver::solveGroupCacheFriendlyIterations(btCollisionObject** bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer) { bool result = true; { BT_PROFILE("solveMLCP"); -// printf("m_A(%d,%d)\n", m_A.rows(),m_A.cols()); + // printf("m_A(%d,%d)\n", m_A.rows(),m_A.cols()); result = solveMLCP(infoGlobal); } @@ -597,44 +580,41 @@ btScalar btMLCPSolver::solveGroupCacheFriendlyIterations(btCollisionObject** bod if (result) { BT_PROFILE("process MLCP results"); - for (int i=0;i<m_allConstraintPtrArray.size();i++) + for (int i = 0; i < m_allConstraintPtrArray.size(); i++) { { btSolverConstraint& c = *m_allConstraintPtrArray[i]; int sbA = c.m_solverBodyIdA; int sbB = c.m_solverBodyIdB; //btRigidBody* orgBodyA = m_tmpSolverBodyPool[sbA].m_originalBody; - // btRigidBody* orgBodyB = m_tmpSolverBodyPool[sbB].m_originalBody; + // btRigidBody* orgBodyB = m_tmpSolverBodyPool[sbB].m_originalBody; btSolverBody& solverBodyA = m_tmpSolverBodyPool[sbA]; btSolverBody& solverBodyB = m_tmpSolverBodyPool[sbB]; - + { - btScalar deltaImpulse = m_x[i]-c.m_appliedImpulse; + btScalar deltaImpulse = m_x[i] - c.m_appliedImpulse; c.m_appliedImpulse = m_x[i]; - solverBodyA.internalApplyImpulse(c.m_contactNormal1*solverBodyA.internalGetInvMass(),c.m_angularComponentA,deltaImpulse); - solverBodyB.internalApplyImpulse(c.m_contactNormal2*solverBodyB.internalGetInvMass(),c.m_angularComponentB,deltaImpulse); + solverBodyA.internalApplyImpulse(c.m_contactNormal1 * solverBodyA.internalGetInvMass(), c.m_angularComponentA, deltaImpulse); + solverBodyB.internalApplyImpulse(c.m_contactNormal2 * solverBodyB.internalGetInvMass(), c.m_angularComponentB, deltaImpulse); } if (infoGlobal.m_splitImpulse) { btScalar deltaImpulse = m_xSplit[i] - c.m_appliedPushImpulse; - solverBodyA.internalApplyPushImpulse(c.m_contactNormal1*solverBodyA.internalGetInvMass(),c.m_angularComponentA,deltaImpulse); - solverBodyB.internalApplyPushImpulse(c.m_contactNormal2*solverBodyB.internalGetInvMass(),c.m_angularComponentB,deltaImpulse); + solverBodyA.internalApplyPushImpulse(c.m_contactNormal1 * solverBodyA.internalGetInvMass(), c.m_angularComponentA, deltaImpulse); + solverBodyB.internalApplyPushImpulse(c.m_contactNormal2 * solverBodyB.internalGetInvMass(), c.m_angularComponentB, deltaImpulse); c.m_appliedPushImpulse = m_xSplit[i]; } - } } } else { - // printf("m_fallback = %d\n",m_fallback); + // printf("m_fallback = %d\n",m_fallback); m_fallback++; - btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyIterations(bodies ,numBodies,manifoldPtr, numManifolds,constraints,numConstraints,infoGlobal,debugDrawer); + btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyIterations(bodies, numBodies, manifoldPtr, numManifolds, constraints, numConstraints, infoGlobal, debugDrawer); } return 0.f; } - - diff --git a/extern/bullet2/src/BulletDynamics/MLCPSolvers/btMLCPSolver.h b/extern/bullet2/src/BulletDynamics/MLCPSolvers/btMLCPSolver.h index 43e85445bad..510ae59e58b 100644 --- a/extern/bullet2/src/BulletDynamics/MLCPSolvers/btMLCPSolver.h +++ b/extern/bullet2/src/BulletDynamics/MLCPSolvers/btMLCPSolver.h @@ -23,15 +23,13 @@ subject to the following restrictions: class btMLCPSolver : public btSequentialImpulseConstraintSolver { - protected: - btMatrixXu m_A; btVectorXu m_b; btVectorXu m_x; btVectorXu m_lo; btVectorXu m_hi; - + ///when using 'split impulse' we solve two separate (M)LCPs btVectorXu m_bSplit; btVectorXu m_xSplit; @@ -39,14 +37,23 @@ protected: btVectorXu m_xSplit2; btAlignedObjectArray<int> m_limitDependencies; - btAlignedObjectArray<btSolverConstraint*> m_allConstraintPtrArray; + btAlignedObjectArray<btSolverConstraint*> m_allConstraintPtrArray; btMLCPSolverInterface* m_solver; int m_fallback; - btScalar m_cfm; - virtual btScalar solveGroupCacheFriendlySetup(btCollisionObject** bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer); - virtual btScalar solveGroupCacheFriendlyIterations(btCollisionObject** bodies ,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer); + /// The following scratch variables are not stateful -- contents are cleared prior to each use. + /// They are only cached here to avoid extra memory allocations and deallocations and to ensure + /// that multiple instances of the solver can be run in parallel. + btMatrixXu m_scratchJ3; + btMatrixXu m_scratchJInvM3; + btAlignedObjectArray<int> m_scratchOfs; + btMatrixXu m_scratchMInv; + btMatrixXu m_scratchJ; + btMatrixXu m_scratchJTranspose; + btMatrixXu m_scratchTmp; + virtual btScalar solveGroupCacheFriendlySetup(btCollisionObject** bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer); + virtual btScalar solveGroupCacheFriendlyIterations(btCollisionObject** bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer); virtual void createMLCP(const btContactSolverInfo& infoGlobal); virtual void createMLCPFast(const btContactSolverInfo& infoGlobal); @@ -55,8 +62,7 @@ protected: virtual bool solveMLCP(const btContactSolverInfo& infoGlobal); public: - - btMLCPSolver( btMLCPSolverInterface* solver); + btMLCPSolver(btMLCPSolverInterface* solver); virtual ~btMLCPSolver(); void setMLCPSolver(btMLCPSolverInterface* solver) @@ -73,21 +79,10 @@ public: m_fallback = num; } - btScalar getCfm() const - { - return m_cfm; - } - void setCfm(btScalar cfm) - { - m_cfm = cfm; - } - - virtual btConstraintSolverType getSolverType() const + virtual btConstraintSolverType getSolverType() const { return BT_MLCP_SOLVER; } - }; - -#endif //BT_MLCP_SOLVER_H +#endif //BT_MLCP_SOLVER_H diff --git a/extern/bullet2/src/BulletDynamics/MLCPSolvers/btMLCPSolverInterface.h b/extern/bullet2/src/BulletDynamics/MLCPSolvers/btMLCPSolverInterface.h index 25bb3f6d327..6b0465b88d7 100644 --- a/extern/bullet2/src/BulletDynamics/MLCPSolvers/btMLCPSolverInterface.h +++ b/extern/bullet2/src/BulletDynamics/MLCPSolvers/btMLCPSolverInterface.h @@ -27,7 +27,7 @@ public: } //return true is it solves the problem successfully - virtual bool solveMLCP(const btMatrixXu & A, const btVectorXu & b, btVectorXu& x, const btVectorXu & lo,const btVectorXu & hi,const btAlignedObjectArray<int>& limitDependency, int numIterations, bool useSparsity = true)=0; + virtual bool solveMLCP(const btMatrixXu& A, const btVectorXu& b, btVectorXu& x, const btVectorXu& lo, const btVectorXu& hi, const btAlignedObjectArray<int>& limitDependency, int numIterations, bool useSparsity = true) = 0; }; -#endif //BT_MLCP_SOLVER_INTERFACE_H +#endif //BT_MLCP_SOLVER_INTERFACE_H diff --git a/extern/bullet2/src/BulletDynamics/MLCPSolvers/btPATHSolver.h b/extern/bullet2/src/BulletDynamics/MLCPSolvers/btPATHSolver.h index 9ec31a6d4e4..7f8eec3f6e3 100644 --- a/extern/bullet2/src/BulletDynamics/MLCPSolvers/btPATHSolver.h +++ b/extern/bullet2/src/BulletDynamics/MLCPSolvers/btPATHSolver.h @@ -14,38 +14,35 @@ subject to the following restrictions: */ ///original version written by Erwin Coumans, October 2013 - #ifndef BT_PATH_SOLVER_H #define BT_PATH_SOLVER_H //#define BT_USE_PATH #ifdef BT_USE_PATH -extern "C" { +extern "C" +{ #include "PATH/SimpleLCP.h" #include "PATH/License.h" #include "PATH/Error_Interface.h" }; - void __stdcall MyError(Void *data, Char *msg) +void __stdcall MyError(Void *data, Char *msg) { - printf("Path Error: %s\n",msg); + printf("Path Error: %s\n", msg); } - void __stdcall MyWarning(Void *data, Char *msg) +void __stdcall MyWarning(Void *data, Char *msg) { - printf("Path Warning: %s\n",msg); + printf("Path Warning: %s\n", msg); } Error_Interface e; - - #include "btMLCPSolverInterface.h" #include "Dantzig/lcp.h" class btPathSolver : public btMLCPSolverInterface { public: - btPathSolver() { License_SetString("2069810742&Courtesy_License&&&USR&2013&14_12_2011&1000&PATH&GEN&31_12_2013&0_0_0&0&0_0"); @@ -55,17 +52,15 @@ public: Error_SetInterface(&e); } - - virtual bool solveMLCP(const btMatrixXu & A, const btVectorXu & b, btVectorXu& x, const btVectorXu & lo,const btVectorXu & hi,const btAlignedObjectArray<int>& limitDependency, int numIterations, bool useSparsity = true) + virtual bool solveMLCP(const btMatrixXu &A, const btVectorXu &b, btVectorXu &x, const btVectorXu &lo, const btVectorXu &hi, const btAlignedObjectArray<int> &limitDependency, int numIterations, bool useSparsity = true) { MCP_Termination status; - int numVariables = b.rows(); - if (0==numVariables) + if (0 == numVariables) return true; - /* - variables - the number of variables in the problem + /* - variables - the number of variables in the problem - m_nnz - the number of nonzeros in the M matrix - m_i - a vector of size m_nnz containing the row indices for M - m_j - a vector of size m_nnz containing the column indices for M @@ -78,16 +73,16 @@ public: btAlignedObjectArray<int> rowIndices; btAlignedObjectArray<int> colIndices; - for (int i=0;i<A.rows();i++) + for (int i = 0; i < A.rows(); i++) { - for (int j=0;j<A.cols();j++) + for (int j = 0; j < A.cols(); j++) { - if (A(i,j)!=0.f) + if (A(i, j) != 0.f) { //add 1, because Path starts at 1, instead of 0 - rowIndices.push_back(i+1); - colIndices.push_back(j+1); - values.push_back(A(i,j)); + rowIndices.push_back(i + 1); + colIndices.push_back(j + 1); + values.push_back(A(i, j)); } } } @@ -97,19 +92,18 @@ public: btAlignedObjectArray<double> rhs; btAlignedObjectArray<double> upperBounds; btAlignedObjectArray<double> lowerBounds; - for (int i=0;i<numVariables;i++) + for (int i = 0; i < numVariables; i++) { upperBounds.push_back(hi[i]); lowerBounds.push_back(lo[i]); rhs.push_back(-b[i]); } - - SimpleLCP(numVariables,numNonZero,&rowIndices[0],&colIndices[0],&values[0],&rhs[0],&lowerBounds[0],&upperBounds[0], &status, &zResult[0]); + SimpleLCP(numVariables, numNonZero, &rowIndices[0], &colIndices[0], &values[0], &rhs[0], &lowerBounds[0], &upperBounds[0], &status, &zResult[0]); if (status != MCP_Solved) { - static const char* gReturnMsgs[] = { + static const char *gReturnMsgs[] = { "Invalid return", "MCP_Solved: The problem was solved", "MCP_NoProgress: A stationary point was found", @@ -122,16 +116,16 @@ public: "MCP_Infeasible: Problem has no solution", "MCP_Error: An error occurred within the code", "MCP_LicenseError: License could not be found", - "MCP_OK" - }; + "MCP_OK"}; - printf("ERROR: The PATH MCP solver failed: %s\n", gReturnMsgs[(unsigned int)status]);// << std::endl; + printf("ERROR: The PATH MCP solver failed: %s\n", gReturnMsgs[(unsigned int)status]); // << std::endl; printf("using Projected Gauss Seidel fallback\n"); - + return false; - } else + } + else { - for (int i=0;i<numVariables;i++) + for (int i = 0; i < numVariables; i++) { x[i] = zResult[i]; //check for #NAN @@ -139,13 +133,10 @@ public: return false; } return true; - } - } }; -#endif //BT_USE_PATH - +#endif //BT_USE_PATH -#endif //BT_PATH_SOLVER_H +#endif //BT_PATH_SOLVER_H diff --git a/extern/bullet2/src/BulletDynamics/MLCPSolvers/btSolveProjectedGaussSeidel.h b/extern/bullet2/src/BulletDynamics/MLCPSolvers/btSolveProjectedGaussSeidel.h index 77cc57c6e0e..c3f4ec39974 100644 --- a/extern/bullet2/src/BulletDynamics/MLCPSolvers/btSolveProjectedGaussSeidel.h +++ b/extern/bullet2/src/BulletDynamics/MLCPSolvers/btSolveProjectedGaussSeidel.h @@ -17,14 +17,22 @@ subject to the following restrictions: #ifndef BT_SOLVE_PROJECTED_GAUSS_SEIDEL_H #define BT_SOLVE_PROJECTED_GAUSS_SEIDEL_H - #include "btMLCPSolverInterface.h" ///This solver is mainly for debug/learning purposes: it is functionally equivalent to the btSequentialImpulseConstraintSolver solver, but much slower (it builds the full LCP matrix) class btSolveProjectedGaussSeidel : public btMLCPSolverInterface { public: - virtual bool solveMLCP(const btMatrixXu & A, const btVectorXu & b, btVectorXu& x, const btVectorXu & lo,const btVectorXu & hi,const btAlignedObjectArray<int>& limitDependency, int numIterations, bool useSparsity = true) + btScalar m_leastSquaresResidualThreshold; + btScalar m_leastSquaresResidual; + + btSolveProjectedGaussSeidel() + : m_leastSquaresResidualThreshold(0), + m_leastSquaresResidual(0) + { + } + + virtual bool solveMLCP(const btMatrixXu& A, const btVectorXu& b, btVectorXu& x, const btVectorXu& lo, const btVectorXu& hi, const btAlignedObjectArray<int>& limitDependency, int numIterations, bool useSparsity = true) { if (!A.rows()) return true; @@ -35,52 +43,65 @@ public: btAssert(A.rows() == b.rows()); int i, j, numRows = A.rows(); - - float delta; - for (int k = 0; k <numIterations; k++) + btScalar delta; + + for (int k = 0; k < numIterations; k++) { - for (i = 0; i <numRows; i++) + m_leastSquaresResidual = 0.f; + for (i = 0; i < numRows; i++) { delta = 0.0f; if (useSparsity) { - for (int h=0;h<A.m_rowNonZeroElements1[i].size();h++) + for (int h = 0; h < A.m_rowNonZeroElements1[i].size(); h++) { - int j = A.m_rowNonZeroElements1[i][h]; - if (j != i)//skip main diagonal + j = A.m_rowNonZeroElements1[i][h]; + if (j != i) //skip main diagonal { - delta += A(i,j) * x[j]; + delta += A(i, j) * x[j]; } } - } else + } + else { - for (j = 0; j <i; j++) - delta += A(i,j) * x[j]; - for (j = i+1; j<numRows; j++) - delta += A(i,j) * x[j]; + for (j = 0; j < i; j++) + delta += A(i, j) * x[j]; + for (j = i + 1; j < numRows; j++) + delta += A(i, j) * x[j]; } - float aDiag = A(i,i); - x [i] = (b [i] - delta) / aDiag; - float s = 1.f; + btScalar aDiag = A(i, i); + btScalar xOld = x[i]; + x[i] = (b[i] - delta) / aDiag; + btScalar s = 1.f; - if (limitDependency[i]>=0) + if (limitDependency[i] >= 0) { s = x[limitDependency[i]]; - if (s<0) - s=1; + if (s < 0) + s = 1; } - - if (x[i]<lo[i]*s) - x[i]=lo[i]*s; - if (x[i]>hi[i]*s) - x[i]=hi[i]*s; + + if (x[i] < lo[i] * s) + x[i] = lo[i] * s; + if (x[i] > hi[i] * s) + x[i] = hi[i] * s; + btScalar diff = x[i] - xOld; + m_leastSquaresResidual += diff * diff; + } + + btScalar eps = m_leastSquaresResidualThreshold; + if ((m_leastSquaresResidual < eps) || (k >= (numIterations - 1))) + { +#ifdef VERBOSE_PRINTF_RESIDUAL + printf("totalLenSqr = %f at iteration #%d\n", m_leastSquaresResidual, k); +#endif + break; } } return true; } - }; -#endif //BT_SOLVE_PROJECTED_GAUSS_SEIDEL_H +#endif //BT_SOLVE_PROJECTED_GAUSS_SEIDEL_H |