--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/ode/src/lcp.cpp Tue Feb 02 01:00:49 2010 +0200
@@ -0,0 +1,1276 @@
+/*************************************************************************
+ * *
+ * 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 (dLCP) and an LCP
+driver function. most optimization occurs in the dLCP 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 dLCP 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 <ode/common.h>
+#include "lcp.h"
+#include <ode/matrix.h>
+#include <ode/misc.h>
+#include "mat.h" // for testing
+#include <ode/timer.h> // for testing
+
+//***************************************************************************
+// code generation parameters
+
+#define dLCP_FAST // use fast dLCP object
+#define dUSE_MALLOC_FOR_ALLOCA
+
+// option 1 : matrix row pointers (less data copying)
+#define ROWPTRS
+#define ATYPE dReal **
+#define AROW(i) (A[i])
+
+// option 2 : no matrix row pointers (slightly faster inner loops)
+//#define NOROWPTRS
+//#define ATYPE dReal *
+//#define AROW(i) (A+(i)*nskip)
+
+// use protected, non-stack memory allocation system
+
+#ifdef dUSE_MALLOC_FOR_ALLOCA
+extern unsigned int dMemoryFlag;
+
+#define ALLOCA(t,v,s) t* v = (t*) malloc(s)
+#define UNALLOCA(t) free(t)
+
+#else
+
+#define ALLOCA(t,v,s) t* v =(t*)dALLOCA16(s)
+#define UNALLOCA(t) /* nothing */
+
+#endif
+
+#define NUB_OPTIMIZATIONS
+
+//***************************************************************************
+
+// 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 swapRowsAndCols (ATYPE A, int n, int i1, int i2, int /*nskip*/,
+ int do_fast_row_swaps)
+{
+ int i;
+
+
+# ifdef ROWPTRS
+ for (i=i1+1; i<i2; i++) A[i1][i] = A[i][i1];
+ for (i=i1+1; i<i2; i++) 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) {
+ dReal *tmpp;
+ tmpp = A[i1];
+ A[i1] = A[i2];
+ A[i2] = tmpp;
+ }
+ else {
+ ALLOCA (dReal,tmprow,n * sizeof(dReal));
+
+#ifdef dUSE_MALLOC_FOR_ALLOCA
+ if (tmprow == NULL) {
+ dMemoryFlag = d_MEMORY_OUT_OF_MEMORY;
+ return;
+ }
+#endif
+
+ memcpy (tmprow,A[i1],n * sizeof(dReal));
+ memcpy (A[i1],A[i2],n * sizeof(dReal));
+ memcpy (A[i2],tmprow,n * sizeof(dReal));
+ UNALLOCA(tmprow);
+ }
+ // swap columns the hard way
+ for (i=i2+1; i<n; i++) {
+ dReal tmp = A[i][i1];
+ A[i][i1] = A[i][i2];
+ A[i][i2] = tmp;
+ }
+# else
+ dReal tmp;
+ ALLOCA (dReal,tmprow,n * sizeof(dReal));
+
+#ifdef dUSE_MALLOC_FOR_ALLOCA
+ if (tmprow == NULL) {
+ return;
+ }
+#endif
+
+ if (i1 > 0) {
+ memcpy (tmprow,A+i1*nskip,i1*sizeof(dReal));
+ memcpy (A+i1*nskip,A+i2*nskip,i1*sizeof(dReal));
+ memcpy (A+i2*nskip,tmprow,i1*sizeof(dReal));
+ }
+ for (i=i1+1; i<i2; i++) {
+ tmp = A[i2*nskip+i];
+ A[i2*nskip+i] = A[i*nskip+i1];
+ A[i*nskip+i1] = tmp;
+ }
+ tmp = A[i1*nskip+i1];
+ A[i1*nskip+i1] = A[i2*nskip+i2];
+ A[i2*nskip+i2] = tmp;
+ for (i=i2+1; i<n; i++) {
+ tmp = A[i*nskip+i1];
+ A[i*nskip+i1] = A[i*nskip+i2];
+ A[i*nskip+i2] = tmp;
+ }
+ UNALLOCA(tmprow);
+# endif
+
+}
+
+
+// swap two indexes in the n*n LCP problem. i1 must be <= i2.
+
+static void swapProblem (ATYPE A, dReal *x, dReal *b, dReal *w, dReal *lo,
+ dReal *hi, int *p, int *state, int *findex,
+ int n, int i1, int i2, int nskip,
+ int do_fast_row_swaps)
+{
+ dReal tmp;
+ int tmpi;
+
+ if (i1==i2) return;
+ swapRowsAndCols (A,n,i1,i2,nskip,do_fast_row_swaps);
+#ifdef dUSE_MALLOC_FOR_ALLOCA
+ if (dMemoryFlag == d_MEMORY_OUT_OF_MEMORY)
+ return;
+#endif
+ tmp = x[i1];
+ x[i1] = x[i2];
+ x[i2] = tmp;
+ tmp = b[i1];
+ b[i1] = b[i2];
+ b[i2] = tmp;
+ tmp = w[i1];
+ w[i1] = w[i2];
+ w[i2] = tmp;
+ tmp = lo[i1];
+ lo[i1] = lo[i2];
+ lo[i2] = tmp;
+ tmp = hi[i1];
+ hi[i1] = hi[i2];
+ hi[i2] = tmp;
+ tmpi = p[i1];
+ p[i1] = p[i2];
+ p[i2] = tmpi;
+ tmpi = state[i1];
+ state[i1] = state[i2];
+ state[i2] = tmpi;
+ if (findex) {
+ tmpi = findex[i1];
+ findex[i1] = findex[i2];
+ findex[i2] = tmpi;
+ }
+}
+
+
+//***************************************************************************
+// dLCP 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 dLCP. see the above definition of dLCP 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'. dLCP 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 dLCP_FAST
+
+struct dLCP {
+ int n,nskip,nub;
+ ATYPE A; // A rows
+ dReal *Adata,*x,*b,*w,*lo,*hi; // permuted LCP problem data
+ dReal *L,*d; // L*D*L' factorization of set C
+ dReal *Dell,*ell,*tmp;
+ int *state,*findex,*p,*C;
+ int nC,nN; // size of each index set
+
+ dLCP (int _n, int _nub, dReal *_Adata, dReal *_x, dReal *_b, dReal *_w,
+ dReal *_lo, dReal *_hi, dReal *_L, dReal *_d,
+ dReal *_Dell, dReal *_ell, dReal *_tmp,
+ int *_state, int *_findex, int *_p, int *_C, dReal **Arows);
+ int getNub() { return nub; }
+ void transfer_i_to_C (int i);
+ void transfer_i_to_N (int /*i*/)
+ { 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);
+ int numC() { return nC; }
+ int numN() { return nN; }
+ int indexC (int i) { return i; }
+ int indexN (int i) { return i+nC; }
+ dReal Aii (int i) { return AROW(i)[i]; }
+ dReal AiC_times_qC (int i, dReal *q) { return dDot (AROW(i),q,nC); }
+ dReal AiN_times_qN (int i, dReal *q) { return dDot (AROW(i)+nC,q+nC,nN); }
+ void pN_equals_ANC_times_qC (dReal *p, dReal *q);
+ void pN_plusequals_ANi (dReal *p, int i, int sign=1);
+ void pC_plusequals_s_times_qC (dReal *p, dReal s, dReal *q)
+ { for (int i=0; i<nC; i++) p[i] += dMUL(s,q[i]); }
+ void pN_plusequals_s_times_qN (dReal *p, dReal s, dReal *q)
+ { for (int i=0; i<nN; i++) p[i+nC] += dMUL(s,q[i+nC]); }
+ void solve1 (dReal *a, int i, int dir=1, int only_transfer=0);
+ void unpermute();
+};
+
+
+dLCP::dLCP (int _n, int _nub, dReal *_Adata, dReal *_x, dReal *_b, dReal *_w,
+ dReal *_lo, dReal *_hi, dReal *_L, dReal *_d,
+ dReal *_Dell, dReal *_ell, dReal *_tmp,
+ int *_state, int *_findex, int *_p, int *_C, dReal **Arows)
+{
+ n = _n;
+ nub = _nub;
+ Adata = _Adata;
+ A = 0;
+ x = _x;
+ b = _b;
+ w = _w;
+ lo = _lo;
+ hi = _hi;
+ L = _L;
+ d = _d;
+ Dell = _Dell;
+ ell = _ell;
+ tmp = _tmp;
+ state = _state;
+ findex = _findex;
+ p = _p;
+ C = _C;
+ nskip = dPAD(n);
+ dSetZero (x,n);
+
+ int k;
+
+# ifdef ROWPTRS
+ // make matrix row pointers
+ A = Arows;
+ for (k=0; k<n; k++) A[k] = Adata + k*nskip;
+# else
+ A = Adata;
+# endif
+
+ nC = 0;
+ nN = 0;
+ for (k=0; k<n; k++) p[k]=k; // initially unpermuted
+
+ /*
+ // for testing, we can do some random swaps in the area i > nub
+ if (nub < n) {
+ for (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);
+ swapProblem (A,x,b,w,lo,hi,p,state,findex,n,i1,i2,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.
+
+ for (k=nub; k<n; k++) {
+ if (findex && findex[k] >= 0) continue;
+ if (lo[k]==-dInfinity && hi[k]==dInfinity) {
+ swapProblem (A,x,b,w,lo,hi,p,state,findex,n,nub,k,nskip,0);
+ 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 (nub > 0) {
+ for (k=0; k<nub; k++) memcpy (L+k*nskip,AROW(k),(k+1)*sizeof(dReal));
+ dFactorLDLT (L,d,nub,nskip);
+ memcpy (x,b,nub*sizeof(dReal));
+ dSolveLDLT (L,d,x,nub,nskip);
+ dSetZero (w,nub);
+ for (k=0; k<nub; k++) C[k] = k;
+ nC = nub;
+ }
+
+ // permute the indexes > nub such that all findex variables are at the end
+ if (findex) {
+ int num_at_end = 0;
+ for (k=n-1; k >= nub; k--) {
+ if (findex[k] >= 0) {
+ swapProblem (A,x,b,w,lo,hi,p,state,findex,n,k,n-1-num_at_end,nskip,1);
+ num_at_end++;
+ }
+ }
+ }
+
+ // print info about indexes
+ /*
+ for (k=0; k<n; k++) {
+ if (k<nub) printf ("C");
+ else if (lo[k]==-dInfinity && hi[k]==dInfinity) printf ("c");
+ else printf (".");
+ }
+ printf ("\n");
+ */
+}
+
+
+void dLCP::transfer_i_to_C (int i)
+{
+ int j;
+ if (nC > 0) {
+ // ell,Dell were computed by solve1(). note, ell = D \ L1solve (L,A(i,C))
+ for (j=0; j<nC; j++) L[nC*nskip+j] = ell[j];
+ d[nC] = dRecip (AROW(i)[i] - dDot(ell,Dell,nC));
+ }
+ else {
+ d[0] = dRecip (AROW(i)[i]);
+ }
+ swapProblem (A,x,b,w,lo,hi,p,state,findex,n,nC,i,nskip,1);
+ C[nC] = nC;
+ nC++;
+
+}
+
+
+void dLCP::transfer_i_from_N_to_C (int i)
+{
+ int j;
+ if (nC > 0) {
+ dReal *aptr = AROW(i);
+# ifdef NUB_OPTIMIZATIONS
+ // if nub>0, initial part of aptr unpermuted
+ for (j=0; j<nub; j++) Dell[j] = aptr[j];
+ for (j=nub; j<nC; j++) Dell[j] = aptr[C[j]];
+# else
+ for (j=0; j<nC; j++) Dell[j] = aptr[C[j]];
+# endif
+ dSolveL1 (L,Dell,nC,nskip);
+ for (j=0; j<nC; j++) ell[j] = dMUL(Dell[j],d[j]);
+ for (j=0; j<nC; j++) L[nC*nskip+j] = ell[j];
+ d[nC] = dRecip (AROW(i)[i] - dDot(ell,Dell,nC));
+ }
+ else {
+ d[0] = dRecip (AROW(i)[i]);
+ }
+ swapProblem (A,x,b,w,lo,hi,p,state,findex,n,nC,i,nskip,1);
+ C[nC] = nC;
+ nN--;
+ nC++;
+
+ // @@@ 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 dLCP::transfer_i_from_C_to_N (int i)
+{
+ // remove a row/column from the factorization, and adjust the
+ // indexes (black magic!)
+ int j,k;
+ for (j=0; j<nC; j++) if (C[j]==i) {
+ dLDLTRemove (A,C,L,d,n,nC,j,nskip);
+ for (k=0; k<nC; k++) if (C[k]==nC-1) {
+ C[k] = C[j];
+ if (j < (nC-1)) memmove (C+j,C+j+1,(nC-j-1)*sizeof(int));
+ break;
+ }
+
+ break;
+ }
+
+ swapProblem (A,x,b,w,lo,hi,p,state,findex,n,i,nC-1,nskip,1);
+ nC--;
+ nN++;
+
+}
+
+
+void dLCP::pN_equals_ANC_times_qC (dReal *p, dReal *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.
+ for (int i=0; i<nN; i++) p[i+nC] = dDot (AROW(i+nC),q,nC);
+}
+
+
+void dLCP::pN_plusequals_ANi (dReal *p, int i, int sign)
+{
+ dReal *aptr = AROW(i)+nC;
+ if (sign > 0) {
+ for (int i=0; i<nN; i++) p[i+nC] += aptr[i];
+ }
+ else {
+ for (int i=0; i<nN; i++) p[i+nC] -= aptr[i];
+ }
+}
+
+
+void dLCP::solve1 (dReal *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.
+
+ int j;
+ if (nC > 0) {
+ dReal *aptr = AROW(i);
+# ifdef NUB_OPTIMIZATIONS
+ // if nub>0, initial part of aptr[] is guaranteed unpermuted
+ for (j=0; j<nub; j++) Dell[j] = aptr[j];
+ for (j=nub; j<nC; j++) Dell[j] = aptr[C[j]];
+# else
+ for (j=0; j<nC; j++) Dell[j] = aptr[C[j]];
+# endif
+ dSolveL1 (L,Dell,nC,nskip);
+ for (j=0; j<nC; j++) ell[j] = dMUL(Dell[j],d[j]);
+
+ if (!only_transfer) {
+ for (j=0; j<nC; j++) tmp[j] = ell[j];
+ dSolveL1T (L,tmp,nC,nskip);
+ if (dir > 0) {
+ for (j=0; j<nC; j++) a[C[j]] = -tmp[j];
+ }
+ else {
+ for (j=0; j<nC; j++) a[C[j]] = tmp[j];
+ }
+ }
+ }
+}
+
+
+void dLCP::unpermute()
+{
+ // now we have to un-permute x and w
+ int j;
+ ALLOCA (dReal,tmp,n*sizeof(dReal));
+#ifdef dUSE_MALLOC_FOR_ALLOCA
+ if (tmp == NULL) {
+ dMemoryFlag = d_MEMORY_OUT_OF_MEMORY;
+ return;
+ }
+#endif
+ memcpy (tmp,x,n*sizeof(dReal));
+ for (j=0; j<n; j++) x[p[j]] = tmp[j];
+ memcpy (tmp,w,n*sizeof(dReal));
+ for (j=0; j<n; j++) w[p[j]] = tmp[j];
+
+ UNALLOCA (tmp);
+}
+
+#endif // dLCP_FAST
+
+//***************************************************************************
+// an unoptimized Dantzig LCP driver routine for the basic LCP problem.
+// must have lo=0, hi=dInfinity, and nub=0.
+
+void dSolveLCPBasic (int n, dReal *A, dReal *x, dReal *b,
+ dReal *w, int /*nub*/, dReal */*lo*/, dReal */*hi*/)
+{
+ int i,k;
+ int nskip = dPAD(n);
+ ALLOCA (dReal,L,n*nskip*sizeof(dReal));
+#ifdef dUSE_MALLOC_FOR_ALLOCA
+ if (L == NULL) {
+ dMemoryFlag = d_MEMORY_OUT_OF_MEMORY;
+ return;
+ }
+#endif
+ ALLOCA (dReal,d,n*sizeof(dReal));
+#ifdef dUSE_MALLOC_FOR_ALLOCA
+ if (d == NULL) {
+ UNALLOCA(L);
+ dMemoryFlag = d_MEMORY_OUT_OF_MEMORY;
+ return;
+ }
+#endif
+ ALLOCA (dReal,delta_x,n*sizeof(dReal));
+#ifdef dUSE_MALLOC_FOR_ALLOCA
+ if (delta_x == NULL) {
+ UNALLOCA(d);
+ UNALLOCA(L);
+ dMemoryFlag = d_MEMORY_OUT_OF_MEMORY;
+ return;
+ }
+#endif
+ ALLOCA (dReal,delta_w,n*sizeof(dReal));
+#ifdef dUSE_MALLOC_FOR_ALLOCA
+ if (delta_w == NULL) {
+ UNALLOCA(delta_x);
+ UNALLOCA(d);
+ UNALLOCA(L);
+ dMemoryFlag = d_MEMORY_OUT_OF_MEMORY;
+ return;
+ }
+#endif
+ ALLOCA (dReal,Dell,n*sizeof(dReal));
+#ifdef dUSE_MALLOC_FOR_ALLOCA
+ if (Dell == NULL) {
+ UNALLOCA(delta_w);
+ UNALLOCA(delta_x);
+ UNALLOCA(d);
+ UNALLOCA(L);
+ dMemoryFlag = d_MEMORY_OUT_OF_MEMORY;
+ return;
+ }
+#endif
+ ALLOCA (dReal,ell,n*sizeof(dReal));
+#ifdef dUSE_MALLOC_FOR_ALLOCA
+ if (ell == NULL) {
+ UNALLOCA(Dell);
+ UNALLOCA(delta_w);
+ UNALLOCA(delta_x);
+ UNALLOCA(d);
+ UNALLOCA(L);
+ dMemoryFlag = d_MEMORY_OUT_OF_MEMORY;
+ return;
+ }
+#endif
+ ALLOCA (dReal,tmp,n*sizeof(dReal));
+#ifdef dUSE_MALLOC_FOR_ALLOCA
+ if (tmp == NULL) {
+ UNALLOCA(ell);
+ UNALLOCA(Dell);
+ UNALLOCA(delta_w);
+ UNALLOCA(delta_x);
+ UNALLOCA(d);
+ UNALLOCA(L);
+ dMemoryFlag = d_MEMORY_OUT_OF_MEMORY;
+ return;
+ }
+#endif
+ ALLOCA (dReal*,Arows,n*sizeof(dReal*));
+#ifdef dUSE_MALLOC_FOR_ALLOCA
+ if (Arows == NULL) {
+ UNALLOCA(tmp);
+ UNALLOCA(ell);
+ UNALLOCA(Dell);
+ UNALLOCA(delta_w);
+ UNALLOCA(delta_x);
+ UNALLOCA(d);
+ UNALLOCA(L);
+ dMemoryFlag = d_MEMORY_OUT_OF_MEMORY;
+ return;
+ }
+#endif
+ ALLOCA (int,p,n*sizeof(int));
+#ifdef dUSE_MALLOC_FOR_ALLOCA
+ if (p == NULL) {
+ UNALLOCA(Arows);
+ UNALLOCA(tmp);
+ UNALLOCA(ell);
+ UNALLOCA(Dell);
+ UNALLOCA(delta_w);
+ UNALLOCA(delta_x);
+ UNALLOCA(d);
+ UNALLOCA(L);
+ dMemoryFlag = d_MEMORY_OUT_OF_MEMORY;
+ return;
+ }
+#endif
+ ALLOCA (int,C,n*sizeof(int));
+#ifdef dUSE_MALLOC_FOR_ALLOCA
+ if (C == NULL) {
+ UNALLOCA(p);
+ UNALLOCA(Arows);
+ UNALLOCA(tmp);
+ UNALLOCA(ell);
+ UNALLOCA(Dell);
+ UNALLOCA(delta_w);
+ UNALLOCA(delta_x);
+ UNALLOCA(d);
+ UNALLOCA(L);
+ dMemoryFlag = d_MEMORY_OUT_OF_MEMORY;
+ return;
+ }
+#endif
+ ALLOCA (int,dummy,n*sizeof(int));
+#ifdef dUSE_MALLOC_FOR_ALLOCA
+ if (dummy == NULL) {
+ UNALLOCA(C);
+ UNALLOCA(p);
+ UNALLOCA(Arows);
+ UNALLOCA(tmp);
+ UNALLOCA(ell);
+ UNALLOCA(Dell);
+ UNALLOCA(delta_w);
+ UNALLOCA(delta_x);
+ UNALLOCA(d);
+ UNALLOCA(L);
+ dMemoryFlag = d_MEMORY_OUT_OF_MEMORY;
+ return;
+ }
+#endif
+
+
+ dLCP lcp (n,0,A,x,b,w,tmp,tmp,L,d,Dell,ell,tmp,dummy,dummy,p,C,Arows);
+ lcp.getNub();
+
+ for (i=0; i<n; i++) {
+ w[i] = lcp.AiC_times_qC (i,x) - b[i];
+ if (w[i] >= 0) {
+ lcp.transfer_i_to_N (i);
+ }
+ else {
+ for (;;) {
+ // compute: delta_x(C) = -A(C,C)\A(C,i)
+ dSetZero (delta_x,n);
+ lcp.solve1 (delta_x,i);
+#ifdef dUSE_MALLOC_FOR_ALLOCA
+ if (dMemoryFlag == d_MEMORY_OUT_OF_MEMORY) {
+ UNALLOCA(dummy);
+ UNALLOCA(C);
+ UNALLOCA(p);
+ UNALLOCA(Arows);
+ UNALLOCA(tmp);
+ UNALLOCA(ell);
+ UNALLOCA(Dell);
+ UNALLOCA(delta_w);
+ UNALLOCA(delta_x);
+ UNALLOCA(d);
+ UNALLOCA(L);
+ return;
+ }
+#endif
+ delta_x[i] = REAL(1.0);
+
+ // compute: delta_w = A*delta_x
+ dSetZero (delta_w,n);
+ lcp.pN_equals_ANC_times_qC (delta_w,delta_x);
+ lcp.pN_plusequals_ANi (delta_w,i);
+ delta_w[i] = lcp.AiC_times_qC (i,delta_x) + lcp.Aii(i);
+
+ // find index to switch
+ int si = i; // si = switch index
+ int si_in_N = 0; // set to 1 if si in N
+ dReal s = -dDIV(w[i],delta_w[i]);
+
+ if (s <= 0) {
+ if (i < (n-1)) {
+ dSetZero (x+i,n-i);
+ dSetZero (w+i,n-i);
+ }
+ goto done;
+ }
+
+ for (k=0; k < lcp.numN(); k++) {
+ if (delta_w[lcp.indexN(k)] < 0) {
+ dReal s2 = -dDIV(w[lcp.indexN(k)],delta_w[lcp.indexN(k)]);
+ if (s2 < s) {
+ s = s2;
+ si = lcp.indexN(k);
+ si_in_N = 1;
+ }
+ }
+ }
+ for (k=0; k < lcp.numC(); k++) {
+ if (delta_x[lcp.indexC(k)] < 0) {
+ dReal s2 = -dDIV(x[lcp.indexC(k)],delta_x[lcp.indexC(k)]);
+ if (s2 < s) {
+ s = s2;
+ si = lcp.indexC(k);
+ si_in_N = 0;
+ }
+ }
+ }
+
+ // apply x = x + s * delta_x
+ lcp.pC_plusequals_s_times_qC (x,s,delta_x);
+ x[i] += s;
+ lcp.pN_plusequals_s_times_qN (w,s,delta_w);
+ w[i] += dMUL(s,delta_w[i]);
+
+ // switch indexes between sets if necessary
+ if (si==i) {
+ w[i] = 0;
+ lcp.transfer_i_to_C (i);
+ break;
+ }
+ if (si_in_N) {
+ w[si] = 0;
+ lcp.transfer_i_from_N_to_C (si);
+ }
+ else {
+ x[si] = 0;
+ lcp.transfer_i_from_C_to_N (si);
+ }
+ }
+ }
+ }
+
+ done:
+ lcp.unpermute();
+
+ UNALLOCA (L);
+ UNALLOCA (d);
+ UNALLOCA (delta_x);
+ UNALLOCA (delta_w);
+ UNALLOCA (Dell);
+ UNALLOCA (ell);
+ UNALLOCA (tmp);
+ UNALLOCA (Arows);
+ UNALLOCA (p);
+ UNALLOCA (C);
+ UNALLOCA (dummy);
+}
+
+//***************************************************************************
+// an optimized Dantzig LCP driver routine for the lo-hi LCP problem.
+
+void dSolveLCP (int n, dReal *A, dReal *x, dReal *b,
+ dReal *w, int nub, dReal *lo, dReal *hi, int *findex)
+{
+
+ int i,k,hit_first_friction_index = 0;
+ int nskip = dPAD(n);
+
+ // if all the variables are unbounded then we can just factor, solve,
+ // and return
+ if (nub >= n) {
+ dFactorLDLT (A,w,n,nskip); // use w for d
+ dSolveLDLT (A,w,b,n,nskip);
+ memcpy (x,b,n*sizeof(dReal));
+ dSetZero (w,n);
+
+ return;
+ }
+ ALLOCA (dReal,L,n*nskip*sizeof(dReal));
+#ifdef dUSE_MALLOC_FOR_ALLOCA
+ if (L == NULL) {
+ dMemoryFlag = d_MEMORY_OUT_OF_MEMORY;
+ return;
+ }
+#endif
+ ALLOCA (dReal,d,n*sizeof(dReal));
+#ifdef dUSE_MALLOC_FOR_ALLOCA
+ if (d == NULL) {
+ UNALLOCA(L);
+ dMemoryFlag = d_MEMORY_OUT_OF_MEMORY;
+ return;
+ }
+#endif
+ ALLOCA (dReal,delta_x,n*sizeof(dReal));
+#ifdef dUSE_MALLOC_FOR_ALLOCA
+ if (delta_x == NULL) {
+ UNALLOCA(d);
+ UNALLOCA(L);
+ dMemoryFlag = d_MEMORY_OUT_OF_MEMORY;
+ return;
+ }
+#endif
+ ALLOCA (dReal,delta_w,n*sizeof(dReal));
+#ifdef dUSE_MALLOC_FOR_ALLOCA
+ if (delta_w == NULL) {
+ UNALLOCA(delta_x);
+ UNALLOCA(d);
+ UNALLOCA(L);
+ dMemoryFlag = d_MEMORY_OUT_OF_MEMORY;
+ return;
+ }
+#endif
+ ALLOCA (dReal,Dell,n*sizeof(dReal));
+#ifdef dUSE_MALLOC_FOR_ALLOCA
+ if (Dell == NULL) {
+ UNALLOCA(delta_w);
+ UNALLOCA(delta_x);
+ UNALLOCA(d);
+ UNALLOCA(L);
+ dMemoryFlag = d_MEMORY_OUT_OF_MEMORY;
+ return;
+ }
+#endif
+ ALLOCA (dReal,ell,n*sizeof(dReal));
+#ifdef dUSE_MALLOC_FOR_ALLOCA
+ if (ell == NULL) {
+ UNALLOCA(Dell);
+ UNALLOCA(delta_w);
+ UNALLOCA(delta_x);
+ UNALLOCA(d);
+ UNALLOCA(L);
+ dMemoryFlag = d_MEMORY_OUT_OF_MEMORY;
+ return;
+ }
+#endif
+ ALLOCA (dReal*,Arows,n*sizeof(dReal*));
+#ifdef dUSE_MALLOC_FOR_ALLOCA
+ if (Arows == NULL) {
+ UNALLOCA(ell);
+ UNALLOCA(Dell);
+ UNALLOCA(delta_w);
+ UNALLOCA(delta_x);
+ UNALLOCA(d);
+ UNALLOCA(L);
+ dMemoryFlag = d_MEMORY_OUT_OF_MEMORY;
+ return;
+ }
+#endif
+ ALLOCA (int,p,n*sizeof(int));
+#ifdef dUSE_MALLOC_FOR_ALLOCA
+ if (p == NULL) {
+ UNALLOCA(Arows);
+ UNALLOCA(ell);
+ UNALLOCA(Dell);
+ UNALLOCA(delta_w);
+ UNALLOCA(delta_x);
+ UNALLOCA(d);
+ UNALLOCA(L);
+ dMemoryFlag = d_MEMORY_OUT_OF_MEMORY;
+ return;
+ }
+#endif
+ ALLOCA (int,C,n*sizeof(int));
+#ifdef dUSE_MALLOC_FOR_ALLOCA
+ if (C == NULL) {
+ UNALLOCA(p);
+ UNALLOCA(Arows);
+ UNALLOCA(ell);
+ UNALLOCA(Dell);
+ UNALLOCA(delta_w);
+ UNALLOCA(delta_x);
+ UNALLOCA(d);
+ UNALLOCA(L);
+ dMemoryFlag = d_MEMORY_OUT_OF_MEMORY;
+ return;
+ }
+#endif
+
+ int dir;
+ dReal dirf;
+
+ // for i in N, state[i] is 0 if x(i)==lo(i) or 1 if x(i)==hi(i)
+ ALLOCA (int,state,n*sizeof(int));
+#ifdef dUSE_MALLOC_FOR_ALLOCA
+ if (state == NULL) {
+ UNALLOCA(C);
+ UNALLOCA(p);
+ UNALLOCA(Arows);
+ UNALLOCA(ell);
+ UNALLOCA(Dell);
+ UNALLOCA(delta_w);
+ UNALLOCA(delta_x);
+ UNALLOCA(d);
+ UNALLOCA(L);
+ dMemoryFlag = d_MEMORY_OUT_OF_MEMORY;
+ return;
+ }
+#endif
+
+ // 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!
+ dLCP *lcp=new dLCP(n,nub,A,x,b,w,lo,hi,L,d,Dell,ell,delta_w,state,findex,p,C,Arows);
+ nub = lcp->getNub();
+
+ // loop over all indexes 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.
+
+ for (i=nub; i<n; i++) {
+ // 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 == 0 && findex && findex[i] >= 0) {
+ // un-permute x into delta_w, which is not being used at the moment
+ for (k=0; k<n; k++) delta_w[p[k]] = x[k];
+
+ // set lo and hi values
+ for (k=i; k<n; k++) {
+ dReal wfk = delta_w[findex[k]];
+ if (wfk == 0) {
+ hi[k] = 0;
+ lo[k] = 0;
+ }
+ else {
+ hi[k] = dFabs (dMUL(hi[k],wfk));
+ lo[k] = -hi[k];
+ }
+ }
+ hit_first_friction_index = 1;
+ }
+
+ // 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);
+ state[i] = 0;
+ }
+ else if (hi[i]==0 && w[i] <= 0) {
+ lcp->transfer_i_to_N (i);
+ state[i] = 1;
+ }
+ 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 (delta_x,i,0,1);
+
+#ifdef dUSE_MALLOC_FOR_ALLOCA
+ if (dMemoryFlag == d_MEMORY_OUT_OF_MEMORY) {
+ UNALLOCA(state);
+ UNALLOCA(C);
+ UNALLOCA(p);
+ UNALLOCA(Arows);
+ UNALLOCA(ell);
+ UNALLOCA(Dell);
+ UNALLOCA(delta_w);
+ UNALLOCA(delta_x);
+ UNALLOCA(d);
+ UNALLOCA(L);
+ return;
+ }
+#endif
+
+ lcp->transfer_i_to_C (i);
+ }
+ else {
+ // we must push x(i) and w(i)
+ for (;;) {
+ // find direction to push on x(i)
+ if (w[i] <= 0) {
+ dir = 1;
+ dirf = REAL(1.0);
+ }
+ else {
+ dir = -1;
+ dirf = REAL(-1.0);
+ }
+
+ // compute: delta_x(C) = -dir*A(C,C)\A(C,i)
+ lcp->solve1 (delta_x,i,dir);
+
+#ifdef dUSE_MALLOC_FOR_ALLOCA
+ if (dMemoryFlag == d_MEMORY_OUT_OF_MEMORY) {
+ UNALLOCA(state);
+ UNALLOCA(C);
+ UNALLOCA(p);
+ UNALLOCA(Arows);
+ UNALLOCA(ell);
+ UNALLOCA(Dell);
+ UNALLOCA(delta_w);
+ UNALLOCA(delta_x);
+ UNALLOCA(d);
+ UNALLOCA(L);
+ return;
+ }
+#endif
+
+ // 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 (delta_w,delta_x);
+ lcp->pN_plusequals_ANi (delta_w,i,dir);
+ delta_w[i] = lcp->AiC_times_qC (i,delta_x) + dMUL(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
+ dReal s = -dDIV(w[i],delta_w[i]);
+ if (dir > 0) {
+ if (hi[i] < dInfinity) {
+ dReal s2 = dDIV((hi[i]-x[i]),dirf); // step to x(i)=hi(i)
+ if (s2 < s) {
+ s = s2;
+ cmd = 3;
+ }
+ }
+ }
+ else {
+ if (lo[i] > -dInfinity) {
+ dReal s2 = dDIV((lo[i]-x[i]),dirf); // step to x(i)=lo(i)
+ if (s2 < s) {
+ s = s2;
+ cmd = 2;
+ }
+ }
+ }
+
+ for (k=0; k < lcp->numN(); k++) {
+ if ((state[lcp->indexN(k)]==0 && delta_w[lcp->indexN(k)] < 0) ||
+ (state[lcp->indexN(k)]!=0 && delta_w[lcp->indexN(k)] > 0)) {
+ // don't bother checking if lo=hi=0
+ if (lo[lcp->indexN(k)] == 0 && hi[lcp->indexN(k)] == 0) continue;
+ dReal s2 = -dDIV(w[lcp->indexN(k)],delta_w[lcp->indexN(k)]);
+ if (s2 < s) {
+ s = s2;
+ cmd = 4;
+ si = lcp->indexN(k);
+ }
+ }
+ }
+
+ for (k=nub; k < lcp->numC(); k++) {
+ if (delta_x[lcp->indexC(k)] < 0 && lo[lcp->indexC(k)] > -dInfinity) {
+ dReal s2 = dDIV((lo[lcp->indexC(k)]-x[lcp->indexC(k)]),delta_x[lcp->indexC(k)]);
+ if (s2 < s) {
+ s = s2;
+ cmd = 5;
+ si = lcp->indexC(k);
+ }
+ }
+ if (delta_x[lcp->indexC(k)] > 0 && hi[lcp->indexC(k)] < dInfinity) {
+ dReal s2 = dDIV((hi[lcp->indexC(k)]-x[lcp->indexC(k)]),delta_x[lcp->indexC(k)]);
+ if (s2 < s) {
+ s = s2;
+ cmd = 6;
+ si = lcp->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 <= 0) {
+ if (i < (n-1)) {
+ dSetZero (x+i,n-i);
+ dSetZero (w+i,n-i);
+ }
+ goto done;
+ }
+
+ // apply x = x + s * delta_x
+ lcp->pC_plusequals_s_times_qC (x,s,delta_x);
+ x[i] += dMUL(s,dirf);
+
+ // apply w = w + s * delta_w
+ lcp->pN_plusequals_s_times_qN (w,s,delta_w);
+ w[i] += dMUL(s,delta_w[i]);
+
+ // 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];
+ state[i] = 0;
+ lcp->transfer_i_to_N (i);
+ break;
+ case 3: // done
+ x[i] = hi[i];
+ state[i] = 1;
+ 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];
+ state[si] = 0;
+ lcp->transfer_i_from_C_to_N (si);
+ break;
+ case 6: // keep going
+ x[si] = hi[si];
+ state[si] = 1;
+ lcp->transfer_i_from_C_to_N (si);
+ break;
+ }
+
+ if (cmd <= 3) break;
+ }
+ }
+ }
+
+ done:
+ lcp->unpermute();
+ delete lcp;
+
+ UNALLOCA (L);
+ UNALLOCA (d);
+ UNALLOCA (delta_x);
+ UNALLOCA (delta_w);
+ UNALLOCA (Dell);
+ UNALLOCA (ell);
+ UNALLOCA (Arows);
+ UNALLOCA (p);
+ UNALLOCA (C);
+ UNALLOCA (state);
+}