void dLCP::solve1 (dReal *a, int i, int dir, int only_transfer)
{
dReal *AA = (dReal*) ALLOCA (n*nskip*sizeof(dReal));
dReal *dd = (dReal*) ALLOCA (n*sizeof(dReal));
dReal *bb = (dReal*) ALLOCA (n*sizeof(dReal));
int ii,jj,AAi,AAj;
last_i_for_solve1 = i;
AAi = 0;
for (ii=0; ii<n; ii++) if (C[ii]) {
AAj = 0;
for (jj=0; jj<n; jj++) if (C[jj]) {
AA[AAi*nskip+AAj] = AROW(ii)[jj];
AAj++;
}
bb[AAi] = AROW(i)[ii];
AAi++;
}
if (AAi==0) return;
dFactorLDLT (AA,dd,AAi,nskip);
dSolveLDLT (AA,dd,bb,AAi,nskip);
AAi=0;
if (dir > 0) {
for (ii=0; ii<n; ii++) if (C[ii]) a[ii] = -bb[AAi++];
}
else {
for (ii=0; ii<n; ii++) if (C[ii]) a[ii] = bb[AAi++];
}
}
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)
{
dUASSERT (_findex==0,"slow dLCP object does not support findex array");
n = _n;
nub = _nub;
Adata = _Adata;
A = 0;
x = _x;
b = _b;
w = _w;
lo = _lo;
hi = _hi;
nskip = dPAD(n);
dSetZero (x,n);
last_i_for_solve1 = -1;
int i,j;
C.setSize (n);
N.setSize (n);
for (int i=0; i<n; i++) {
C[i] = 0;
N[i] = 0;
}
# ifdef ROWPTRS
// make matrix row pointers
A = Arows;
for (i=0; i<n; i++) A[i] = Adata + i*nskip;
# else
A = Adata;
# endif
// lets make A symmetric
for (i=0; i<n; i++) {
for (j=i+1; j<n; j++) AROW(i)[j] = AROW(j)[i];
}
// if nub>0, put all indexes 0..nub-1 into C and solve for x
if (nub > 0) {
for (i=0; i<nub; i++) memcpy (_L+i*nskip,AROW(i),(i+1)*sizeof(dReal));
dFactorLDLT (_L,_d,nub,nskip);
memcpy (x,b,nub*sizeof(dReal));
dSolveLDLT (_L,_d,x,nub,nskip);
dSetZero (_w,nub);
for (i=0; i<nub; i++) C[i] = 1;
}
}
void testSolveLDLT()
{
dReal A[MSIZE4*MSIZE], L[MSIZE4*MSIZE], d[MSIZE], x[MSIZE],
b[MSIZE], btest[MSIZE], diff;
HEADER;
dMakeRandomMatrix (A,MSIZE,MSIZE,1.0);
dMultiply2 (L,A,A,MSIZE,MSIZE,MSIZE);
memcpy (A,L,MSIZE4*MSIZE*sizeof(dReal));
dMakeRandomMatrix (b,MSIZE,1,1.0);
memcpy (x,b,MSIZE*sizeof(dReal));
dFactorLDLT (L,d,MSIZE,MSIZE4);
dSolveLDLT (L,d,x,MSIZE,MSIZE4);
dMultiply2 (btest,A,x,MSIZE,MSIZE,1);
diff = dMaxDifference(b,btest,MSIZE,1);
printf ("\tmaximum difference = %.6e - %s\n",diff,
diff > tol ? "FAILED" : "passed");
}
bool dSolveLCP (int n, dReal *A, dReal *x, dReal *b,
dReal *outer_w/*=nullptr*/, int nub, dReal *lo, dReal *hi, int *findex, bool earlyTermination)
{
dAASSERT (n>0 && A && x && b && lo && hi && nub >= 0 && nub <= n);
# ifndef dNODEBUG
{
// check restrictions on lo and hi
for (int k=0; k<n; ++k) dIASSERT (lo[k] <= 0 && hi[k] >= 0);
}
# endif
// if all the variables are unbounded then we can just factor, solve,
// and return
if (nub >= n) {
dReal *d = new dReal[n];
dSetZero (d, n);
int nskip = dPAD(n);
dFactorLDLT (A, d, n, nskip);
dSolveLDLT (A, d, b, n, nskip);
memcpy (x, b, n*sizeof(dReal));
delete[] d;
return true;
}
const int nskip = dPAD(n);
dReal *L = new dReal[ (n*nskip)];
dReal *d = new dReal[ (n)];
dReal *w = outer_w ? outer_w : (new dReal[n]);
dReal *delta_w = new dReal[ (n)];
dReal *delta_x = new dReal[ (n)];
dReal *Dell = new dReal[ (n)];
dReal *ell = new dReal[ (n)];
#ifdef ROWPTRS
dReal **Arows = new dReal* [n];
#else
dReal **Arows = nullptr;
#endif
int *p = new int[n];
int *C = new int[n];
// for i in N, state[i] is 0 if x(i)==lo(i) or 1 if x(i)==hi(i)
bool *state = new bool[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!
dLCP lcp(n,nskip,nub,A,x,b,w,lo,hi,L,d,Dell,ell,delta_w,state,findex,p,C,Arows);
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) {
bool 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) delta_w[p[j]] = x[j];
// set lo and hi values
for (int k=i; k<n; ++k) {
dReal wfk = delta_w[findex[k]];
if (wfk == 0) {
hi[k] = 0;
lo[k] = 0;
}
else {
hi[k] = dFabs (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);
state[i] = false;
}
else if (hi[i]==0 && w[i] <= 0) {
lcp.transfer_i_to_N (i);
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 (delta_x,i,0,1);
lcp.transfer_i_to_C (i);
}
else {
// we must push x(i) and w(i)
for (;;) {
int dir;
dReal dirf;
// 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);
// 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) + 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 = -w[i]/delta_w[i];
if (dir > 0) {
if (hi[i] < dInfinity) {
dReal 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] > -dInfinity) {
dReal 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 (!state[indexN_k] ? delta_w[indexN_k] < 0 : delta_w[indexN_k] > 0) {
// don't bother checking if lo=hi=0
if (lo[indexN_k] == 0 && hi[indexN_k] == 0) continue;
dReal s2 = -w[indexN_k] / 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 (delta_x[indexC_k] < 0 && lo[indexC_k] > -dInfinity) {
dReal s2 = (lo[indexC_k]-x[indexC_k]) / delta_x[indexC_k];
if (s2 < s) {
s = s2;
cmd = 5;
si = indexC_k;
}
}
if (delta_x[indexC_k] > 0 && hi[indexC_k] < dInfinity) {
dReal s2 = (hi[indexC_k]-x[indexC_k]) / 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 <= REAL(0.0)) {
if (earlyTermination) {
if (!outer_w)
delete[] w;
delete[] L;
delete[] d;
delete[] delta_w;
delete[] delta_x;
delete[] Dell;
delete[] ell;
#ifdef ROWPTRS
delete[] Arows;
#endif
delete[] p;
delete[] C;
delete[] state;
return false;
}
// We shouldn't be overly aggressive about printing this warning,
// because sometimes it gets spammed if s is just a tiny bit beneath
// 0.0.
if (s < REAL(-1e-6)) {
dMessage (d_ERR_LCP, "LCP internal error, s <= 0 (s=%.4e)",
(double)s);
}
if (i < n) {
dSetZero (x+i,n-i);
dSetZero (w+i,n-i);
}
s_error = true;
break;
}
// apply x = x + s * delta_x
lcp.pC_plusequals_s_times_qC (x, s, delta_x);
x[i] += s * dirf;
// apply w = w + s * delta_w
lcp.pN_plusequals_s_times_qN (w, s, delta_w);
w[i] += s * 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];
state[i] = false;
lcp.transfer_i_to_N (i);
break;
case 3: // done
x[i] = hi[i];
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];
state[si] = false;
lcp.transfer_i_from_C_to_N (si, nullptr);
break;
case 6: // keep going
x[si] = hi[si];
state[si] = true;
lcp.transfer_i_from_C_to_N (si, nullptr);
break;
}
if (cmd <= 3) break;
} // for (;;)
} // else
if (s_error) {
break;
}
} // for (int i=adj_nub; i<n; ++i)
lcp.unpermute();
if (!outer_w)
delete[] w;
delete[] L;
delete[] d;
delete[] delta_w;
delete[] delta_x;
delete[] Dell;
delete[] ell;
#ifdef ROWPTRS
delete[] Arows;
#endif
delete[] p;
delete[] C;
delete[] state;
return true;
}
dLCP::dLCP (int _n, int _nskip, int _nub, dReal *_Adata, dReal *_x, dReal *_b, dReal *_w,
dReal *_lo, dReal *_hi, dReal *_L, dReal *_d,
dReal *_Dell, dReal *_ell, dReal *_tmp,
bool *_state, int *_findex, int *_p, int *_C, dReal **Arows):
m_n(_n), m_nskip(_nskip), m_nub(_nub), m_nC(0), m_nN(0),
# ifdef ROWPTRS
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)
{
{
dSetZero (m_x,m_n);
}
{
# ifdef ROWPTRS
// make matrix row pointers
dReal *aptr = _Adata;
ATYPE 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);
swapProblem (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;
dReal *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]==-dInfinity && hi[k]==dInfinity) {
swapProblem (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;
{
dReal *Lrow = m_L;
const int nskip = m_nskip;
for (int j=0; j<nub; Lrow+=nskip, ++j) memcpy(Lrow,AROW(j),(j+1)*sizeof(dReal));
}
dFactorLDLT (m_L,m_d,nub,m_nskip);
memcpy (m_x,m_b,nub*sizeof(dReal));
dSolveLDLT (m_L,m_d,m_x,nub,m_nskip);
dSetZero (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) {
swapProblem (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]==-dInfinity && m_hi[k]==dInfinity) printf ("c");
else printf (".");
}
printf ("\n");
}
*/
}
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::solve1 (dReal *a, int i, int dir, int only_transfer)
{
ALLOCA (dReal,AA,n*nskip*sizeof(dReal));
#ifdef dUSE_MALLOC_FOR_ALLOCA
if (AA == NULL) {
dMemoryFlag = d_MEMORY_OUT_OF_MEMORY;
return;
}
#endif
ALLOCA (dReal,dd,n*sizeof(dReal));
#ifdef dUSE_MALLOC_FOR_ALLOCA
if (dd == NULL) {
UNALLOCA(AA);
dMemoryFlag = d_MEMORY_OUT_OF_MEMORY;
return;
}
#endif
ALLOCA (dReal,bb,n*sizeof(dReal));
#ifdef dUSE_MALLOC_FOR_ALLOCA
if (bb == NULL) {
UNALLOCA(AA);
UNALLOCA(dd);
dMemoryFlag = d_MEMORY_OUT_OF_MEMORY;
return;
}
#endif
int ii,jj,AAi,AAj;
last_i_for_solve1 = i;
AAi = 0;
for (ii=0; ii<n; ii++) if (C[ii]) {
AAj = 0;
for (jj=0; jj<n; jj++) if (C[jj]) {
AA[AAi*nskip+AAj] = AROW(ii)[jj];
AAj++;
}
bb[AAi] = AROW(i)[ii];
AAi++;
}
if (AAi==0) {
UNALLOCA (AA);
UNALLOCA (dd);
UNALLOCA (bb);
return;
}
dFactorLDLT (AA,dd,AAi,nskip);
dSolveLDLT (AA,dd,bb,AAi,nskip);
AAi=0;
if (dir > 0) {
for (ii=0; ii<n; ii++) if (C[ii]) a[ii] = -bb[AAi++];
}
else {
for (ii=0; ii<n; ii++) if (C[ii]) a[ii] = bb[AAi++];
}
UNALLOCA (AA);
UNALLOCA (dd);
UNALLOCA (bb);
}
void dSolveLCP (int n, dReal *A, dReal *x, dReal *b,
dReal *w, int nub, dReal *lo, dReal *hi, int *findex)
{
dAASSERT (n>0 && A && x && b && w && lo && hi && nub >= 0 && nub <= n);
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;
}
# ifndef dNODEBUG
// check restrictions on lo and hi
for (k=0; k<n; k++) dIASSERT (lo[k] <= 0 && hi[k] >= 0);
# endif
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 (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) + 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 = -w[i]/delta_w[i];
if (dir > 0) {
if (hi[i] < dInfinity) {
dReal s2 = (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 = (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 = -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 = (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 = (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) {
dMessage (d_ERR_LCP, "LCP internal error, s <= 0 (s=%.4e)",s);
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] += s * dirf;
// apply w = w + s * delta_w
lcp->pN_plusequals_s_times_qN (w,s,delta_w);
w[i] += 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);
}