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] = 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.
# ifdef DEBUG_LCP
checkFactorization (A,L,d,nC,C,nskip);
# endif
}
void dLCP::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;
dReal *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] = dRecip (AROW(i)[i] - dDot(m_ell,m_Dell,nC));
}
else {
m_d[0] = dRecip (AROW(i)[i]);
}
swapProblem (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
}
# ifdef DEBUG_LCP
checkFactorization (m_A,m_L,m_d,m_nC,m_C,m_nskip);
if (i < (m_n-1)) checkPermutations (i+1,m_n,m_nC,m_nN,m_p,m_C);
# endif
}
void dLCP::transfer_i_from_N_to_C (int i)
{
{
if (m_nC > 0) {
{
dReal *const aptr = AROW(i);
dReal *Dell = m_Dell;
const int *C = m_C;
# ifdef NUB_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
}
dSolveL1 (m_L,m_Dell,m_nC,m_nskip);
{
const int nC = m_nC;
dReal *const Ltgt = m_L + nC*m_nskip;
dReal *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;
dReal Aii_dDot = AROW(i)[i] - dDot(m_ell, m_Dell, nC);
if(dFabs(Aii_dDot) < 1e-16) {
Aii_dDot += 1e-6;
}
m_d[nC] = dRecip (Aii_dDot);
}
else {
if(dFabs(AROW(i)[i]) < 1e-16) {
AROW(i)[i] += 1e-6;
}
m_d[0] = dRecip (AROW(i)[i]);
}
swapProblem (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.
# ifdef DEBUG_LCP
checkFactorization (m_A,m_L,m_d,m_nC,m_C,m_nskip);
# endif
}
void dLCP::transfer_i_from_C_to_N (int i, void *tmpbuf)
{
{
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) {
dLDLTRemove (m_A,C,m_L,m_d,m_n,nC,j,m_nskip,tmpbuf);
int k;
if (last_idx == -1) {
for (k=j+1 ; k<nC; ++k) {
if (C[k]==nC-1) {
break;
}
}
dIASSERT (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;
}
}
dIASSERT (j < nC);
swapProblem (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
}
# ifdef DEBUG_LCP
checkFactorization (m_A,m_L,m_d,m_nC,m_C,m_nskip);
# endif
}
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++;
# ifdef DEBUG_LCP
checkFactorization (A,L,d,nC,C,nskip);
if (i < (n-1)) checkPermutations (i+1,n,nC,nN,p,C);
# endif
}
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;
}
dIASSERT (k < nC);
break;
}
dIASSERT (j < nC);
swapProblem (A,x,b,w,lo,hi,p,state,findex,n,i,nC-1,nskip,1);
nC--;
nN++;
# ifdef DEBUG_LCP
checkFactorization (A,L,d,nC,C,nskip);
# endif
}
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");
*/
}
