/************************************************************************* * * * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. * * All rights reserved. Email: russ@q12.org Web: www.q12.org * * * * This library is free software; you can redistribute it and/or * * modify it under the terms of EITHER: * * (1) The GNU Lesser General Public License as published by the Free * * Software Foundation; either version 2.1 of the License, or (at * * your option) any later version. The text of the GNU Lesser * * General Public License is included with this library in the * * file LICENSE.TXT. * * (2) The BSD-style license that is included with this library in * * the file LICENSE-BSD.TXT. * * * * This library is distributed in the hope that it will be useful, * * but WITHOUT ANY WARRANTY; without even the implied warranty of * * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * * LICENSE.TXT and LICENSE-BSD.TXT for more details. * * * *************************************************************************/ /* THE ALGORITHM ------------- solve A*x = b+w, with x and w subject to certain LCP conditions. each x(i),w(i) must lie on one of the three line segments in the following diagram. each line segment corresponds to one index set : w(i) /|\ | : | | : | |i in N : w>0 | |state[i]=0 : | | : | | : i in C w=0 + +-----------------------+ | : | | : | w<0 | : |i in N | : |state[i]=1 | : | | : | +-------|-----------|-----------|----------> x(i) lo 0 hi the Dantzig algorithm proceeds as follows: for i=1:n * if (x(i),w(i)) is not on the line, push x(i) and w(i) positive or negative towards the line. as this is done, the other (x(j),w(j)) for j<i are constrained to be on the line. if any (x,w) reaches the end of a line segment then it is switched between index sets. * i is added to the appropriate index set depending on what line segment it hits. we restrict lo(i) <= 0 and hi(i) >= 0. this makes the algorithm a bit simpler, because the starting point for x(i),w(i) is always on the dotted line x=0 and x will only ever increase in one direction, so it can only hit two out of the three line segments. NOTES ----- this is an implementation of "lcp_dantzig2_ldlt.m" and "lcp_dantzig_lohi.m". the implementation is split into an LCP problem object (btLCP) and an LCP driver function. most optimization occurs in the btLCP object. a naive implementation of the algorithm requires either a lot of data motion or a lot of permutation-array lookup, because we are constantly re-ordering rows and columns. to avoid this and make a more optimized algorithm, a non-trivial data structure is used to represent the matrix A (this is implemented in the fast version of the btLCP object). during execution of this algorithm, some indexes in A are clamped (set C), some are non-clamped (set N), and some are "don't care" (where x=0). A,x,b,w (and other problem vectors) are permuted such that the clamped indexes are first, the unclamped indexes are next, and the don't-care indexes are last. this permutation is recorded in the array `p'. initially p = 0..n-1, and as the rows and columns of A,x,b,w are swapped, the corresponding elements of p are swapped. because the C and N elements are grouped together in the rows of A, we can do lots of work with a fast dot product function. if A,x,etc were not permuted and we only had a permutation array, then those dot products would be much slower as we would have a permutation array lookup in some inner loops. A is accessed through an array of row pointers, so that element (i,j) of the permuted matrix is A[i][j]. this makes row swapping fast. for column swapping we still have to actually move the data. during execution of this algorithm we maintain an L*D*L' factorization of the clamped submatrix of A (call it `AC') which is the top left nC*nC submatrix of A. there are two ways we could arrange the rows/columns in AC. (1) AC is always permuted such that L*D*L' = AC. this causes a problem when a row/column is removed from C, because then all the rows/columns of A between the deleted index and the end of C need to be rotated downward. this results in a lot of data motion and slows things down. (2) L*D*L' is actually a factorization of a *permutation* of AC (which is itself a permutation of the underlying A). this is what we do - the permutation is recorded in the vector C. call this permutation A[C,C]. when a row/column is removed from C, all we have to do is swap two rows/columns and manipulate C. */ #include "btDantzigLCP.h" #include <string.h>//memcpy bool s_error = false; //*************************************************************************** // code generation parameters #define btLCP_FAST // use fast btLCP object // option 1 : matrix row pointers (less data copying) #define BTROWPTRS #define BTATYPE btScalar ** #define BTAROW(i) (m_A[i]) // option 2 : no matrix row pointers (slightly faster inner loops) //#define NOROWPTRS //#define BTATYPE btScalar * //#define BTAROW(i) (m_A+(i)*m_nskip) #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. * B is an n*1 matrix that contains the right hand sides. * B is stored by columns and its leading dimension is also lskip. * B is overwritten with X. * this processes blocks of 2*2. * 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 */ } } /* solve L*X=B, with B containing 2 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. * B is an n*2 matrix that contains the right hand sides. * B is stored by columns and its leading dimension is also lskip. * B is overwritten with X. * this processes blocks of 2*2. * 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 */ } } 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! */ } } /* 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. * B is an n*1 matrix that contains the right hand sides. * B is stored by columns and its leading dimension is also lskip. * B is overwritten with X. * this processes blocks of 4*4. * 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; } } /* solve L^T * x=b, with b containing 1 right hand side. * L is an n*n lower triangular matrix with ones on the diagonal. * L is stored by rows and its leading dimension is lskip. * b is an n*1 matrix that contains the right hand side. * b is overwritten with x. * 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 btVectorScale (btScalar *a, const btScalar *d, int n) { 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) { 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 // A is nskip. this only references and swaps the lower triangle. // if `do_fast_row_swaps' is nonzero and row pointers are being used, then // 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) { 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) { 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. // // two index sets C and N are kept. each set holds a subset of // the variable indexes 0..n-1. an index can only be in one set. // initially both sets are empty. // // the index set C is special: solutions to A(C,C)\A(C,i) can be generated. //*************************************************************************** // fast implementation of btLCP. see the above definition of btLCP for // interface comments. // // `p' records the permutation of A,x,b,w,etc. p is initially 1:n and is // permuted as the other vectors/matrices are permuted. // // A,x,b,w,lo,hi,state,findex,p,c are permuted such that sets C,N have // contiguous indexes. the don't-care indexes follow N. // // an L*D*L' factorization is maintained of A(C,C), and whenever indexes are // added or removed from the set C the factorization is updated. // thus L*D*L'=A[C,C], i.e. a permuted top left nC*nC submatrix of A. // the leading dimension of the matrix L is always `nskip'. // // at the start there may be other indexes that are unbounded but are not // included in `nub'. btLCP will permute the matrix so that absolutely all // unbounded vectors are at the start. thus there may be some initial // permutation. // // the algorithms here assume certain patterns, particularly with respect to // index transfer. #ifdef btLCP_FAST 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 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); 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); 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); 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), #else 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) { { 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 } { 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; const int nub = m_nub; if (nub < n) { for (int k=0; k<100; k++) { int i1,i2; do { i1 = dRandInt(n-nub)+nub; i2 = dRandInt(n-nub)+nub; } while (i1 > i2); //printf ("--> %d %d\n",i1,i2); btSwapProblem (m_A,m_x,m_b,m_w,m_lo,m_hi,m_p,m_state,m_findex,n,i1,i2,m_nskip,0); } } */ // 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++; } } } // print info about indexes /* { const int n = m_n; const int nub = m_nub; for (int k=0; k<n; k++) { if (k<nub) printf ("C"); else if (m_lo[k]==-BT_INFINITY && m_hi[k]==BT_INFINITY) printf ("c"); else printf ("."); } printf ("\n"); } */ } 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); 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) { { 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) { 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; 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); { 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 _GETA(i,j) (A[(i)*nskip+(j)]) #define BTGETA(i,j) ((i > j) ? _BTGETA(i,j) : _BTGETA(j,i)) inline size_t btEstimateLDLTAddTLTmpbufSize(int nskip) { 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) { 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); } } // 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) { { 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) { // 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) { 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) { 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) { 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) { // 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]; } 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]; } } #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) { s_error = false; // 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) lcp.unpermute(); return !s_error; }